From e6dd562873969dc732ffaf1376c72481ca5c681f Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Tue, 6 Jun 2023 08:28:39 +0200 Subject: [PATCH 01/59] Sync humble with newest changes (#123) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Introduce canopen system interface. * Enable easy testing temporarily. * Print config paths on init. * Add device manager and executor. * Start device manager in system interface. * Add nmt and rpdo callbacks. * Remove pedantic cmake flags. * Add internal caching structures for canopen nodes. * Expose necessary stuff from proxy driver. * Apply suggestions from code review * Fix dependencies for canopen_ros2_control. * Move device manager instantation into on_config. * Introduce canopen system interface. * Enable easy testing temporarily. * Print config paths on init. * Add device manager and executor. * Start device manager in system interface. * Add nmt and rpdo callbacks. * Remove pedantic cmake flags. * Add internal caching structures for canopen nodes. * Expose necessary stuff from proxy driver. * Apply suggestions from code review * Add missig dependencies and execute tests only when testing. * Remove unneccesary deps. * Rename "device manager" to "device container" and disable test because it is now working in the current setup. * Update .gitignore * Remove some unecessary changes. * Fix merging issues. * Update visibility-control macros. * Disable test because they can not be eaisly tested. * Update canopen_core/CMakeLists.txt * Solve Boot Error (#49) * Update CI for new version branches * Simplify configuration folder and use existing .eds, .dcf file. Improve test launch file. Update runtime deps. * Add template for canopen proxy controller. * Add dummy services, rt publishers and subscribers to proxy controller. * Expose controller plugin. Start canopen proxy controller instance in example. * Add publishing of rpdo and nmt state. * Add service one shot mechanisms. * Apply suggestions from code review * Add tests to canopen_tests * Reorganise test launch system * update package.xml * further changes * Remove bus.yml * Adapt canopen_system.launch.py for 2 nodes * Update documentation in the readme * Add documentation about testing ros2_control generic interface. * Disable dynamic loading for containers (#50) * disable loader service * Remove artifacts * Publish joint state instead of velocity topics (#47) * disable loader service * add custom target/command and install to macro * publish jointstate * correct variable name squiggle * Minor changes to driver and slave * Update lely core library * Add sensor_msgs to dependencies * Remove artifacts * Remove some artifacts * Add configuration parameter passthrough (#52) * Apply suggestions from code review Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> * Correct macro names. * Add service qos specific profile. * Introduce tests. Adapt proxy controller for easier testing. * Update README.md * make documentation on test with ros2_control more detailed Make the test documentation a small example with explanations of the functionality. * Add in code documentation for canopen_core (#53) * Document device container node * Document lely_master_bridge * Document Lifecycle Device Container * Document Lifecycle Device Manager * Document LifecyleMasterNode * Document Master Node * Fix error * Document lifecycle base driver * Document lely bridge * Document canopen_proxy_driver * Document canopen_402_driver * Restructure documentation (#37) * Document device container node * Document lely_master_bridge * Document Lifecycle Device Container * Document Lifecycle Device Manager * Document LifecyleMasterNode * Document Master Node * Fix error * Document lifecycle base driver * Document lely bridge * Document canopen_proxy_driver * Document canopen_402_driver * Update sphinx documentation * Add profiled position to mock slave (#58) * Implement review requests regarding tests. * Add core node interfaces * Add errors * Add node base classes * Remove device and do some renaming * Add tests to canopen core * Update CmakeFile of canopen core * Add canopen_master_driver package and contents * Make changes to canopen_base_driver for new structure * Change canopen_base_driver for templating problems * Add canopen_proxy_driver with new framework * canopen_402_driver adaption to new framework * Update header guards * Add device container and general changes to make things work. * Add function to device container * Integration with ros2_control * Add type accessor functions to device_container * add node interface accessor and lifecycle information to drivers * Add master dcfs and remove from gitignore * Add 402 driver functions for ros2_control * Add CanopenDriverInterface Documentation * Feature parity for lifecycle nodes * Fix canopen_master_driver for explicit instantiation * Fix canopen_master_driver tests * Fix tests canopen_core * Fix tests base driver * Try running source install/setup.bash * Fix integration tests * Integrate lifecycle manager * Fix get speed and get position * Fix node_canopen_402_drivers add_to_master and handlers * Streamline logging * Remove canopen_lifecycle.launch.py as it i no longer needed. * Add lifecycle manager to device_container library * Undo formatting in ros2_control * fix ci * Fix 402 issues from testing * undo renaming can_interface_name -> can_interface * Publish to joint_states, not joint_state (#63) Co-authored-by: G.A. vd. Hoorn Co-authored-by: Christoph Hellmann Santos * Add unit tests for canopen_core (#64) * Testing changes to canopen_core * Testing changes to canopen_base_driver and canopen_402_driver * Add canopen_core tests (90% coverage) * Fix DriverException error in canopen_402_driver * Catch errors in nmt and rpdo listeners * Fix naming issues * Fix deactivate transition * Fix unclean shutdown * Rename canopen_mock_slave package to canopen_fake_slaves (#66) * Testing changes to canopen_core * Testing changes to canopen_base_driver and canopen_402_driver * Add canopen_core tests (90% coverage) * Fix DriverException error in canopen_402_driver * Catch errors in nmt and rpdo listeners * Fix naming issues * Fix deactivate transition * Fix unclean shutdown * Rename canopen_mock_slave to canopen_fake_slaves * Build flage CANOPEN_ENABLED for disabling tests on CI. * Update deployment * Documentation for streamlined design (#67) * Add canopen_core tests (90% coverage) * Restructure and add plantuml * Changes to quickstart/configuration * Revert "Add canopen_core tests (90% coverage)" as it is not needed. This reverts commit 771c498347f190777fb28edfd5044b96cbfd7bf0. * Create custom driver documentation * Remove breathe api reference and use doxygen * Update interface and naming information for drivers * Update test documentation * install plantuml * Update README.md * Add bare-bone 402 profile system interface. * Add position and speed getter. * State and command interfaces. * Update dependencies. * To protected members for easier inheritance policy. * Fix public fcn visibility. * Adapt 402 hardware interface to device container getter. * Prepare read/write/ * Extend 402 functions via public methods - same as callback based ones. * Expose 402 main functionalities to ros2_control system interface. * Add vel and pos interfaves. * Update proxy canopen system. * Add basic read and write. Divide targets into position, velocity, effort interfaces. * Duplicate some code for configure, init, write phase from proxy driver. * Set target based on condition. * Handle init, recover, halt. Switch modes. * Fix feedback for services for proxy driver and controlller. * Prepare cia 402 device controller. * Add base function ret values first. * State and command interfaces. * Add services for one shot interfaces in cia402 profile. * Better handling of base class on_methods. * Update runtime deps. * Fix joint states scaling. * Add virtual can example for cia 402. * Fix internal launch test. * Fix proxy test. * intra_process_comms * Doxygen documentation for canopen_core (#78) * canopen_core in code documentation * Some more documentation * intra_process_comms * Doxygen documentation for canopen_core (#78) * canopen_core in code documentation * Some more documentation * Remove scalers * Clean cia402 fake shutdown (#72) * adapt fake cia402 slave * Handle demand set master failure (#70) * adapt fake cia402 slave * Add retries for demand_set_master in case of failure * Scaling factors for position and velocity (#74) * Introduce scaling factors * Remove false license statements (#76) * Remove false license statements * Disable device container tests (#77) * Add formatters as used in ros2_control framework. * Changes to format and checkers * Substitute @BUS_CONFIG_PATH@ in bus configuration file * Use @BUS_CONFIG_PATH@ variable in bus configuration files * Precommit changes (#79) * Precommit changes * Update to clang-format-14 * Don't treat options section as another device * Use options section in test bus config files * Remove references to sympy.true (#84) Co-authored-by: James Ward * add short documentation Signed-off-by: Christoph Hellmann Santos * Add dcf_path to bus.ymls Signed-off-by: Christoph Hellmann Santos * Don't treat options as driver Signed-off-by: Christoph Hellmann Santos * Format updates Signed-off-by: Christoph Hellmann Santos * Better organize dependencies (#88) * Add EMCY callback to base driver (#91) * Add post build testing in readme (#87) Signed-off-by: Christoph Hellmann Santos * Simplify 402 driver (#89) * Split motor.hpp and motor.cpp into different files Signed-off-by: Christoph Hellmann Santos * Fix missing symbol error Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos * Add Interpolated Position Mode (linear only, no PT or PVT) (#90) * Add Interpolated Position Mode (linear only, no PT or PVT) Signed-off-by: Christoph Hellmann Santos * add interpolated position mode to system interface Signed-off-by: Christoph Hellmann Santos * Add interpolated position mode to controllers. Signed-off-by: Christoph Hellmann Santos * Add to interpolated position mode to documentation Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos * Fix typo in README (#92) * Correct repo link (#94) * Implemented thread-safe queue for rpdo and emcy listener (#97) * Boost lock free queue implemetation * include boost libraries in CMakelists * Testing rpdo/tpdo ping pond * pre-commit changes * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking * Fixed typo * pre-commit update * FIxed: properly export Boost libraries * Update code documentation * proper vel and pos scaling from device * Include rpdo/tpdo test in launch_test. (#98) * Implement rpdo/tpdo test * Removed unnecessary files * Fix stack smashing (#103) Signed-off-by: Christoph Hellmann Santos * Motor Profile Updates (#101) * Extend and fix info statement. * Fix service handler overwriting. * Consider enum 3 as profiled velocity. Remove some code duplication by reusing private setters in service cbs. Create setter for interpolated position mode. * Fix cyclic position mode. * Simplify write method cases defined by mode of op. * Add driver dictionaries (#110) * Get slave eds and bin in node_canopen_driver Signed-off-by: Christoph Hellmann Santos * Add dictionary to base driver Signed-off-by: Christoph Hellmann Santos * Enable dictionary in proxy drivers Signed-off-by: Christoph Hellmann Santos * Add a few test objects Signed-off-by: Christoph Hellmann Santos * Add pdo checks Signed-off-by: Christoph Hellmann Santos * Adjust 402 driver Signed-off-by: Christoph Hellmann Santos * Fix tests Signed-off-by: Christoph Hellmann Santos * rename to get_xx_queue Signed-off-by: Christoph Hellmann Santos * Add typed sdo operations Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos * Remove type indication from msg and srv interfaces (#112) * Get slave eds and bin in node_canopen_driver Signed-off-by: Christoph Hellmann Santos * Add dictionary to base driver Signed-off-by: Christoph Hellmann Santos * Enable dictionary in proxy drivers Signed-off-by: Christoph Hellmann Santos * Add a few test objects Signed-off-by: Christoph Hellmann Santos * Add pdo checks Signed-off-by: Christoph Hellmann Santos * Adjust 402 driver Signed-off-by: Christoph Hellmann Santos * Fix tests Signed-off-by: Christoph Hellmann Santos * rename to get_xx_queue Signed-off-by: Christoph Hellmann Santos * Add typed sdo operations Signed-off-by: Christoph Hellmann Santos * Remove object datatype where possible Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos * Reduce processor load (#111) * Get slave eds and bin in node_canopen_driver Signed-off-by: Christoph Hellmann Santos * Add dictionary to base driver Signed-off-by: Christoph Hellmann Santos * Enable dictionary in proxy drivers Signed-off-by: Christoph Hellmann Santos * Add a few test objects Signed-off-by: Christoph Hellmann Santos * Add pdo checks Signed-off-by: Christoph Hellmann Santos * Adjust 402 driver Signed-off-by: Christoph Hellmann Santos * Fix tests Signed-off-by: Christoph Hellmann Santos * rename to get_xx_queue Signed-off-by: Christoph Hellmann Santos * Add typed sdo operations Signed-off-by: Christoph Hellmann Santos * Remove object datatype where possible Signed-off-by: Christoph Hellmann Santos * Add plain operation mode setting + switchingstate Signed-off-by: Christoph Hellmann Santos * Add robot system interface Signed-off-by: Christoph Hellmann Santos * Add robot system controller Signed-off-by: Christoph Hellmann Santos * Add robot_system_tests Signed-off-by: Christoph Hellmann Santos * Add a bit of documentation Signed-off-by: Christoph Hellmann Santos * Add in code documentation Signed-off-by: Christoph Hellmann Santos * Fix bug Signed-off-by: Christoph Hellmann Santos * Add examples section Signed-off-by: Christoph Hellmann Santos * Fix set_target for interpolated mode * Switch to rclcpp::sleep_for Signed-off-by: Christoph Hellmann Santos * Fix initialization for state and command interface variables * Add remade robot system interfce Signed-off-by: Christoph Hellmann Santos * Add copyright info Signed-off-by: Christoph Hellmann Santos * Fix missing return statement Signed-off-by: Christoph Hellmann Santos * processing behavior improvement Signed-off-by: Christoph Hellmann Santos * Minor changes to make things work Signed-off-by: Christoph Hellmann Santos * Add poll_timer_callback Signed-off-by: Christoph Hellmann Santos * Fix format Signed-off-by: Christoph Hellmann Santos * Add polling mode variable for config. Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Vishnuprasad Prachandabhanu * Robot system interface (#113) * Get slave eds and bin in node_canopen_driver Signed-off-by: Christoph Hellmann Santos * Add dictionary to base driver Signed-off-by: Christoph Hellmann Santos * Enable dictionary in proxy drivers Signed-off-by: Christoph Hellmann Santos * Add a few test objects Signed-off-by: Christoph Hellmann Santos * Add pdo checks Signed-off-by: Christoph Hellmann Santos * Adjust 402 driver Signed-off-by: Christoph Hellmann Santos * Fix tests Signed-off-by: Christoph Hellmann Santos * rename to get_xx_queue Signed-off-by: Christoph Hellmann Santos * Add typed sdo operations Signed-off-by: Christoph Hellmann Santos * Remove object datatype where possible Signed-off-by: Christoph Hellmann Santos * Add plain operation mode setting + switchingstate Signed-off-by: Christoph Hellmann Santos * Add robot system interface Signed-off-by: Christoph Hellmann Santos * Add robot system controller Signed-off-by: Christoph Hellmann Santos * Add robot_system_tests Signed-off-by: Christoph Hellmann Santos * Add a bit of documentation Signed-off-by: Christoph Hellmann Santos * Add in code documentation Signed-off-by: Christoph Hellmann Santos * Fix bug Signed-off-by: Christoph Hellmann Santos * Add examples section Signed-off-by: Christoph Hellmann Santos * Fix set_target for interpolated mode * Switch to rclcpp::sleep_for Signed-off-by: Christoph Hellmann Santos * Fix initialization for state and command interface variables * Add remade robot system interfce Signed-off-by: Christoph Hellmann Santos * Add copyright info Signed-off-by: Christoph Hellmann Santos * Fix missing return statement Signed-off-by: Christoph Hellmann Santos * processing behavior improvement Signed-off-by: Christoph Hellmann Santos * Minor changes to make things work Signed-off-by: Christoph Hellmann Santos * Add poll_timer_callback Signed-off-by: Christoph Hellmann Santos * Fix format Signed-off-by: Christoph Hellmann Santos * Add polling mode variable for config. Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Vishnuprasad Prachandabhanu * Enable simplified bus.yml format (#115) * Get slave eds and bin in node_canopen_driver Signed-off-by: Christoph Hellmann Santos * Add dictionary to base driver Signed-off-by: Christoph Hellmann Santos * Enable dictionary in proxy drivers Signed-off-by: Christoph Hellmann Santos * Add a few test objects Signed-off-by: Christoph Hellmann Santos * Add pdo checks Signed-off-by: Christoph Hellmann Santos * Adjust 402 driver Signed-off-by: Christoph Hellmann Santos * Fix tests Signed-off-by: Christoph Hellmann Santos * rename to get_xx_queue Signed-off-by: Christoph Hellmann Santos * Add typed sdo operations Signed-off-by: Christoph Hellmann Santos * Remove object datatype where possible Signed-off-by: Christoph Hellmann Santos * Add plain operation mode setting + switchingstate Signed-off-by: Christoph Hellmann Santos * Add robot system interface Signed-off-by: Christoph Hellmann Santos * Add robot system controller Signed-off-by: Christoph Hellmann Santos * Add robot_system_tests Signed-off-by: Christoph Hellmann Santos * Add a bit of documentation Signed-off-by: Christoph Hellmann Santos * Add in code documentation Signed-off-by: Christoph Hellmann Santos * Fix bug Signed-off-by: Christoph Hellmann Santos * Add examples section Signed-off-by: Christoph Hellmann Santos * Fix set_target for interpolated mode * Switch to rclcpp::sleep_for Signed-off-by: Christoph Hellmann Santos * Fix initialization for state and command interface variables * Add remade robot system interfce Signed-off-by: Christoph Hellmann Santos * Add copyright info Signed-off-by: Christoph Hellmann Santos * Fix missing return statement Signed-off-by: Christoph Hellmann Santos * processing behavior improvement Signed-off-by: Christoph Hellmann Santos * Minor changes to make things work Signed-off-by: Christoph Hellmann Santos * Add poll_timer_callback Signed-off-by: Christoph Hellmann Santos * Fix format Signed-off-by: Christoph Hellmann Santos * Add polling mode variable for config. Signed-off-by: Christoph Hellmann Santos * Add cogen Signed-off-by: Christoph Hellmann Santos * Add example usage for cogen Signed-off-by: Christoph Hellmann Santos * Remove explicit path Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Vishnuprasad Prachandabhanu * add dedicated documentation for humble, rolling and iron Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Lovro Co-authored-by: Denis Štogl Co-authored-by: Denis Štogl Co-authored-by: Dr.-Ing. Denis Štogl Co-authored-by: G.A. vd. Hoorn Co-authored-by: Błażej Sowa Co-authored-by: James Ward Co-authored-by: James Ward Co-authored-by: Chris Schindlbeck Co-authored-by: Vishnuprasad Prachandabhanu <32260301+ipa-vsp@users.noreply.github.com> Co-authored-by: Vishnuprasad Prachandabhanu --- .clang-format | 17 + .github/ISSUE_TEMPLATE/bug_report.md | 6 +- .github/ISSUE_TEMPLATE/question.md | 6 +- .github/workflows/ci-format.yml | 23 + .github/workflows/ci-ros-lint.yml.backup | 39 + .github/workflows/galactic.yml | 33 - .github/workflows/humble.yml | 8 +- .github/workflows/humble_documentation.yml | 64 + .github/workflows/{foxy.yml => iron.yml} | 12 +- .github/workflows/iron_documentation.yml | 64 + .github/workflows/rolling.yml | 7 +- ...entation.yml => rolling_documentation.yml} | 25 +- .gitignore | 1 - .gitlab-ci.yml | 2 +- .pre-commit-config.yaml | 144 + Dockerfile | 2 +- README.md | 66 +- canopen/doxygen/Doxyfile | 4 +- canopen/sphinx/Makefile | 2 - canopen/sphinx/_static/css/custom.css | 2 +- canopen/sphinx/alpha-test-description.rst | 49 - canopen/sphinx/conf.py | 69 +- .../sphinx/developers-guide/architecture.rst | 176 + .../design-objectives.rst | 6 +- .../new-device-manager.rst | 1 - .../sphinx/developers-guide/new-driver.rst | 322 ++ .../sphinx/developers-guide/new-master.rst | 2 + .../{ => developers-guide}/overview.rst | 17 +- canopen/sphinx/device-manager.rst | 44 - canopen/sphinx/driver-implementation.rst | 4 - .../hardware-tests/trinamic-pandrive.rst | 30 - canopen/sphinx/index.rst | 57 +- canopen/sphinx/integration.rst | 2 - canopen/sphinx/new-driver.rst | 45 - canopen/sphinx/new-master.rst | 12 - .../configuration.rst} | 35 +- canopen/sphinx/quickstart/examples.rst | 27 + .../sphinx/{ => quickstart}/installation.rst | 2 +- .../operation.rst} | 0 canopen/sphinx/sil-tests.rst | 10 - .../software-tests/concurrency-test.rst | 14 - canopen/sphinx/software-tests/nmt-test.rst | 14 - canopen/sphinx/software-tests/pdo-test.rst | 14 - .../software-tests/proxy-driver-test.rst | 23 + .../ros2_control_system-test.rst | 49 + canopen/sphinx/software-tests/sdo-test.rst | 14 - canopen/sphinx/system-interface.rst | 11 - .../sphinx/templates/alpha-test-template.rst | 28 - canopen/sphinx/tested-hardware.rst | 7 - canopen/sphinx/testing-and-benchmarking.rst | 2 - .../cia402-driver.rst} | 47 +- .../configuration}/configuration.rst | 23 +- canopen/sphinx/{ => user-guide}/master.rst | 12 +- .../operation/managed-service-interface.rst | 52 + .../sphinx/user-guide/operation/operation.rst | 32 + .../operation/ros2-control-interface.rst | 77 + .../operation/service-interface.rst | 54 + .../proxy-driver.rst} | 21 +- canopen_402_driver/CMakeLists.txt | 166 +- .../include/canopen_402_driver/base.hpp | 115 +- .../canopen_402_driver/canopen_402_driver.hpp | 218 -- .../canopen_402_driver/cia402_driver.hpp | 87 + .../include/canopen_402_driver/command.hpp | 66 + .../default_homing_mode.hpp | 43 + .../canopen_402_driver/homing_mode.hpp | 27 + .../lifecycle_canopen_402_driver.hpp | 277 -- .../lifecycle_cia402_driver.hpp | 82 + .../canopen_402_driver/mc_device_driver.hpp | 311 -- .../include/canopen_402_driver/mode.hpp | 31 + .../mode_forward_helper.hpp | 41 + .../canopen_402_driver/mode_target_helper.hpp | 74 + .../include/canopen_402_driver/motor.hpp | 675 +--- .../node_canopen_402_driver.hpp | 332 ++ .../node_canopen_402_driver_impl.hpp | 605 +++ .../profiled_position_mode.hpp | 79 + .../include/canopen_402_driver/state.hpp | 58 + .../canopen_402_driver/visibility_control.h | 48 +- .../canopen_402_driver/word_accessor.hpp | 37 + canopen_402_driver/package.xml | 10 +- canopen_402_driver/src/canopen_402_driver.cpp | 203 - canopen_402_driver/src/cia402_driver.cpp | 14 + canopen_402_driver/src/command.cpp | 102 + .../src/default_homing_mode.cpp | 106 + .../src/lifecycle_canopen_402_driver.cpp | 264 -- .../src/lifecycle_cia402_driver.cpp | 15 + canopen_402_driver/src/motor.cpp | 949 ++--- .../node_canopen_402_driver.cpp | 8 + canopen_402_driver/src/state.cpp | 87 + canopen_402_driver/test/CMakeLists.txt | 9 + canopen_402_driver/test/master.dcf | 694 ++++ .../test/test_driver_component.cpp | 44 + canopen_base_driver/CMakeLists.txt | 148 +- .../canopen_base_driver/base_driver.hpp | 47 + .../canopen_base_driver.hpp | 120 - .../canopen_base_driver/lely_bridge.hpp | 234 -- .../lely_driver_bridge.hpp | 849 +++++ .../lifecycle_base_driver.hpp | 48 + .../lifecycle_canopen_base_driver.hpp | 151 - .../node_canopen_base_driver.hpp | 118 + .../node_canopen_base_driver_impl.hpp | 317 ++ .../canopen_base_driver/visibility_control.h | 48 +- canopen_base_driver/package.xml | 6 +- canopen_base_driver/src/base_driver.cpp | 27 + .../src/canopen_base_driver.cpp | 66 - canopen_base_driver/src/lely_bridge.cpp | 244 -- .../src/lely_driver_bridge.cpp | 399 ++ .../src/lifecycle_base_driver.cpp | 28 + .../src/lifecycle_canopen_base_driver.cpp | 125 - .../node_canopen_base_driver.cpp | 9 + canopen_base_driver/test/CMakeLists.txt | 28 + canopen_base_driver/test/master.dcf | 694 ++++ .../test/test_base_driver_component.cpp | 44 + .../test_node_canopen_base_driver_ros.cpp | 75 + canopen_core/CMakeLists.txt | 239 +- canopen_core/ConfigExtras.cmake | 1 + .../canopen_core/configuration_manager.hpp | 139 +- canopen_core/include/canopen_core/device.hpp | 367 -- .../include/canopen_core/device_container.hpp | 347 ++ .../canopen_core/device_container_error.hpp | 30 + .../canopen_core/device_container_node.hpp | 144 - .../include/canopen_core/driver_error.hpp | 29 + .../include/canopen_core/driver_node.hpp | 314 ++ .../include/canopen_core/exchange.hpp | 139 +- .../canopen_core/lely_master_bridge.hpp | 73 - .../lifecycle_device_container_node.hpp | 162 - .../lifecycle_device_manager_node.hpp | 222 -- .../canopen_core/lifecycle_manager.hpp | 289 ++ .../canopen_core/lifecycle_master_node.hpp | 125 - .../include/canopen_core/master_error.hpp | 27 + .../include/canopen_core/master_node.hpp | 348 +- .../node_interfaces/node_canopen_driver.hpp | 370 ++ .../node_canopen_driver_interface.hpp | 111 + .../node_interfaces/node_canopen_master.hpp | 351 ++ .../node_canopen_master_interface.hpp | 86 + .../include/canopen_core/visibility_control.h | 50 +- canopen_core/launch/canopen.launch.py | 72 +- .../launch/canopen_lifecycle.launch.py | 83 - canopen_core/package.xml | 8 +- canopen_core/readme.md | 12 +- canopen_core/scripts/setup_vcan.sh | 2 +- canopen_core/src/configuration_manager.cpp | 53 +- canopen_core/src/device_container.cpp | 469 +++ canopen_core/src/device_container_error.cpp | 12 + canopen_core/src/device_container_node.cpp | 312 +- canopen_core/src/driver_error.cpp | 11 + canopen_core/src/driver_node.cpp | 104 + canopen_core/src/lely_master_bridge.cpp | 226 -- .../src/lifecycle_device_container_node.cpp | 348 -- .../src/lifecycle_device_manager_node.cpp | 314 -- canopen_core/src/lifecycle_manager.cpp | 304 ++ canopen_core/src/lifecycle_master_node.cpp | 181 - canopen_core/src/master_error.cpp | 12 + canopen_core/src/master_node.cpp | 168 +- .../node_interfaces/node_canopen_driver.cpp | 91 + .../node_interfaces/node_canopen_master.cpp | 4 + canopen_core/test/CMakeLists.txt | 132 + .../test/bus_configs/bad_driver_duplicate.yml | 14 + .../test/bus_configs/bad_driver_no_driver.yml | 6 + .../test/bus_configs/bad_driver_no_id.yml | 6 + .../bus_configs/bad_driver_no_package.yml | 6 + .../test/bus_configs/bad_master_no_driver.yml | 4 + .../test/bus_configs/bad_master_no_id.yml | 4 + .../bus_configs/bad_master_no_package.yml | 4 + .../test/bus_configs/bad_no_master.yml | 14 + canopen_core/test/bus_configs/good_driver.yml | 7 + canopen_core/test/bus_configs/good_master.yml | 5 + .../good_master_and_two_driver.yml | 19 + canopen_core/test/master.dcf | 694 ++++ canopen_core/test/simple.eds | 270 ++ canopen_core/test/test_canopen_driver.cpp | 91 + canopen_core/test/test_canopen_master.cpp | 83 + canopen_core/test/test_device_container.cpp | 848 +++++ canopen_core/test/test_errors.cpp | 27 + .../test/test_lifecycle_canopen_driver.cpp | 124 + .../test/test_lifecycle_canopen_master.cpp | 116 + canopen_core/test/test_lifecycle_manager.cpp | 400 ++ .../test/test_node_canopen_driver.cpp | 298 ++ .../test/test_node_canopen_master.cpp | 445 +++ .../CMakeLists.txt | 36 +- .../LICENSE | 0 canopen_fake_slaves/Readme.md | 7 + .../config/cia402_slave.eds | 1 - .../config/simple_slave.eds | 2 +- .../canopen_fake_slaves/base_slave.hpp | 87 + .../canopen_fake_slaves/basic_slave.hpp | 115 + .../canopen_fake_slaves/cia402_slave.hpp | 655 ++++ .../canopen_fake_slaves/motion_generator.hpp | 109 + .../launch/basic_slave.launch.py | 95 + .../launch/cia402_slave.launch.py | 95 + .../package.xml | 5 +- canopen_fake_slaves/src/basic_slave.cpp | 14 + canopen_fake_slaves/src/cia402_slave.cpp | 13 + canopen_fake_slaves/src/motion_generator.cpp | 216 ++ canopen_interfaces/msg/COData.msg | 2 - canopen_interfaces/srv/COHeartbeatID.srv | 2 +- canopen_interfaces/srv/CONode.srv | 2 +- canopen_interfaces/srv/CORead.srv | 4 +- canopen_interfaces/srv/COReadID.srv | 11 +- canopen_interfaces/srv/COTargetDouble.srv | 2 +- canopen_interfaces/srv/COWrite.srv | 4 +- canopen_interfaces/srv/COWriteID.srv | 13 +- canopen_master_driver/CMakeLists.txt | 153 + canopen_master_driver/LICENSE | 202 + .../lely_master_bridge.hpp | 154 + .../lifecycle_master_driver.hpp | 46 + .../canopen_master_driver/master_driver.hpp | 41 + .../node_canopen_basic_master.hpp | 69 + .../node_canopen_basic_master_impl.hpp | 122 + .../visibility_control.h | 35 + canopen_master_driver/package.xml | 25 + .../src/lely_master_bridge.cpp | 135 + .../src/lifecycle_master_driver.cpp | 17 + canopen_master_driver/src/master_driver.cpp | 18 + .../node_canopen_basic_master.cpp | 8 + canopen_master_driver/test/CMakeLists.txt | 46 + canopen_master_driver/test/master.dcf | 694 ++++ .../test/test_master_driver_component.cpp | 44 + .../test/test_node_canopen_basic_master.cpp | 52 + .../test_node_canopen_basic_master_ros.cpp | 76 + .../include/canopen_mock_slave/base_slave.hpp | 73 - .../canopen_mock_slave/basic_slave.hpp | 78 - .../canopen_mock_slave/cia402_slave.hpp | 564 --- .../launch/basic_slave.launch.py | 94 - .../launch/cia402_slave.launch.py | 94 - canopen_mock_slave/src/basic_slave.cpp | 14 - canopen_mock_slave/src/cia402_slave.cpp | 14 - canopen_mock_slave/src/slave.cpp | 179 - canopen_proxy_driver/CMakeLists.txt | 131 +- .../config/concurrency_test/master.dcf | 1446 +++++++ .../config/nmt_test/master.dcf | 506 +++ .../config/pdo_test/master.dcf | 506 +++ .../config/sdo_test/master.dcf | 506 +++ .../canopen_proxy_driver.hpp | 116 - .../lifecycle_canopen_proxy_driver.hpp | 117 - .../lifecycle_proxy_driver.hpp | 73 + .../node_canopen_proxy_driver.hpp | 67 + .../node_canopen_proxy_driver_impl.hpp | 306 ++ .../canopen_proxy_driver/proxy_driver.hpp | 72 + .../canopen_proxy_driver/visibility_control.h | 48 +- canopen_proxy_driver/package.xml | 7 +- canopen_proxy_driver/readme.md | 6 + .../src/canopen_proxy_driver.cpp | 154 - .../src/lifecycle_canopen_proxy_driver.cpp | 277 -- .../src/lifecycle_proxy_driver.cpp | 29 + .../node_canopen_proxy_driver.cpp | 8 + canopen_proxy_driver/src/proxy_driver.cpp | 27 + canopen_proxy_driver/test/CMakeLists.txt | 28 + canopen_proxy_driver/test/master.dcf | 694 ++++ .../test/test_driver_component.cpp | 44 + .../test/test_node_interface.cpp | 74 + canopen_ros2_control/CMakeLists.txt | 87 + canopen_ros2_control/canopen_ros2_control.xml | 23 + .../config/cia402_ros2_control.yaml | 42 + canopen_ros2_control/config/ros2_control.yaml | 13 + .../canopen_ros2_control/canopen_system.hpp | 208 + .../canopen_ros2_control/cia402_data.hpp | 209 + .../canopen_ros2_control/cia402_system.hpp | 132 + .../include/canopen_ros2_control/helpers.hpp | 62 + .../canopen_ros2_control/robot_system.hpp | 191 + .../canopen_ros2_control/visibility_control.h | 49 + .../launch/canopen_system.launch.py | 277 ++ .../launch/cia402_system.launch.py | 274 ++ canopen_ros2_control/package.xml | 41 + canopen_ros2_control/src/canopen_system.cpp | 290 ++ canopen_ros2_control/src/cia402_system.cpp | 391 ++ canopen_ros2_control/src/robot_system.cpp | 338 ++ .../test/test_canopen_system.cpp | 67 + .../urdf/canopen_system.ros2_control.xacro | 30 + .../urdf/canopen_system.urdf.xacro | 50 + .../urdf/cia402_system.ros2_control.xacro | 30 + .../urdf/cia402_system.urdf.xacro | 50 + canopen_ros2_controllers/CMakeLists.txt | 101 + .../canopen_ros2_controllers.xml | 20 + .../canopen_proxy_controller.hpp | 132 + .../cia402_device_controller.hpp | 133 + .../cia402_robot_controller.hpp | 153 + .../cia402_robot_controller.yaml | 14 + .../visibility_control.h | 49 + canopen_ros2_controllers/package.xml | 33 + .../src/canopen_proxy_controller.cpp | 355 ++ .../src/cia402_device_controller.cpp | 165 + .../src/cia402_robot_controller.cpp | 237 ++ .../test/test_canopen_proxy_controller.cpp | 173 + .../test/test_canopen_proxy_controller.hpp | 276 ++ .../test_load_canopen_proxy_controller.cpp | 41 + canopen_tests/CMakeLists.txt | 30 +- canopen_tests/README.md | 4 + canopen_tests/config/cia402/bus.yml | 38 +- canopen_tests/config/cia402/cia402_slave.eds | 1 - canopen_tests/config/cia402_lifecycle/bus.yml | 18 +- .../config/cia402_lifecycle/cia402_slave.eds | 1 - canopen_tests/config/robot_control/bus.yml | 95 + .../config/robot_control/cia402_slave.eds | 3368 +++++++++++++++++ .../config/robot_control/prbt_0_1.dcf | 2052 ++++++++++ .../robot_control/ros2_controllers.yaml | 54 + canopen_tests/config/simple/bus.yml | 15 +- canopen_tests/config/simple/simple.eds | 16 +- canopen_tests/config/simple_lifecycle/bus.yml | 19 + .../config/simple_lifecycle/simple.eds | 270 ++ .../launch/cia402_lifecycle_setup.launch.py | 28 +- canopen_tests/launch/cia402_setup.launch.py | 28 +- .../launch/proxy_lifecycle_setup.launch.py | 46 +- canopen_tests/launch/proxy_setup.launch.py | 75 + .../launch/robot_control_setup.launch.py | 111 + canopen_tests/launch/view_urdf.launch.py | 54 + .../launch_tests/test_proxy_driver.py | 40 +- .../test_proxy_lifecycle_driver.py | 119 + canopen_tests/package.xml | 10 +- canopen_tests/rviz/robot_controller.rviz | 203 + .../robot_controller.macro.xacro | 71 + .../robot_controller.ros2_control.xacro | 29 + .../robot_controller.urdf.xacro | 23 + canopen_utils/canopen_utils/cyclic_tester.py | 8 +- .../canopen_utils/simple_rpdo_tpdo_tester.py | 64 + canopen_utils/canopen_utils/test_node.py | 50 +- canopen_utils/no_tests/_test_copyright.py | 4 +- canopen_utils/no_tests/_test_flake8.py | 4 +- canopen_utils/no_tests/_test_pep257.py | 4 +- canopen_utils/setup.py | 26 +- lely_core_libraries/CMakeLists.txt | 17 +- .../cmake/lely_core_libraries-extras.cmake | 53 +- lely_core_libraries/cogen/__init__.py | 0 lely_core_libraries/cogen/cogen.py | 61 + lely_core_libraries/setup.cfg | 3 + 324 files changed, 34422 insertions(+), 9204 deletions(-) create mode 100644 .clang-format create mode 100644 .github/workflows/ci-format.yml create mode 100644 .github/workflows/ci-ros-lint.yml.backup delete mode 100644 .github/workflows/galactic.yml create mode 100644 .github/workflows/humble_documentation.yml rename .github/workflows/{foxy.yml => iron.yml} (84%) create mode 100644 .github/workflows/iron_documentation.yml rename .github/workflows/{documentation.yml => rolling_documentation.yml} (76%) create mode 100644 .pre-commit-config.yaml delete mode 100644 canopen/sphinx/alpha-test-description.rst create mode 100644 canopen/sphinx/developers-guide/architecture.rst rename canopen/sphinx/{ => developers-guide}/design-objectives.rst (94%) rename canopen/sphinx/{ => developers-guide}/new-device-manager.rst (98%) create mode 100644 canopen/sphinx/developers-guide/new-driver.rst create mode 100644 canopen/sphinx/developers-guide/new-master.rst rename canopen/sphinx/{ => developers-guide}/overview.rst (89%) delete mode 100644 canopen/sphinx/device-manager.rst delete mode 100644 canopen/sphinx/driver-implementation.rst delete mode 100644 canopen/sphinx/hardware-tests/trinamic-pandrive.rst delete mode 100644 canopen/sphinx/integration.rst delete mode 100644 canopen/sphinx/new-driver.rst delete mode 100644 canopen/sphinx/new-master.rst rename canopen/sphinx/{configuration-package.rst => quickstart/configuration.rst} (93%) create mode 100644 canopen/sphinx/quickstart/examples.rst rename canopen/sphinx/{ => quickstart}/installation.rst (77%) rename canopen/sphinx/{running-configuration-package.rst => quickstart/operation.rst} (100%) delete mode 100644 canopen/sphinx/sil-tests.rst delete mode 100644 canopen/sphinx/software-tests/concurrency-test.rst delete mode 100644 canopen/sphinx/software-tests/nmt-test.rst delete mode 100644 canopen/sphinx/software-tests/pdo-test.rst create mode 100644 canopen/sphinx/software-tests/proxy-driver-test.rst create mode 100644 canopen/sphinx/software-tests/ros2_control_system-test.rst delete mode 100644 canopen/sphinx/software-tests/sdo-test.rst delete mode 100644 canopen/sphinx/system-interface.rst delete mode 100644 canopen/sphinx/templates/alpha-test-template.rst delete mode 100644 canopen/sphinx/tested-hardware.rst delete mode 100644 canopen/sphinx/testing-and-benchmarking.rst rename canopen/sphinx/{motion-controller.rst => user-guide/cia402-driver.rst} (62%) rename canopen/sphinx/{ => user-guide/configuration}/configuration.rst (92%) rename canopen/sphinx/{ => user-guide}/master.rst (85%) create mode 100644 canopen/sphinx/user-guide/operation/managed-service-interface.rst create mode 100644 canopen/sphinx/user-guide/operation/operation.rst create mode 100644 canopen/sphinx/user-guide/operation/ros2-control-interface.rst create mode 100644 canopen/sphinx/user-guide/operation/service-interface.rst rename canopen/sphinx/{proxy-device.rst => user-guide/proxy-driver.rst} (80%) delete mode 100644 canopen_402_driver/include/canopen_402_driver/canopen_402_driver.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/command.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/homing_mode.hpp delete mode 100644 canopen_402_driver/include/canopen_402_driver/lifecycle_canopen_402_driver.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp delete mode 100644 canopen_402_driver/include/canopen_402_driver/mc_device_driver.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/mode.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/state.hpp create mode 100644 canopen_402_driver/include/canopen_402_driver/word_accessor.hpp delete mode 100644 canopen_402_driver/src/canopen_402_driver.cpp create mode 100644 canopen_402_driver/src/cia402_driver.cpp create mode 100644 canopen_402_driver/src/command.cpp create mode 100644 canopen_402_driver/src/default_homing_mode.cpp delete mode 100644 canopen_402_driver/src/lifecycle_canopen_402_driver.cpp create mode 100644 canopen_402_driver/src/lifecycle_cia402_driver.cpp create mode 100644 canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp create mode 100644 canopen_402_driver/src/state.cpp create mode 100644 canopen_402_driver/test/CMakeLists.txt create mode 100644 canopen_402_driver/test/master.dcf create mode 100644 canopen_402_driver/test/test_driver_component.cpp create mode 100644 canopen_base_driver/include/canopen_base_driver/base_driver.hpp delete mode 100644 canopen_base_driver/include/canopen_base_driver/canopen_base_driver.hpp delete mode 100644 canopen_base_driver/include/canopen_base_driver/lely_bridge.hpp create mode 100644 canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp create mode 100644 canopen_base_driver/include/canopen_base_driver/lifecycle_base_driver.hpp delete mode 100644 canopen_base_driver/include/canopen_base_driver/lifecycle_canopen_base_driver.hpp create mode 100644 canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp create mode 100644 canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp create mode 100644 canopen_base_driver/src/base_driver.cpp delete mode 100644 canopen_base_driver/src/canopen_base_driver.cpp delete mode 100644 canopen_base_driver/src/lely_bridge.cpp create mode 100644 canopen_base_driver/src/lely_driver_bridge.cpp create mode 100644 canopen_base_driver/src/lifecycle_base_driver.cpp delete mode 100644 canopen_base_driver/src/lifecycle_canopen_base_driver.cpp create mode 100644 canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp create mode 100644 canopen_base_driver/test/CMakeLists.txt create mode 100644 canopen_base_driver/test/master.dcf create mode 100644 canopen_base_driver/test/test_base_driver_component.cpp create mode 100644 canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp create mode 100644 canopen_core/ConfigExtras.cmake delete mode 100644 canopen_core/include/canopen_core/device.hpp create mode 100644 canopen_core/include/canopen_core/device_container.hpp create mode 100644 canopen_core/include/canopen_core/device_container_error.hpp delete mode 100644 canopen_core/include/canopen_core/device_container_node.hpp create mode 100644 canopen_core/include/canopen_core/driver_error.hpp create mode 100644 canopen_core/include/canopen_core/driver_node.hpp delete mode 100644 canopen_core/include/canopen_core/lely_master_bridge.hpp delete mode 100644 canopen_core/include/canopen_core/lifecycle_device_container_node.hpp delete mode 100644 canopen_core/include/canopen_core/lifecycle_device_manager_node.hpp create mode 100644 canopen_core/include/canopen_core/lifecycle_manager.hpp delete mode 100644 canopen_core/include/canopen_core/lifecycle_master_node.hpp create mode 100644 canopen_core/include/canopen_core/master_error.hpp create mode 100644 canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp create mode 100644 canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp create mode 100644 canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp create mode 100644 canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp delete mode 100644 canopen_core/launch/canopen_lifecycle.launch.py create mode 100644 canopen_core/src/device_container.cpp create mode 100644 canopen_core/src/device_container_error.cpp create mode 100644 canopen_core/src/driver_error.cpp create mode 100644 canopen_core/src/driver_node.cpp delete mode 100644 canopen_core/src/lely_master_bridge.cpp delete mode 100644 canopen_core/src/lifecycle_device_container_node.cpp delete mode 100644 canopen_core/src/lifecycle_device_manager_node.cpp create mode 100644 canopen_core/src/lifecycle_manager.cpp delete mode 100644 canopen_core/src/lifecycle_master_node.cpp create mode 100644 canopen_core/src/master_error.cpp create mode 100644 canopen_core/src/node_interfaces/node_canopen_driver.cpp create mode 100644 canopen_core/src/node_interfaces/node_canopen_master.cpp create mode 100644 canopen_core/test/CMakeLists.txt create mode 100644 canopen_core/test/bus_configs/bad_driver_duplicate.yml create mode 100644 canopen_core/test/bus_configs/bad_driver_no_driver.yml create mode 100644 canopen_core/test/bus_configs/bad_driver_no_id.yml create mode 100644 canopen_core/test/bus_configs/bad_driver_no_package.yml create mode 100644 canopen_core/test/bus_configs/bad_master_no_driver.yml create mode 100644 canopen_core/test/bus_configs/bad_master_no_id.yml create mode 100644 canopen_core/test/bus_configs/bad_master_no_package.yml create mode 100644 canopen_core/test/bus_configs/bad_no_master.yml create mode 100644 canopen_core/test/bus_configs/good_driver.yml create mode 100644 canopen_core/test/bus_configs/good_master.yml create mode 100644 canopen_core/test/bus_configs/good_master_and_two_driver.yml create mode 100644 canopen_core/test/master.dcf create mode 100644 canopen_core/test/simple.eds create mode 100644 canopen_core/test/test_canopen_driver.cpp create mode 100644 canopen_core/test/test_canopen_master.cpp create mode 100644 canopen_core/test/test_device_container.cpp create mode 100644 canopen_core/test/test_errors.cpp create mode 100644 canopen_core/test/test_lifecycle_canopen_driver.cpp create mode 100644 canopen_core/test/test_lifecycle_canopen_master.cpp create mode 100644 canopen_core/test/test_lifecycle_manager.cpp create mode 100644 canopen_core/test/test_node_canopen_driver.cpp create mode 100644 canopen_core/test/test_node_canopen_master.cpp rename {canopen_mock_slave => canopen_fake_slaves}/CMakeLists.txt (86%) rename {canopen_mock_slave => canopen_fake_slaves}/LICENSE (100%) create mode 100644 canopen_fake_slaves/Readme.md rename {canopen_mock_slave => canopen_fake_slaves}/config/cia402_slave.eds (93%) rename {canopen_mock_slave => canopen_fake_slaves}/config/simple_slave.eds (99%) create mode 100644 canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp create mode 100644 canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp create mode 100644 canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp create mode 100644 canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp create mode 100644 canopen_fake_slaves/launch/basic_slave.launch.py create mode 100644 canopen_fake_slaves/launch/cia402_slave.launch.py rename {canopen_mock_slave => canopen_fake_slaves}/package.xml (91%) create mode 100644 canopen_fake_slaves/src/basic_slave.cpp create mode 100644 canopen_fake_slaves/src/cia402_slave.cpp create mode 100644 canopen_fake_slaves/src/motion_generator.cpp create mode 100644 canopen_master_driver/CMakeLists.txt create mode 100644 canopen_master_driver/LICENSE create mode 100644 canopen_master_driver/include/canopen_master_driver/lely_master_bridge.hpp create mode 100644 canopen_master_driver/include/canopen_master_driver/lifecycle_master_driver.hpp create mode 100644 canopen_master_driver/include/canopen_master_driver/master_driver.hpp create mode 100644 canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp create mode 100644 canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp create mode 100644 canopen_master_driver/include/canopen_master_driver/visibility_control.h create mode 100644 canopen_master_driver/package.xml create mode 100644 canopen_master_driver/src/lely_master_bridge.cpp create mode 100644 canopen_master_driver/src/lifecycle_master_driver.cpp create mode 100644 canopen_master_driver/src/master_driver.cpp create mode 100644 canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp create mode 100644 canopen_master_driver/test/CMakeLists.txt create mode 100644 canopen_master_driver/test/master.dcf create mode 100644 canopen_master_driver/test/test_master_driver_component.cpp create mode 100644 canopen_master_driver/test/test_node_canopen_basic_master.cpp create mode 100644 canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp delete mode 100644 canopen_mock_slave/include/canopen_mock_slave/base_slave.hpp delete mode 100644 canopen_mock_slave/include/canopen_mock_slave/basic_slave.hpp delete mode 100644 canopen_mock_slave/include/canopen_mock_slave/cia402_slave.hpp delete mode 100644 canopen_mock_slave/launch/basic_slave.launch.py delete mode 100644 canopen_mock_slave/launch/cia402_slave.launch.py delete mode 100644 canopen_mock_slave/src/basic_slave.cpp delete mode 100644 canopen_mock_slave/src/cia402_slave.cpp delete mode 100644 canopen_mock_slave/src/slave.cpp create mode 100644 canopen_proxy_driver/config/concurrency_test/master.dcf create mode 100644 canopen_proxy_driver/config/nmt_test/master.dcf create mode 100644 canopen_proxy_driver/config/pdo_test/master.dcf create mode 100644 canopen_proxy_driver/config/sdo_test/master.dcf delete mode 100644 canopen_proxy_driver/include/canopen_proxy_driver/canopen_proxy_driver.hpp delete mode 100644 canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_canopen_proxy_driver.hpp create mode 100644 canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_proxy_driver.hpp create mode 100644 canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp create mode 100644 canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp create mode 100644 canopen_proxy_driver/include/canopen_proxy_driver/proxy_driver.hpp create mode 100644 canopen_proxy_driver/readme.md delete mode 100644 canopen_proxy_driver/src/canopen_proxy_driver.cpp delete mode 100644 canopen_proxy_driver/src/lifecycle_canopen_proxy_driver.cpp create mode 100644 canopen_proxy_driver/src/lifecycle_proxy_driver.cpp create mode 100644 canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp create mode 100644 canopen_proxy_driver/src/proxy_driver.cpp create mode 100644 canopen_proxy_driver/test/CMakeLists.txt create mode 100644 canopen_proxy_driver/test/master.dcf create mode 100644 canopen_proxy_driver/test/test_driver_component.cpp create mode 100644 canopen_proxy_driver/test/test_node_interface.cpp create mode 100644 canopen_ros2_control/CMakeLists.txt create mode 100644 canopen_ros2_control/canopen_ros2_control.xml create mode 100644 canopen_ros2_control/config/cia402_ros2_control.yaml create mode 100644 canopen_ros2_control/config/ros2_control.yaml create mode 100644 canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp create mode 100644 canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp create mode 100644 canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp create mode 100644 canopen_ros2_control/include/canopen_ros2_control/helpers.hpp create mode 100644 canopen_ros2_control/include/canopen_ros2_control/robot_system.hpp create mode 100644 canopen_ros2_control/include/canopen_ros2_control/visibility_control.h create mode 100644 canopen_ros2_control/launch/canopen_system.launch.py create mode 100644 canopen_ros2_control/launch/cia402_system.launch.py create mode 100644 canopen_ros2_control/package.xml create mode 100644 canopen_ros2_control/src/canopen_system.cpp create mode 100644 canopen_ros2_control/src/cia402_system.cpp create mode 100644 canopen_ros2_control/src/robot_system.cpp create mode 100644 canopen_ros2_control/test/test_canopen_system.cpp create mode 100644 canopen_ros2_control/urdf/canopen_system.ros2_control.xacro create mode 100644 canopen_ros2_control/urdf/canopen_system.urdf.xacro create mode 100644 canopen_ros2_control/urdf/cia402_system.ros2_control.xacro create mode 100644 canopen_ros2_control/urdf/cia402_system.urdf.xacro create mode 100644 canopen_ros2_controllers/CMakeLists.txt create mode 100644 canopen_ros2_controllers/canopen_ros2_controllers.xml create mode 100644 canopen_ros2_controllers/include/canopen_ros2_controllers/canopen_proxy_controller.hpp create mode 100644 canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_device_controller.hpp create mode 100644 canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp create mode 100644 canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml create mode 100644 canopen_ros2_controllers/include/canopen_ros2_controllers/visibility_control.h create mode 100644 canopen_ros2_controllers/package.xml create mode 100644 canopen_ros2_controllers/src/canopen_proxy_controller.cpp create mode 100644 canopen_ros2_controllers/src/cia402_device_controller.cpp create mode 100644 canopen_ros2_controllers/src/cia402_robot_controller.cpp create mode 100644 canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp create mode 100644 canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp create mode 100644 canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp create mode 100644 canopen_tests/README.md create mode 100644 canopen_tests/config/robot_control/bus.yml create mode 100644 canopen_tests/config/robot_control/cia402_slave.eds create mode 100644 canopen_tests/config/robot_control/prbt_0_1.dcf create mode 100644 canopen_tests/config/robot_control/ros2_controllers.yaml create mode 100644 canopen_tests/config/simple_lifecycle/bus.yml create mode 100644 canopen_tests/config/simple_lifecycle/simple.eds create mode 100644 canopen_tests/launch/proxy_setup.launch.py create mode 100644 canopen_tests/launch/robot_control_setup.launch.py create mode 100644 canopen_tests/launch/view_urdf.launch.py create mode 100644 canopen_tests/launch_tests/test_proxy_lifecycle_driver.py create mode 100644 canopen_tests/rviz/robot_controller.rviz create mode 100644 canopen_tests/urdf/robot_controller/robot_controller.macro.xacro create mode 100644 canopen_tests/urdf/robot_controller/robot_controller.ros2_control.xacro create mode 100644 canopen_tests/urdf/robot_controller/robot_controller.urdf.xacro create mode 100644 canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py create mode 100644 lely_core_libraries/cogen/__init__.py create mode 100644 lely_core_libraries/cogen/cogen.py create mode 100644 lely_core_libraries/setup.cfg diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..4f6e5c85 --- /dev/null +++ b/.clang-format @@ -0,0 +1,17 @@ +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BreakBeforeBraces: Allman +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +IndentWidth: 2 +TabWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: true +IncludeBlocks: Preserve +... diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 25e7b034..c8878e2b 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -19,9 +19,9 @@ A clear and concise description of what you expected to happen. **Logs** **Setup:** - - Device: - - OS: - - ROS-Distro: + - Device: + - OS: + - ROS-Distro: - Branch/Commit: **Additional context** diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md index 0b1a24ac..56979035 100644 --- a/.github/ISSUE_TEMPLATE/question.md +++ b/.github/ISSUE_TEMPLATE/question.md @@ -14,9 +14,9 @@ A clear and concise description of what the bug is. Add build or execution log for context. **Setup:** - - Device: - - OS: - - ROS-Distro: + - Device: + - OS: + - ROS-Distro: - Branch/Commit: **Additional context** diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml new file mode 100644 index 00000000..36ea2778 --- /dev/null +++ b/.github/workflows/ci-format.yml @@ -0,0 +1,23 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: + workflow_dispatch: + pull_request: + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v2 + with: + python-version: 3.10.6 + - name: Install system hooks + run: sudo apt update && sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v2.0.3 + with: + extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml.backup b/.github/workflows/ci-ros-lint.yml.backup new file mode 100644 index 00000000..807f890e --- /dev/null +++ b/.github/workflows/ci-ros-lint.yml.backup @@ -0,0 +1,39 @@ +name: ROS Lint +on: + pull_request: + +jobs: + ament_lint: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + matrix: + linter: [cppcheck, copyright, lint_cmake] + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: ${{ matrix.linter }} + package-name: + $PKG_NAME$ + + ament_lint_100: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + matrix: + linter: [cpplint] + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: cpplint + arguments: "--linelength=100 --filter=-whitespace/newline" + package-name: + ros2_canopen diff --git a/.github/workflows/galactic.yml b/.github/workflows/galactic.yml deleted file mode 100644 index b1a3d673..00000000 --- a/.github/workflows/galactic.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: galactic - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - workflow_dispatch: - -jobs: - industrial_ci: - name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [galactic] - ROS_REPO: [testing] - env: - CCACHE_DIR: "${{ github.workspace }}/.ccache" - steps: - - uses: actions/checkout@v2 - - uses: actions/cache@v2 - with: - path: ${{ env.CCACHE_DIR }} - key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} - restore-keys: | - ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- - - uses: 'ros-industrial/industrial_ci@master' - env: - BEFORE_INSTALL_TARGET_DEPENDENCIES: 'sudo apt-get update' - ROS_DISTRO: ${{ matrix.ROS_DISTRO }} - ROS_REPO: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index b6319c3b..db46e326 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -1,11 +1,11 @@ name: humble -on: +on: push: - branches: [ master ] + branches: [ humble ] pull_request: - branches: [ master ] - workflow_dispatch: + branches: [ humble ] + workflow_dispatch: jobs: industrial_ci: diff --git a/.github/workflows/humble_documentation.yml b/.github/workflows/humble_documentation.yml new file mode 100644 index 00000000..fb04666b --- /dev/null +++ b/.github/workflows/humble_documentation.yml @@ -0,0 +1,64 @@ +name: HUMBLE Documentation + +# Controls when the workflow will run +on: + # Triggers the workflow on push or pull request events but only for the master branch + push: + branches: [ humble ] + + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + # This workflow contains a single job called "build" + build_documentation: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + + - name: Install dependencies + run: | + sudo apt-get update -qq + sudo apt-get install -y -qq doxygen graphviz plantuml + pip install sphinx-rtd-theme + pip install sphinxcontrib-plantuml + pip install sphinx-mdinclude + pip install breathe + pip install exhale + + - name: Build doxygen + run: | + cd ./canopen/doxygen/ + doxygen + cd ../.. + + - name: Build documentation + run: | + cd ./canopen/sphinx/ + make html + cd ../.. + + - name: Create commit + run: | + git clone https://github.com/ros-industrial/ros2_canopen.git --branch gh-pages --single-branch gh-pages + mkdir -p gh-pages/manual/humble/ + mkdir -p gh-pages/api/humble/ + cp -r ./canopen/sphinx/_build/html/* gh-pages/manual/humble/ + cp -r ./canopen/doxygen/_build/html/* gh-pages/api/humble/ + cd gh-pages + git config --local user.email "action@github.com" + git config --local user.name "GitHub Action" + git add . + git commit -m "Update documentation" -a || true + + - name: Push changes + uses: ad-m/github-push-action@master + with: + branch: gh-pages + directory: gh-pages + github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/foxy.yml b/.github/workflows/iron.yml similarity index 84% rename from .github/workflows/foxy.yml rename to .github/workflows/iron.yml index 7c625b79..5b99389a 100644 --- a/.github/workflows/foxy.yml +++ b/.github/workflows/iron.yml @@ -1,11 +1,11 @@ -name: foxy +name: iron -on: +on: push: - branches: [ master ] + branches: [ iron ] pull_request: - branches: [ master ] - workflow_dispatch: + branches: [ iron ] + workflow_dispatch: jobs: industrial_ci: @@ -14,7 +14,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [foxy] + ROS_DISTRO: [iron] ROS_REPO: [testing] env: CCACHE_DIR: "${{ github.workspace }}/.ccache" diff --git a/.github/workflows/iron_documentation.yml b/.github/workflows/iron_documentation.yml new file mode 100644 index 00000000..a8bcef2b --- /dev/null +++ b/.github/workflows/iron_documentation.yml @@ -0,0 +1,64 @@ +name: IRON Documentation + +# Controls when the workflow will run +on: + # Triggers the workflow on push or pull request events but only for the master branch + push: + branches: [ iron ] + + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + # This workflow contains a single job called "build" + build_documentation: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + + - name: Install dependencies + run: | + sudo apt-get update -qq + sudo apt-get install -y -qq doxygen graphviz plantuml + pip install sphinx-rtd-theme + pip install sphinxcontrib-plantuml + pip install sphinx-mdinclude + pip install breathe + pip install exhale + + - name: Build doxygen + run: | + cd ./canopen/doxygen/ + doxygen + cd ../.. + + - name: Build documentation + run: | + cd ./canopen/sphinx/ + make html + cd ../.. + + - name: Create commit + run: | + git clone https://github.com/ros-industrial/ros2_canopen.git --branch gh-pages --single-branch gh-pages + mkdir -p gh-pages/manual/iron/ + mkdir -p gh-pages/api/iron/ + cp -r ./canopen/sphinx/_build/html/* gh-pages/manual/iron/ + cp -r ./canopen/doxygen/_build/html/* gh-pages/api/iron/ + cd gh-pages + git config --local user.email "action@github.com" + git config --local user.name "GitHub Action" + git add . + git commit -m "Update documentation" -a || true + + - name: Push changes + uses: ad-m/github-push-action@master + with: + branch: gh-pages + directory: gh-pages + github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 58a7171d..62b5a37b 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -1,11 +1,11 @@ name: rolling -on: +on: push: - branches: [ master ] + branches: [ master ] pull_request: branches: [ master ] - workflow_dispatch: + workflow_dispatch: jobs: industrial_ci: @@ -29,5 +29,6 @@ jobs: - uses: 'ros-industrial/industrial_ci@master' env: BEFORE_INSTALL_TARGET_DEPENDENCIES: 'sudo apt-get update' + AFTER_BUILD_TARGET_WORKSPACE: 'set +u && COLCON_TRACE=0 AMENT_TRACE_SETUP_FILES=0 source /root/target_ws/install/setup.bash && set -u' ROS_DISTRO: ${{ matrix.ROS_DISTRO }} ROS_REPO: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/documentation.yml b/.github/workflows/rolling_documentation.yml similarity index 76% rename from .github/workflows/documentation.yml rename to .github/workflows/rolling_documentation.yml index 10e44def..70428392 100644 --- a/.github/workflows/documentation.yml +++ b/.github/workflows/rolling_documentation.yml @@ -1,4 +1,4 @@ -name: Documentation +name: ROLLING Documentation # Controls when the workflow will run on: @@ -20,36 +20,45 @@ jobs: steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - uses: actions/checkout@v3 - + - name: Install dependencies run: | sudo apt-get update -qq - sudo apt-get install -y -qq doxygen graphviz + sudo apt-get install -y -qq doxygen graphviz plantuml pip install sphinx-rtd-theme + pip install sphinxcontrib-plantuml pip install sphinx-mdinclude pip install breathe pip install exhale - + + - name: Build doxygen + run: | + cd ./canopen/doxygen/ + doxygen + cd ../.. + - name: Build documentation run: | cd ./canopen/sphinx/ make html cd ../.. - + - name: Create commit run: | git clone https://github.com/ros-industrial/ros2_canopen.git --branch gh-pages --single-branch gh-pages - cp -r ./canopen/sphinx/_build/html/* gh-pages/ + mkdir -p gh-pages/manual/rolling/ + mkdir -p gh-pages/api/rolling/ + cp -r ./canopen/sphinx/_build/html/* gh-pages/manual/rolling/ + cp -r ./canopen/doxygen/_build/html/* gh-pages/api/rolling/ cd gh-pages git config --local user.email "action@github.com" git config --local user.name "GitHub Action" git add . git commit -m "Update documentation" -a || true - + - name: Push changes uses: ad-m/github-push-action@master with: branch: gh-pages directory: gh-pages github_token: ${{ secrets.GITHUB_TOKEN }} - diff --git a/.gitignore b/.gitignore index 6c9c03f0..ee765265 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,3 @@ **/__pycache__/ **/_build **/api -**/master.dcf \ No newline at end of file diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index aadabf21..be5b496b 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -35,7 +35,7 @@ sphinx: paths: - canopen/sphinx/_build tags: - - asprunner + - asprunner .build: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..40651881 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,144 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.3.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + exclude: canopen_core/test/bus_configs/bad_driver_duplicate.yml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + - id: fix-byte-order-marker + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v2.38.2 + hooks: + - id: pyupgrade + args: [--py36-plus] + + - repo: https://github.com/psf/black + rev: 22.8.0 + hooks: + - id: black + args: ["--line-length=99"] + + # PEP 257 + # - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 + # rev: v0.3.3 + # hooks: + # - id: pep257 + # args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + + # - repo: https://github.com/pycqa/flake8 + # rev: 5.0.4 + # hooks: + # - id: flake8 + # args: ["--ignore=E501"] + + # CPP hooks + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-14 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] + # The same options as in ament_cppcheck are used, but its not working... + #- repo: https://github.com/pocc/pre-commit-hooks + #rev: v1.1.1 + #hooks: + #- id: cppcheck + #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] + + # - repo: local + # hooks: + # - id: ament_cppcheck + # name: ament_cppcheck + # description: Static code analysis of C/C++ files. + # stages: [commit] + # entry: ament_cppcheck + # language: system + # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + # # Maybe use https://github.com/cpplint/cpplint instead + # - repo: local + # hooks: + # - id: ament_cpplint + # name: ament_cpplint + # description: Static code analysis of C/C++ files. + # stages: [commit] + # entry: ament_cpplint + # language: system + # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + # args: ["--linelength=100", "--filter=-whitespace/newline"] + + # # Cmake hooks + # - repo: local + # hooks: + # - id: ament_lint_cmake + # name: ament_lint_cmake + # description: Check format of CMakeLists.txt files. + # stages: [commit] + # entry: ament_lint_cmake + # language: system + # files: CMakeLists\.txt$ + + # # Copyright + # - repo: local + # hooks: + # - id: ament_copyright + # name: ament_copyright + # description: Check if copyright notice is available in all files. + # stages: [commit] + # entry: ament_copyright + # language: system + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.0.0 + hooks: + - id: doc8 + args: ['--max-line-length=100', '--ignore=D001'] + exclude: CHANGELOG\.rst$ + + - repo: https://github.com/pre-commit/pygrep-hooks + rev: v1.9.0 + hooks: + - id: rst-backticks + exclude: CHANGELOG\.rst$ + - id: rst-directive-colons + - id: rst-inline-touching-normal + + # Spellcheck in comments and docs + # skipping of *.svg files is not working... + - repo: https://github.com/codespell-project/codespell + rev: v2.2.1 + hooks: + - id: codespell + args: ['--write-changes'] + exclude: CHANGELOG\.rst|\.(svg|pyc|drawio|dcf|eds)$ diff --git a/Dockerfile b/Dockerfile index c420e7c5..73b521f8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -18,4 +18,4 @@ RUN . /opt/ros/galactic/setup.sh \ && rosdep init && rosdep update \ && rosdep install --from-paths src --ignore-src -r -y \ && colcon build \ - && . install/setup.sh \ No newline at end of file + && . install/setup.sh diff --git a/README.md b/README.md index 2c39c394..d2c08abf 100644 --- a/README.md +++ b/README.md @@ -1,34 +1,62 @@ # ROS2 CANopen -[![Build Status](https://github.com/ros-industrial/ros2_canopen/workflows/foxy/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) -[![Build Status](https://github.com/ros-industrial/ros2_canopen/workflows/galactic/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) -[![Build Status](https://github.com/ros-industrial/ros2_canopen/workflows/humble/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) [![Build Status](https://github.com/ros-industrial/ros2_canopen/workflows/rolling/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) [![Documentation Status](https://github.com/ros-industrial/ros2_canopen/workflows/Documentation/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) -| Package | License | -|--------------|-----------| -| canopen | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) | -| canopen_core | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) | -| canopen_interfaces | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) | -| canopen_base_driver | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) | -| canopen_proxy_driver | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) | -| canopen_402_driver | [![license - LGPLv3](https://img.shields.io/:license-LGPL%203.0-yellow.svg)](https://opensource.org/licenses/LGPL-3.0) | -| canopen_utils | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) | -| lely_core_libraries | [![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) | - ## Documentation -The documentation is generated using sphinx and doxygen. The current version for master can be found [here](https://ros-industrial.github.io/ros2_canopen/). +The documentation consists of two parts: a manual and an api reference. +The documentation is built for rolling (master), iron and humble and hosted on github pages. +Older ROS 2 releases are EOL and are not supported anymore. + +### Rolling +* Manual: https://ros-industrial.github.io/ros2_canopen/manual/rolling/ +* API reference: https://ros-industrial.github.io/ros2_canopen/api/rolling/ + +### Iron +* Manual: https://ros-industrial.github.io/ros2_canopen/manual/iron/ +* API reference: https://ros-industrial.github.io/ros2_canopen/api/iron/ + +### Humble +* Manual: https://ros-industrial.github.io/ros2_canopen/manual/humble/ +* API reference: https://ros-industrial.github.io/ros2_canopen/api/humble/ ## Status Currently under development. Not for production use. **Available Features:** * Device Manager (using rclcpp::components) -* CANopen Master (Service Interface) +* MasterDriver (Service Interface) * ProxyDriver (Service Interface) -* MotionControllerDriver (Service Interface) +* Cia402Driver (Service Interface) +* Generic ros2_control Interface (implementing `hardware_interface::SystemInterface`) - check https://control.ros.org for more details + + +**Post build testing** +To test stack after it was built from source you should first setup a virtual can network. +```bash +sudo modprobe vcan +sudo ip link add dev vcan0 type vcan +sudo ip link set vcan0 txqueuelen 1000 +sudo ip link set up vcan0 +``` +Then you can run the integration tests contained in canopen_tests package. +```bash +launch_test src/ros2_canopen/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py +launch_test src/ros2_canopen/canopen_tests/launch_tests/test_proxy_driver.py +``` + +## Contributing +This repository uses `pre-commit` for code formatting. +This program has to be setup locally and installed inside the repository. +For this execute in the repository folder following commands: +``` +sudo apt install -y pre-commit +pre-commit install +``` +The checks are automatically executed before each commit. +This helps you to always commit well formatted code. +To run all the checks manually use `pre-commit run -a` command. +For the other options check `pre-commit --help`. -**Features under Development:** -* System Interface (using ros2_control::SystemInterface) +In a case of an "emergency" you can avoid execution of pre-commit hooks by adding `-n` flag to `git commit` command - this is NOT recommended to do if you don't know what are you doing! diff --git a/canopen/doxygen/Doxyfile b/canopen/doxygen/Doxyfile index 6481ba2e..a370ef24 100644 --- a/canopen/doxygen/Doxyfile +++ b/canopen/doxygen/Doxyfile @@ -8,7 +8,7 @@ PROJECT_BRIEF = "C++ ROS CANopen Library" INPUT = ../../canopen_core/include ../../canopen_base_driver/include ../../canopen_402_driver/include ../../canopen_proxy_driver/include RECURSIVE = YES -OUTPUT_DIRECTORY = doc_output +OUTPUT_DIRECTORY = _build EXTRACT_ALL = YES SORT_MEMBER_DOCS = NO @@ -20,4 +20,4 @@ ENABLE_PREPROCESSING = YES DOT_GRAPH_MAX_NODES = 101 -FILE_PATTERNS = *.cpp *.h *.hpp *.md \ No newline at end of file +FILE_PATTERNS = *.cpp *.h *.hpp *.md diff --git a/canopen/sphinx/Makefile b/canopen/sphinx/Makefile index 1341c46f..7c644899 100644 --- a/canopen/sphinx/Makefile +++ b/canopen/sphinx/Makefile @@ -22,5 +22,3 @@ clean: # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). %: Makefile @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) - - diff --git a/canopen/sphinx/_static/css/custom.css b/canopen/sphinx/_static/css/custom.css index 094fe6ef..54d2f2e7 100644 --- a/canopen/sphinx/_static/css/custom.css +++ b/canopen/sphinx/_static/css/custom.css @@ -14,4 +14,4 @@ thead { table { white-space: normal; -} \ No newline at end of file +} diff --git a/canopen/sphinx/alpha-test-description.rst b/canopen/sphinx/alpha-test-description.rst deleted file mode 100644 index 262604c6..00000000 --- a/canopen/sphinx/alpha-test-description.rst +++ /dev/null @@ -1,49 +0,0 @@ -Alpha Tester Guide -================== - -Setup ------ - -Inorder to test the alpha with your CANopen device you need -to execute the following steps. - -1. Gather the EDS files for the CANopen devices that you want to test and have connected to your bus. - -2. Create a configuration package following the guide (:doc:`configuration-package`), an example configuration package can be found here (https://github.com/ipa-cmmmh/trinamic_pd42_can) - -Tests ------ -The following table describes the tests that you could execute with your device -to help us check the functionalities of the package. -Currently, we recommend running the tests as super user. - -.. csv-table:: Tests - :header: "Name", "Description" - :delim: ; - - Launching device manager (no lazy load); Run your launch script, that you created as described in the Setup section. Once the setup is done, check with ros2 node list, that device_container_node, master and all devices you specified in your bus configuration are present. - Initialise devices; For each driver node call the init service. The driver node should now have brought the device into operational state and have executed the standard home method. Homing method needs to be set correctly, potentially set it in bus configuration file via SDO call. - Operational modes; For each driver check that the operation modes of the device can be activated using the operation mode services exposed. Also Check if you can set a target using the target service. Set necessary parameters for movements in bus configuration via SDO. - -Profiled Velocity Mode Test -++++++++++++++++++++++++++++ -.. code_block:: - - $ ros2 service call /trinamic_pd42/init std_srvs/srv/Trigger - $ ros2 service call /trinamic_pd42/velocity_mode std_srvs/srv/Trigger - $ ros2 service call /trinamic_pd42/target canopen_interfaces/srv/COTargetDouble "{target: [speed]}" - -Profiled Position Mode Test -++++++++++++++++++++++++++++ -.. code_block:: - - $ ros2 service call /trinamic_pd42/init std_srvs/srv/Trigger - $ ros2 service call /trinamic_pd42/position_mode std_srvs/srv/Trigger - $ ros2 service call /trinamic_pd42/target canopen_interfaces/srv/COTargetDouble "{target: [position]}" - - -Documentation -------------- - -1. If you have discovered a bug, please report it as an issue on GitHub. -2. You should also document the tests you ran even if everything works. In order to do so, please use the Alpha Test Template provided in canopen/sphinx/templates. Create a copy of the file with a good name in the folder hardware tests, add the information about your tests (you can also add entries to the table) and create a pull request with your additions. \ No newline at end of file diff --git a/canopen/sphinx/conf.py b/canopen/sphinx/conf.py index 86559f04..a0137f98 100644 --- a/canopen/sphinx/conf.py +++ b/canopen/sphinx/conf.py @@ -10,16 +10,16 @@ # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. # - +from pathlib import Path # -- Project information ----------------------------------------------------- -project = 'ros2_canopen' -copyright = '2022, Christoph Hellmann Santos, Harshavardhan Deshpande' -author = 'Christoph Hellmann Santos, Harshavardhan Deshpande' +project = "ros2_canopen" +copyright = "2022, Christoph Hellmann Santos, Harshavardhan Deshpande" +author = "Christoph Hellmann Santos, Harshavardhan Deshpande" # The full version, including alpha/beta/rc tags -release = '0.0.1' +release = "0.0.1" # -- General configuration --------------------------------------------------- @@ -28,47 +28,46 @@ # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx_rtd_theme', - 'sphinx_mdinclude', - 'sphinx.ext.imgmath', - 'sphinx.ext.todo', - 'sphinx.ext.graphviz', - 'breathe', - 'exhale' + "sphinx_rtd_theme", + "sphinx_mdinclude", + "sphinx.ext.imgmath", + "sphinx.ext.todo", + "sphinx.ext.graphviz", + "sphinxcontrib.plantuml", + "breathe", ] -breathe_projects = { "ros2_canopen": "../doxygen/doc_output/xml" } breathe_default_project = "ros2_canopen" -exhale_args = { - # These arguments are required - "containmentFolder": "./api", - "rootFileName": "library_root.rst", - "doxygenStripFromPath": "..", - # Heavily encouraged optional argument (see docs) - "rootFileTitle": "API Reference", - # Suggested optional arguments - "createTreeView": True, - # TIP: if using the sphinx-bootstrap-theme, you need - # "treeViewIsBootstrap": True, - "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = ../../canopen_core/include/ ../../canopen_base_driver/include ../../canopen_402_driver/include ../../canopen_proxy_driver/include" + +def get_package(package: str): + path = Path(__file__).parent.parent.parent.joinpath(f"{package}/include/{package}") + files_gen = path.glob("*.hpp") + files = [] + for file in files_gen: + files.append(file.name) + return (path, files) + + +breathe_projects = { + "ros2_canopen": "../doxygen/_build/xml/", } + # Tell sphinx what the primary language being documented is. -primary_domain = 'cpp' +primary_domain = "cpp" # Tell sphinx what the pygments highlight language should be. -highlight_language = 'cpp' +highlight_language = "cpp" # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # -- Options for HTML output ------------------------------------------------- @@ -76,14 +75,12 @@ # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] html_logo = "images/ros2-canopen-logo.png" -html_css_files = [ - 'css/custom.css' -] -pygments_style = 'sphinx' +html_css_files = ["css/custom.css"] +pygments_style = "sphinx" diff --git a/canopen/sphinx/developers-guide/architecture.rst b/canopen/sphinx/developers-guide/architecture.rst new file mode 100644 index 00000000..bceccd4b --- /dev/null +++ b/canopen/sphinx/developers-guide/architecture.rst @@ -0,0 +1,176 @@ +Architecture +============= + +The architecture of the ROS2 CANopen stack is based on the composition +concept of ROS2. In ROS2 components are dynamically loadable ROS2 nodes +which are loaded by a component manager. + +Device Container +"""""""""""""""""" +The core of the ROS2 CANopen stack is the ros2_canopen::DeviceContainer which implements +the rclcpp::ComponentManager class. The DeviceContainer enables loading drivers for the +CANopen master and the devices on the bus, that have been exported as rclcpp_components. +CANopen Master drivers need to implement the ros2_canopen::CanopenMasterInterface, CANopen +Device drivers need to implement the ros2_canopen::CanopenDriverInterface. + +The difference between the ros2_canopen::DeviceContainer and the rclcpp::ComponentContainer +is that the device container loads only master and driver components specified in the bus +configuration (bus.yml). It does not have services for loading components online. All components +that are connected or will be connected to the CANopen Bus need to be known and specified in +the bus.yml before starting the device container. + +.. uml:: + + rclcpp::ComponentManager <|-- ros2_canopen::DeviceContainer + + class rclcpp::ComponentManager + { + std::weak_ptr executor_ + std::vector get_component_resources(std::string package_name) + std::shared_ptr create_component_factory(ComponentResource resource) + void on_list_nodes(...) + virtual void set_executor(const std::weak_ptr executor) + } + + + class ros2_canopen::DeviceContainer + { + std::shared_ptr can_master_ + std::shared_ptr lifecycle_manager_ + std::map> drivers_ + void on_list_nodes(...) override + virtual void set_executor(const std::weak_ptr executor) override + } + +The device container will first load the master specified in the bus configuration. Then +it will load the drivers specified in the bus configuration. If the master and drivers +specified in the bus configuration are managed nodes it will as well load the ros2_canopen::LifecycleManager. + + +CANopen Master Driver Architecture +""""""""""""""""""""""""""""""""""" + +The architecture for CANopen master drivers looks as depicted in the class diagram. All master drivers +consist of three main classes. + +The first class is the functionality class that contains the drivers functionailities independently of +the ROS2 node type. This class needs to implement the ros2_canopen::node_interfaces::NodeCanopenMasterInterface. +ros2_canopen::node_interfaces::NodeCanopenMaster is an abstract class that provides some useful functionality and +implements the ros2_canopen::node_interfaces::NodeCanopenMasterInterface. Usually, master drivers will inherit from +ros2_canopen::node_interfaces::NodeCanopenMaster. + +The second class is the class that wraps the functionality class in a rclcpp::Node. This class should implement the +ros2_canopen::CanopenMasterInterface. The canopen_core package provides a convenience class ros2_canopen::CanopenMaster +that should be inherited from and implements the ros2_canopen::CanopenMasterInterface. + +The third class is the class that wraps the functionality class in a rclcpp_lifecycle::LifecycleNode. This class should implement the +ros2_canopen::CanopenMasterInterface. The canopen_core package provides a convenience class ros2_canopen::LifecycleCanopenMaster +that should be inherited from and implements the ros2_canopen::CanopenMasterInterface. + +.. uml:: + + + package "canopen_core" { + interface ros2_canopen::CanopenMasterInterface + interface ros2_canopen::node_interfaces::NodeCanopenMasterInterface + abstract ros2_canopen::LifecycleCanopenMaster + abstract ros2_canopen::node_interfaces::NodeCanopenMaster + abstract ros2_canopen::CanopenMaster + + + ros2_canopen::node_interfaces::NodeCanopenMasterInterface - ros2_canopen::CanopenMaster : < has + ros2_canopen::LifecycleCanopenMaster - ros2_canopen::node_interfaces::NodeCanopenMasterInterface : > has + + ros2_canopen::CanopenMasterInterface <|-- ros2_canopen::LifecycleCanopenMaster + ros2_canopen::node_interfaces::NodeCanopenMasterInterface <|-- ros2_canopen::node_interfaces::NodeCanopenMaster + ros2_canopen::CanopenMasterInterface <|-- ros2_canopen::CanopenMaster + + } + + package "canopen_master_driver" { + + class ros2_canopen::LifecycleMasterDriver << (C, blue) Component>> + class ros2_canopen::node_interfaces::NodeCanopenBasicMaster + class ros2_canopen::MasterDriver << (C, blue) Component>> + ros2_canopen::LifecycleMasterDriver - ros2_canopen::node_interfaces::NodeCanopenBasicMaster: > has + ros2_canopen::node_interfaces::NodeCanopenBasicMaster - ros2_canopen::MasterDriver : < has + ros2_canopen::LifecycleCanopenMaster <|-- ros2_canopen::LifecycleMasterDriver + ros2_canopen::node_interfaces::NodeCanopenMaster <|-- ros2_canopen::node_interfaces::NodeCanopenBasicMaster + ros2_canopen::CanopenMaster <|-- ros2_canopen::MasterDriver + } + + +CANopen Device Driver Architecture +""""""""""""""""""""""""""""""""""" + +The architecture for CANopen device drivers looks as depicted in the class diagram. All device drivers +consist of three main classes. + +The first class is the functionality class that contains the drivers functionailities independently of +the ROS2 node type. This class needs to implement the ros2_canopen::node_interfaces::NodeCanopenDriverInterface. +ros2_canopen::node_interfaces::NodeCanopenDriver is an abstract class that provides some useful functionality and +implements the ros2_canopen::node_interfaces::NodeCanopenDriverInterface. If you plan to write a driver from scratch +based on Lely Core library, your functionality class should inherit from ros2_canopen::node_interfaces::NodeCanopenDriver. +If you want to use the existing lely_driver_bridge, your functionality class should inherit from ros2_canopen::NodeCanopenBaseDriver. + +The second class is the class that wraps the functionality class in a rclcpp::Node. This class should implement the +ros2_canopen::CanopenDriverInterface. The canopen_core package provides a convenience class ros2_canopen::CanopenDriver +that should be inherited from and implements the ros2_canopen::CanopenDriverInterface. + +The third class is the class that wraps the functionality class in a rclcpp_lifecycle::LifecycleNode. This class should implement the +ros2_canopen::CanopenDriverInterface. The canopen_core package provides a convenience class ros2_canopen::LifecycleCanopenDriver +that should be inherited from and implements the ros2_canopen::CanopenDriverInterface. + +.. uml:: + + package "canopen_core" { + interface ros2_canopen::CanopenDriverInterface + interface ros2_canopen::node_interfaces::NodeCanopenDriverInterface + abstract ros2_canopen::LifecycleCanopenDriver + abstract ros2_canopen::node_interfaces::NodeCanopenDriver + abstract ros2_canopen::CanopenDriver + + + ros2_canopen::node_interfaces::NodeCanopenDriverInterface - ros2_canopen::CanopenDriver : < has + ros2_canopen::LifecycleCanopenDriver - ros2_canopen::node_interfaces::NodeCanopenDriverInterface : > has + + ros2_canopen::CanopenDriverInterface <|-- ros2_canopen::LifecycleCanopenDriver + ros2_canopen::node_interfaces::NodeCanopenDriverInterface <|-- ros2_canopen::node_interfaces::NodeCanopenDriver + ros2_canopen::CanopenDriverInterface <|-- ros2_canopen::CanopenDriver + + } + + + package "canopen_base_driver" { + + class ros2_canopen::LifecycleBaseDriver << (C, blue) Component>> + class ros2_canopen::node_interfaces::NodeCanopenBaseDriver + class ros2_canopen::BaseDriver << (C, blue) Component>> + ros2_canopen::LifecycleBaseDriver - ros2_canopen::node_interfaces::NodeCanopenBaseDriver: > has + ros2_canopen::node_interfaces::NodeCanopenBaseDriver - ros2_canopen::BaseDriver : < has + ros2_canopen::LifecycleCanopenDriver <|-- ros2_canopen::LifecycleBaseDriver + ros2_canopen::node_interfaces::NodeCanopenDriver <|-- ros2_canopen::node_interfaces::NodeCanopenBaseDriver + ros2_canopen::CanopenDriver <|-- ros2_canopen::BaseDriver + } + + package "canopen_proxy_driver" { + class ros2_canopen::LifecycleProxyDriver << (C, blue) Component>> + class ros2_canopen::node_interfaces::NodeCanopenProxyDriver + class ros2_canopen::ProxyDriver << (C, blue) Component>> + ros2_canopen::LifecycleProxyDriver - ros2_canopen::node_interfaces::NodeCanopenProxyDriver: > has + ros2_canopen::node_interfaces::NodeCanopenProxyDriver - ros2_canopen::ProxyDriver : < has + ros2_canopen::LifecycleCanopenDriver <|-- ros2_canopen::LifecycleProxyDriver + ros2_canopen::node_interfaces::NodeCanopenBaseDriver <|-- ros2_canopen::node_interfaces::NodeCanopenProxyDriver + ros2_canopen::CanopenDriver <|-- ros2_canopen::ProxyDriver + } + + package "canopen_402_driver" { + class ros2_canopen::LifecycleCia402Driver << (C, blue) Component>> + class ros2_canopen::node_interfaces::NodeCanopen402Driver + class ros2_canopen::Cia402Driver << (C, blue) Component>> + ros2_canopen::LifecycleCia402Driver - ros2_canopen::node_interfaces::NodeCanopen402Driver: > has + ros2_canopen::node_interfaces::NodeCanopen402Driver - ros2_canopen::Cia402Driver : < has + ros2_canopen::LifecycleCanopenDriver <|-- ros2_canopen::LifecycleCia402Driver + ros2_canopen::node_interfaces::NodeCanopenProxyDriver <|-- ros2_canopen::node_interfaces::NodeCanopen402Driver + ros2_canopen::CanopenDriver <|-- ros2_canopen::Cia402Driver + } diff --git a/canopen/sphinx/design-objectives.rst b/canopen/sphinx/developers-guide/design-objectives.rst similarity index 94% rename from canopen/sphinx/design-objectives.rst rename to canopen/sphinx/developers-guide/design-objectives.rst index 05cee396..7f0e362c 100644 --- a/canopen/sphinx/design-objectives.rst +++ b/canopen/sphinx/developers-guide/design-objectives.rst @@ -1,4 +1,4 @@ -Development Objectives +Design Objectives ====================== The ros_canopen stack had a number of drawbacks especially when it came @@ -16,8 +16,8 @@ development goals. Objective; Description Understandability and Extendability; One major drawback of ros_canopen was that actually extending it with new drivers required to understand the complex stack with its different layers. Robust Parallel Requests; When multiple nodes are running on the same bus, it should be possible to make requests to the nodes concurrently from ROS2 and have the canopen master handle the sequencing. - Easy Maintenance; Maintenance effort should be reduced as much as possible. Therefore, a clean and clear code structure and documentation is needed and only funcitonalities that are not already available from other high quality open source libraries should be self implmented. + Easy Maintenance; Maintenance effort should be reduced as much as possible. Therefore, a clean and clear code structure and documentation is needed and only funcitonalities that are not already available from other high quality open source libraries should be self implemented. Enable controlling drives via ros2_control; A driver for CIA402 and a ros2_control interface need to be developed. Enable controlling drives via ros2 ervice interface; A driver for CIA402 and a service interface need to be developed. Enable proxy functionalities via ros2 interface; For debugging purposes a proxy driver should be developed, which enables sending and receiving CANopen objects via a ros2 interface. - Good enough documentation; Write documentation for using and understanding the ros2_canopen stack. \ No newline at end of file + Good enough documentation; Write documentation for using and understanding the ros2_canopen stack. diff --git a/canopen/sphinx/new-device-manager.rst b/canopen/sphinx/developers-guide/new-device-manager.rst similarity index 98% rename from canopen/sphinx/new-device-manager.rst rename to canopen/sphinx/developers-guide/new-device-manager.rst index 168dd6bc..2558616b 100644 --- a/canopen/sphinx/new-device-manager.rst +++ b/canopen/sphinx/developers-guide/new-device-manager.rst @@ -1,3 +1,2 @@ Creating your device manager ============================ - diff --git a/canopen/sphinx/developers-guide/new-driver.rst b/canopen/sphinx/developers-guide/new-driver.rst new file mode 100644 index 00000000..9252b4dc --- /dev/null +++ b/canopen/sphinx/developers-guide/new-driver.rst @@ -0,0 +1,322 @@ +Creating a new device driver +============================ + +Creating your own device driver is fairly easy in ros2_canopen. You should do this if you +need to create a driver for a specific device or a specific device profile that is not yet supported. If you create +a driver for a device profile we are happy to integrate the package into this repository - simply create +a PR. + +What you need to do +"""""""""""""""""""" +To create a new driver you need to implement at least two classes. One being the functional class, +that contains your drivers functionalities. The other being a ROS2 wrapper node. Generally we recommend +creating one ROS2 wrapper node and another ROS2 lifecycle wrapper node. + +How you do it +"""""""""""""" +First you need to decide from which extension point you want to start. Usually, this is either the core interface, the base driver +or the proxy driver. Base driver provides you with all necessary callbacks for CANopen functionalities but does +not come with any ROS2 interface. Proxy driver has a simple forwarding ROS2 interface that is useful for any driver. +The core interface comes without any functionality, you need to implement everything on your own. +We recommend creating you driver based on Proxy driver, this will be explained here. + +Create the package +------------------ +Create your new package using the standard ros2 pkg commands. Make sure you add the following dependencies: + +* rclcpp +* rclcpp_components +* rclcpp_lifecycle +* lifecycle_msgs +* canopen_core +* canopen_interfaces +* canopen_base_driver +* canopen_proxy_driver +* lely_core_libraries +* std_msgs +* std_srvs + +Once done add a subfolder ``node_interfaces`` in the ``src/`` and the ``include/[pacakge_name]/`` folders. + + +Create the functionality class +------------------------------ +The functionality class is structured similar to a LifecycleNode. The functionality class +has the following callback functions that are related to lifecycle which you can override: + +* void configure(bool called_from_base) +* void activate(bool called_from_base) +* void deactivate(bool called_from_base) +* void cleanup(bool called_from_base) +* void shutdown(bool called_from_base) + +CANopen functionality +********************* +In addition to the functions there are callbacks for CANopen functionality that you can +override: + +* void on_rpdo(COData data) +* void on_nmt(NmtState nmt_state) + +To interact with the CANopen device you can use the ros2_canopen::LelyDriverBridge object, +that is stored in the functionality class (this->driver_). The ros2_canopen::LelyDriverBridge +provides the following functions you should use in your driver: + +* void nmt_command(NmtState nmt_state) +* void tpdo_transmit(COData data) +* std::futureasync_sdo_write(COData data) +* std::futureasync_sdo_read(COData data) + +.. note:: + + The CANopen related functionality can only be used in the activate function or timers/threads that + were started by the activate function. If you start timers or threads in the activate function, that + use CANopen functionality, these have to be stopped in the deactivate function. + +ROS2 functionality +****************** +ROS2 functionlity is available via the ``node_`` object of the functionality class. This +object has a templated type and can either be a ``rclcpp::Node`` or ``rclcpp_lifecycle::LifecycleNode``. +You can use the standard functions like create_timer, create_publisher etc. + +.. note:: + + Currently it seems, that when you use template functions i.e. ``node_->create_publisher(...)`` you + need to create a template specialisation. + + + +Your class declaration should look like this: + +.. code-block:: C++ + :name: "node_interfaces/node_canopen_xxx_driver.hpp" + + node_interfaces/node_canopen_xxx_driver.hpp: + + #include + + template + class NodeCanopenXXXDriver : public NodeCanopenProxyDriver + { + static_assert( + std::is_base_of::value || + std::is_base_of::value, + "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); + public: + NodeCanopenXXXDriver(NODETYPE *node); + + /*Your overrides etc. go here*/ + }; + +Your member definitions go here: + +.. code-block:: C++ + :name: "node_interfaces/node_canopen_xxx_driver_impl.hpp" + + node_interfaces/node_canopen_xxx_driver.hpp: + + #include node_interfaces/node_canopen_xxx_driver.hpp + + /*Your function definitions go here.*/ + +Your explicit template instantiations go here: + +.. code-block:: C++ + :name: "node_interfaces/node_canopen_xxx_driver.cpp" + + node_interfaces/node_canopen_xxx_driver.cpp: + + #include node_interfaces/node_canopen_xxx_driver.hpp + #include node_interfaces/node_canopen_xxx_driver_impl.hpp + + template class ::node_interfaces::NodeCanopenXXXDriver; + template class ::node_interfaces::NodeCanopenXXXDriver; + + +Create the ROS2 wrapper classes +------------------------------- + +The ROS2 wrapper classes are fairly easy to create once you wrote the functionality +class. The wrappers simply use the functionality class to provide the functionality. +The ROS2 wrapper class should always be derived from ``ros2_canopen::CanopenDriver`` or +``ros2_canopen::LifecycleCanopenDriver`` . + + +The declaration should look like this: + +.. code:: + + lifecycle_xxx_driver.hpp: + + #include "canopen_xxx_driver/node_interfaces/node_canopen_xxx_driver.hpp" + #include "canopen_core/driver_node.hpp" + + /** + * @brief Lifecycle Proxy Driver + * + * A very basic driver without any functionality. + * + */ + class LifecycleXXXDriver : public ros2_canopen::LifecycleCanopenDriver + { + std::shared_ptr> node_canopen_xxx_driver_; + public: + LifecycleXXXDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); + }; + +The definitions should look like this: + +.. code:: + + + #include "canopen_xxx_driver/lifecycle_proxy_driver.hpp" + + using namespace ros2_canopen; + + + LifecycleXXXDriver::LifecycleXXXDriver(rclcpp::NodeOptions node_options) : LifecycleCanopenDriver(node_options) + { + node_canopen_xxx_driver_ = std::make_shared>(this); + node_canopen_proxy_driver_ = std::static_pointer_cast(node_canopen_xxx_driver_); + node_canopen_driver_ = std::static_pointer_cast(node_canopen_xxx_driver_); + } + + #include "rclcpp_components/register_node_macro.hpp" + RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleXXXDriver) + + +Adapt the CMakeLists.txt +************************ +The CMakeLists.txt file should look like this: + +.. code:: CMAKE + + cmake_minimum_required(VERSION 3.8) + project(canopen_xxx_driver) + + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) + endif() + + # find dependencies + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_ros REQUIRED) + find_package(rclcpp REQUIRED) + find_package(rclcpp_lifecycle REQUIRED) + find_package(rclcpp_components REQUIRED) + find_package(canopen_core REQUIRED) + find_package(canopen_interfaces REQUIRED) + find_package(canopen_base_driver REQUIRED) + find_package(canopen_proxy_driver REQUIRED) + find_package(lely_core_libraries REQUIRED) + find_package(std_msgs REQUIRED) + find_package(std_srvs REQUIRED) + + set(dependencies + rclcpp + rclcpp_components + rclcpp_lifecycle + lifecycle_msgs + canopen_core + canopen_interfaces + canopen_base_driver + canopen_proxy_driver + lely_core_libraries + std_msgs + std_srvs + ) + + # Functionality library + add_library(node_canopen_xxx_driver + src/node_interfaces/node_canopen_xxx_driver.cpp + ) + target_compile_features(node_canopen_xxx_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + target_compile_options(node_canopen_xxx_driver PUBLIC -Wl,--no-undefined) + target_include_directories(node_canopen_xxx_driver PUBLIC + $ + $) + + ament_target_dependencies( + node_canopen_xxx_driver + ${dependencies} + ) + + # Lifecycle driver + add_library(lifecycle_xxx_driver + src/lifecycle_xxx_driver.cpp + ) + target_compile_features(lifecycle_xxx_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + target_compile_options(lifecycle_xxx_driver PUBLIC -Wl,--no-undefined) + target_include_directories(lifecycle_xxx_driver PUBLIC + $ + $) + + target_link_libraries(lifecycle_xxx_driver + node_canopen_xxx_driver + ) + ament_target_dependencies( + lifecycle_xxx_driver + ${dependencies} + ) + # Causes the visibility macros to use dllexport rather than dllimport, + # which is appropriate when building the dll but not consuming it. + target_compile_definitions(lifecycle_xxx_driver PRIVATE "CANOPEN_XXX_DRIVER_BUILDING_LIBRARY") + + rclcpp_components_register_nodes(lifecycle_xxx_driver "ros2_canopen::LifecycleXXXDriver") + set(node_plugins "${node_plugins}ros2_canopen::LifecycleXXXDriver;$\n") + + + # Non lifecycle driver + add_library(xxx_driver + src/xxx_driver.cpp + ) + target_compile_features(xxx_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + target_compile_options(xxx_driver PUBLIC -Wl,--no-undefined) + target_include_directories(xxx_driver PUBLIC + $ + $) + target_link_libraries(xxx_driver + node_canopen_xxx_driver + ) + + ament_target_dependencies( + xxx_driver + ${dependencies} + ) + + # Causes the visibility macros to use dllexport rather than dllimport, + # which is appropriate when building the dll but not consuming it. + target_compile_definitions(xxx_driver PRIVATE "CANOPEN_XXX_DRIVER_BUILDING_LIBRARY") + + rclcpp_components_register_nodes(xxx_driver "ros2_canopen::XXXDriver") + set(node_plugins "${node_plugins}ros2_canopen::XXXDriver;$\n") + + install( + DIRECTORY include/ + DESTINATION include + ) + + install( + TARGETS lifecycle_xxx_driver xxx_driver node_canopen_xxx_driver + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + if(BUILD_TESTING) + endif() + + ament_export_include_directories( + include + ) + ament_export_libraries( + lifecycle_xxx_driver + xxx_driver + node_canopen_xxx_driver + ) + ament_export_targets( + export_${PROJECT_NAME} + ) + + ament_package() diff --git a/canopen/sphinx/developers-guide/new-master.rst b/canopen/sphinx/developers-guide/new-master.rst new file mode 100644 index 00000000..639f416d --- /dev/null +++ b/canopen/sphinx/developers-guide/new-master.rst @@ -0,0 +1,2 @@ +Creating a new master +===================== diff --git a/canopen/sphinx/overview.rst b/canopen/sphinx/developers-guide/overview.rst similarity index 89% rename from canopen/sphinx/overview.rst rename to canopen/sphinx/developers-guide/overview.rst index 4f888cb6..92b07fce 100644 --- a/canopen/sphinx/overview.rst +++ b/canopen/sphinx/developers-guide/overview.rst @@ -5,37 +5,36 @@ lelycore canopen library as opposed to the previous ros_canopen stack. In ros2_canopen the bus configuration is simplified through the use of lelycore's configutaration toolchain. -ros2_canopen contains a number of packages that serve different serve different puposes. +ros2_canopen contains a number of packages that serve different serve different purposes. * **canopen**: - + Meta-package for easy installation and contains overall documentation of the ros2_canopen stack. * **lely_core_libraries**: - + A colcon package wrapper for the lelycore canopen library, for convenient installation with rosdep. * **canopen_core**: - + Contains the core structures of the ros2_canopen stack such as the device manager and the master node and the driver node interface. * **canopen_base_driver**: - + This package contains the base implementation of a ROS2 CANopen device driver. It can base used by other drivers for easy extension. * **canopen_proxy_driver**: - - Contains an implmentation of a proxy driver which simply forwards CANopen functionality + + Contains an implementation of a proxy driver which simply forwards CANopen functionality for a specific device via ROS2 services and messages. * **canopen_402_driver**: Contains an implementation of the CIA402 profile for motion controllers and exposes the profiles functionalities via ROS2 services and messages. The implementation is - copied from ros_canopen/canopen_402 and this package is licensed accordingly under + copied from ros_canopen/canopen_402 and this package is licensed accordingly under GNU Lesser General Public License v3.0! - diff --git a/canopen/sphinx/device-manager.rst b/canopen/sphinx/device-manager.rst deleted file mode 100644 index 201edf13..00000000 --- a/canopen/sphinx/device-manager.rst +++ /dev/null @@ -1,44 +0,0 @@ -Device Manager -============== - -The device manager implements ROS2 component manager interface and can therefore be handled similar to a -container node. This enables flexibly loading and unloading the driver nodes necessary for commanding -each device on the bus. The user can decide when the device should be activated. - - -.. figure:: images/device-manager.png - :alt: Device Manager Concept - - device manager concept - -The device manager will uses the bus description file to identify the correct drivers for each devices. -On startup it will load the CANopen master node and pass the generated DCF files to configure the CANopen master -correctly for you bus configuration. Once a CANopen Node comes online (i.e. sends the boot indication) the CANopen master -will configure the node with the parameters and commands specified in the bus configuration for that device. - -The device manager will then go on loading the drivers specified in the bus configuration file if they are not -marked as lazy loaded. If the driver is marked as lazy loaded it is not loaded on startup of the device manager but when the user call the component load service -for that specific driver. - -All loaded nodes are added to the device manager's executor. - -.. figure:: images/device-manager-usage.png - :alt: Device Manager Usage - - device manager usage - -Configuration -------------- -The device manager has the following configuration parameters. - -.. csv-table:: Parameters - :header: "Parameter", "Type", "Description" - - bus_conf, string, (Mandatory) Path to the bus configuration YAML-file - master_dcf, string, (Mandatory) Path to the DCF file to be used by the master node. Usually generated by dcfgen as master.dcf. - master_bin, string, (Optional) Path to the concise DCF (.bin) file to be used to configure the master. Usually generated by dcfgen as master.bin. (default: "") - can_interface, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) - - - - \ No newline at end of file diff --git a/canopen/sphinx/driver-implementation.rst b/canopen/sphinx/driver-implementation.rst deleted file mode 100644 index 263a6203..00000000 --- a/canopen/sphinx/driver-implementation.rst +++ /dev/null @@ -1,4 +0,0 @@ -Driver Implementation -===================== - - diff --git a/canopen/sphinx/hardware-tests/trinamic-pandrive.rst b/canopen/sphinx/hardware-tests/trinamic-pandrive.rst deleted file mode 100644 index 3d5b7c7b..00000000 --- a/canopen/sphinx/hardware-tests/trinamic-pandrive.rst +++ /dev/null @@ -1,30 +0,0 @@ -Trinamic PANdrive PD42-1-1270 -============================= - -Test setup ----------- - - -.. csv-table:: Tests - :header: "Equipment", "Description" - :delim: ; - - Test systen; Lenovo ThinkPad - Operating System; Ubuntu 20.04 on VirtualBox - ROS2 Distro; ROS2 Galactic - CANopen Interface; PeakCAN - CANopen Devices; Single Trinamic PD42 Drive - Stack Version; - - -Tested service calls --------------------- - -.. csv-table:: Tests - :header: "Name", "Status", "Comment" - :delim: ; - - Launching device manager (no lazy load); OK; No problems, everything works fine. - Initialise devices; OK; Trinamic PD42 is initialised and configuration from YAML is correctly loaded - Profiled Velocity ; OK; Works fine. Velocity can be set via target service call. - Profiled Position; OK; Works fine. Position can be set via target service call. \ No newline at end of file diff --git a/canopen/sphinx/index.rst b/canopen/sphinx/index.rst index 0bb2307c..01b2d876 100644 --- a/canopen/sphinx/index.rst +++ b/canopen/sphinx/index.rst @@ -1,47 +1,48 @@ ROS2 CANopen Stack ================== -This is the documentation of the ROS2 CANopen stack. - +This is the documentation of the ROS2 CANopen stack. .. toctree:: :maxdepth: 1 - :caption: Getting started + :caption: Quickstart + :glob: + + quickstart/installation + quickstart/configuration + quickstart/operation + quickstart/examples - overview - installation - .. toctree:: :maxdepth: 1 - :caption: Concepts and Design + :caption: User Guide + :glob: - design-objectives - configuration - device-manager - system-interface - master - proxy-device - motion-controller + user-guide/configuration/configuration + user-guide/operation/operation + user-guide/operation/service-interface + user-guide/operation/managed-service-interface + user-guide/operation/ros2-control-interface + user-guide/master + user-guide/proxy-driver + user-guide/cia402-driver -.. toctree:: - :maxdepth: 1 - :caption: Usage - - configuration-package - running-configuration-package - .. toctree:: :maxdepth: 1 - :caption: Extension + :caption: Developer Guide + :glob: - new-driver - new-master + developers-guide/design-objectives + developers-guide/overview + developers-guide/architecture + developers-guide/new-driver + developers-guide/new-master + API Reference .. toctree:: :maxdepth: 1 - :caption: Tests and Benchmarks + :caption: Software Tests + :glob: - alpha-test-description - sil-tests - tested-hardware \ No newline at end of file + software-tests/** diff --git a/canopen/sphinx/integration.rst b/canopen/sphinx/integration.rst deleted file mode 100644 index 75e265ee..00000000 --- a/canopen/sphinx/integration.rst +++ /dev/null @@ -1,2 +0,0 @@ -Integration -=========== diff --git a/canopen/sphinx/new-driver.rst b/canopen/sphinx/new-driver.rst deleted file mode 100644 index 6afc71da..00000000 --- a/canopen/sphinx/new-driver.rst +++ /dev/null @@ -1,45 +0,0 @@ -Creating a new device driver -============================ - -Creating your own device driver is fairly easy in ros2_canopen. You should do this if you -need to create a driver for a specific device or a specific device profile. If you create -a driver for a device profile we are happy to integrate the package into this repository - simply create -a PR. - -Option 1: From BaseDriver -------------------------------- -Derive your driver from BaseDriver, which has callbacks for rpdo and nmt changes, but no ros services -whatsoever. - -.. doxygenclass:: ros2_canopen::BaseDriver - :project: ros2_canopen - :protected-members: - - -Option 2: From ProxyDriver --------------------------------- -Derive your driver from ProxyDriver, which has services for nmt, sdo and publishers and subscribers for pdo included. - -.. doxygenclass:: ros2_canopen::ProxyDriver - :project: ros2_canopen - :protected-members: - - - -Option 3: From scratch ----------------------- -All ros2_canopen drivers need to implement the DriverInterface. This makes them a ROS2 Node that we are -able to load using plugin lib. - -.. doxygenclass:: ros2_canopen::DriverInterface - :project: ros2_canopen - :members: - - -For implementation of your driver you can use the LelyBridge Class which implements a lely-driver that -is easy to use from a ROS node. Or write a similar Class yourself, for example using lely's LoopDriver. - -.. doxygenclass:: ros2_canopen::LelyBridge - :project: ros2_canopen - :members: - diff --git a/canopen/sphinx/new-master.rst b/canopen/sphinx/new-master.rst deleted file mode 100644 index 58b056e6..00000000 --- a/canopen/sphinx/new-master.rst +++ /dev/null @@ -1,12 +0,0 @@ -Creating a new master -===================== -*This is a planned extension point, the functionality is not yet implemented* - -If you need more services for the master or less or simply want to create your own master -implementation. - -Master implementations need to implement the MasterInterface. - -.. doxygenclass:: ros2_canopen::MasterInterface - :project: ros2_canopen - :members: \ No newline at end of file diff --git a/canopen/sphinx/configuration-package.rst b/canopen/sphinx/quickstart/configuration.rst similarity index 93% rename from canopen/sphinx/configuration-package.rst rename to canopen/sphinx/quickstart/configuration.rst index 9b4ee2a8..84ce6e0f 100644 --- a/canopen/sphinx/configuration-package.rst +++ b/canopen/sphinx/quickstart/configuration.rst @@ -15,7 +15,7 @@ you need to have (usually one per CAN interface) and which slaves you have. | **package_name**: Name of the package | **bus_config_name**: Name of the the bus configuration (you can have multiple) -You can use `ros2 pkg create` command to create your package. +You can use ``ros2 pkg create`` command to create your package. .. code-block:: console @@ -41,14 +41,15 @@ Now your package directory should look like this. Bus configuration creation ------------------------------ -#. **Bus configuration decisions** +#. **Bus configuration decisions** Decide how many bus configurations you need. Add one subfolder for each bus configuration in your `config`-folder. .. code-block:: bash + $ mkdir -p {bus_config_name} -#. **Add device information to bus configurations** +#. **Add device information to bus configurations** Once you created the folders for your bus configurations add the .eds files for the devices that are in the bus configuration to the respective folder. @@ -68,10 +69,10 @@ Bus configuration creation ├── CMakeLists.txt └── package.xml -#. **Create the bus configuration specifications** +#. **Create the bus configuration specifications** To specify the bus configuration ros2_canopen uses the a YAML-file called bus.yml. Create the file in the respective bus configuration folder. - + .. code-block:: console $ touch bus.yml @@ -93,26 +94,26 @@ Bus configuration creation ├── CMakeLists.txt └── package.xml -#. **Edit the bus configuration specifications** +#. **Edit the bus configuration specifications** You need to modify each bus.yml file according to your needs. First you need to define where these files and generated files will be found at runtime. This is usually the following if you use colcon to build from source. .. code-block:: yaml - + options: dcf_path: install/{package_name}/share/{package_name}/config/{bus_config_name} Then you need to define your master. - + .. code-block:: yaml - + master: node_id: [node id] - package: [ros2 package where to find the master driver (usually canopen_core)] - driver: [component type of the driver (ros2_canopen::MasterNode or ros2_canopen::LifecycleMasterNode)] - + package: [ros2 package where to find the master driver (usually canopen_core)] + driver: [component type of the driver (ros2_canopen::MasterDriver or ros2_canopen::LifecycleMasterDriver)] + Make sure, that you specify a lifecycle master if you use the lifecycled version of ros2_canopen. And add other configuration data as necessary. A documentation of configuration options available can be found in the :doc:`configuration` documentation. @@ -125,7 +126,7 @@ Bus configuration creation [unique slave name]: node_id: [node id] - package: [ros2 package where to find the driver] + package: [ros2 package where to find the driver] driver: [qualified name of the driver] Make sure you use a lifecycle slave if you use the lifecycled version of ros2_canopen. @@ -156,7 +157,7 @@ Add the following code: PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), - "/canopen.launch.py", # if lifecycled operation canope_lifecycle.launch.py + "/canopen.launch.py", ] ), launch_arguments={ @@ -236,9 +237,3 @@ Finally we need to adjust the CMakeLists.txt file to pick everything up correctl endif() ament_package() - - - - - - diff --git a/canopen/sphinx/quickstart/examples.rst b/canopen/sphinx/quickstart/examples.rst new file mode 100644 index 00000000..a79913e5 --- /dev/null +++ b/canopen/sphinx/quickstart/examples.rst @@ -0,0 +1,27 @@ +Examples +======== + +In order to tryout the library a few examples are provided in the ``canopen_tests`` directory. +You can run them if you have started the vcan0 interface. + +Service Interface +--------------------- + +.. code-block:: bash + + ros2 launch canopen_tests ci402_setup.launch.py + + +Managed Service Interface +------------------------- + +.. code-block:: bash + + ros2 launch canopen_tests ci402_lifecycle_setup.launch.py + +ROS2 Control +------------ + +.. code-block:: bash + + ros2 launch canopen_tests robot_control_setup.launch.py diff --git a/canopen/sphinx/installation.rst b/canopen/sphinx/quickstart/installation.rst similarity index 77% rename from canopen/sphinx/installation.rst rename to canopen/sphinx/quickstart/installation.rst index dbd3bfd4..35fc07c8 100644 --- a/canopen/sphinx/installation.rst +++ b/canopen/sphinx/quickstart/installation.rst @@ -5,7 +5,7 @@ build with colcon and your done. .. code-block:: console - $ git clone https://gitlab.cc-asp.fraunhofer.de/ipa326/ros-industrial/ros2_canopen + $ git clone https://github.com/ros-industrial/ros2_canopen.git $ cd .. $ rosdep install --from-paths src/ros2_canopen --ignore-src -r -y $ colcon build diff --git a/canopen/sphinx/running-configuration-package.rst b/canopen/sphinx/quickstart/operation.rst similarity index 100% rename from canopen/sphinx/running-configuration-package.rst rename to canopen/sphinx/quickstart/operation.rst diff --git a/canopen/sphinx/sil-tests.rst b/canopen/sphinx/sil-tests.rst deleted file mode 100644 index 62b0d4dd..00000000 --- a/canopen/sphinx/sil-tests.rst +++ /dev/null @@ -1,10 +0,0 @@ -Software in the loop Tests -========================== - -The ROS2 CANopen Stack is continuously tested using linux's virtual can and ROS2's launch_test framework. -These tests are executed on each new release of the framework. - -.. toctree:: - :glob: - - software-tests/* \ No newline at end of file diff --git a/canopen/sphinx/software-tests/concurrency-test.rst b/canopen/sphinx/software-tests/concurrency-test.rst deleted file mode 100644 index db323e55..00000000 --- a/canopen/sphinx/software-tests/concurrency-test.rst +++ /dev/null @@ -1,14 +0,0 @@ -Concurrency Test -================ - -Test details ------------- - -.. csv-table:: Tests - :header: "Detail", "Information" - :delim: ; - - Package; canopen_proxy_driver - Test file; test/test_concurrency.py - Description; Checks concurrent reads and writes with SDO. - Prequisites; van0 must be available \ No newline at end of file diff --git a/canopen/sphinx/software-tests/nmt-test.rst b/canopen/sphinx/software-tests/nmt-test.rst deleted file mode 100644 index 429af8dc..00000000 --- a/canopen/sphinx/software-tests/nmt-test.rst +++ /dev/null @@ -1,14 +0,0 @@ -NMT test -======== - -Test details ------------- - -.. csv-table:: Tests - :header: "Detail", "Information" - :delim: ; - - Package; canopen_proxy_driver - Test file; test/test_nmt.py - Description; Checks whether nmt services are available and can be called. - Prequisites; van0 must be available \ No newline at end of file diff --git a/canopen/sphinx/software-tests/pdo-test.rst b/canopen/sphinx/software-tests/pdo-test.rst deleted file mode 100644 index 53234305..00000000 --- a/canopen/sphinx/software-tests/pdo-test.rst +++ /dev/null @@ -1,14 +0,0 @@ -PDO test -======== - -Test details ------------- - -.. csv-table:: Tests - :header: "Detail", "Information" - :delim: ; - - Package; canopen_proxy_driver - Test file; test/test_pdo.py - Description; Checks whether pdos from a device are received. - Prequisites; van0 must be available \ No newline at end of file diff --git a/canopen/sphinx/software-tests/proxy-driver-test.rst b/canopen/sphinx/software-tests/proxy-driver-test.rst new file mode 100644 index 00000000..18d599ff --- /dev/null +++ b/canopen/sphinx/software-tests/proxy-driver-test.rst @@ -0,0 +1,23 @@ +Lifecycle Proxy Driver Test +=========================== + +.. csv-table:: Testdetails + :header: "Detail", "Information" + :delim: ; + + Package; canopen_tests + Test file; launch_tests/test_lifecycle_proxy_driver.py + Description; Tests nmt, sdo and lifecycle of lifecycle proxy driver + Prerequisites; vcan0 must be available + +Proxy Driver Test +=========================== + +.. csv-table:: Testdetails + :header: "Detail", "Information" + :delim: ; + + Package; canopen_tests + Test file; launch_tests/test_proxy_driver.py + Description; Tests nmt, sdo and lifecycle of proxy driver + Prerequisites; vcan0 must be available diff --git a/canopen/sphinx/software-tests/ros2_control_system-test.rst b/canopen/sphinx/software-tests/ros2_control_system-test.rst new file mode 100644 index 00000000..1809dd1b --- /dev/null +++ b/canopen/sphinx/software-tests/ros2_control_system-test.rst @@ -0,0 +1,49 @@ +ros2_control SystemInterface test +================================= + +Test details +------------ + +.. csv-table:: Tests + :header: "Detail", "Information" + :delim: ; + + Package; canopen_ros2_control + Test file; launch/canopen_system.launch.py + Description; Create an exemplary ros2_control SystemInterface with CAN master and communicates to a slave node. + Prerequisites; vcan0 must be available + + +Explanation of the test +------------------------ + +The test is starting generic system interface and generic controller for CanOpen devices. +Generic system interface enables integration of values from the CAN Bus into ros2_control framework and the controller enables you to send and receive data from the CAN bus through ros2_control to ROS2. + +The next few lines show you some command to have exemplary usage of the ros2_control integration: + +1. After the test is started check ``/dynamic_joint_states`` to show internal data from CAN nodes in ros2_control system. + + .. code-block:: bash + + ros2 topic echo /dynamic_joint_states + + +2. Open a new terminal and echo data from the topic ``/node_1_controller/rpdo``. + + .. code-block:: bash + + ros2 topic echo /node_1_controller/rpdo + + +3. In a new terminal publish some data to the controller to write them to the CAN bus: + + .. code-block:: bash + + ros2 topic pub -r 100 node_1_controller/rpdo canopen_interfaces/msg/COData " + index: 25 + subindex: 35 + data: 238 + type: 8" + + Now watch how data in the first two opened terminals are changing. diff --git a/canopen/sphinx/software-tests/sdo-test.rst b/canopen/sphinx/software-tests/sdo-test.rst deleted file mode 100644 index 844c78f1..00000000 --- a/canopen/sphinx/software-tests/sdo-test.rst +++ /dev/null @@ -1,14 +0,0 @@ -SDO test -======== - -Test details ------------- - -.. csv-table:: Tests - :header: "Detail", "Information" - :delim: ; - - Package; canopen_proxy_driver - Test file; test/test_sdo.py - Description; Checks whether the read and write services are available and tries to read and write values. - Prequisites; van0 must be available \ No newline at end of file diff --git a/canopen/sphinx/system-interface.rst b/canopen/sphinx/system-interface.rst deleted file mode 100644 index 8cdd4278..00000000 --- a/canopen/sphinx/system-interface.rst +++ /dev/null @@ -1,11 +0,0 @@ -System Interface Manager -======================== -*Not yet implemented.* - -The system interface manager implements a configurable ros2_control SystemInterface. -THe implmentation plan is depicted in the following graphic. - -.. figure:: images/system-interface.png - :alt: ros-control interface - - system interface manager concept diff --git a/canopen/sphinx/templates/alpha-test-template.rst b/canopen/sphinx/templates/alpha-test-template.rst deleted file mode 100644 index af8b36f1..00000000 --- a/canopen/sphinx/templates/alpha-test-template.rst +++ /dev/null @@ -1,28 +0,0 @@ -Trinamic PANdrive PD42-1-1270 -============================= - -Test setup ----------- - -.. csv-table:: Tests - :header: "Equipment", "Description" - :delim: ; - - Test systen; [PC hardware you use] - Operating System; [OS you use] - ROS2 Distro; [Distro you use] - CANopen Interface; [Interface (PeakCAN, ...)] - CANopen Devices; [Devices you use] - Stack Version; [Commit of ros2_canopen Stack you use for tests] - - -Tests --------------------- - -.. csv-table:: Tests - :header: "Name", "Status", "Comment" - :delim: ; - - Launching device manager (no lazy load); NA OK NG; ... - Initialise devices; NA OK NG; ... - Operational modes; NA OK NG; ... \ No newline at end of file diff --git a/canopen/sphinx/tested-hardware.rst b/canopen/sphinx/tested-hardware.rst deleted file mode 100644 index a74c427d..00000000 --- a/canopen/sphinx/tested-hardware.rst +++ /dev/null @@ -1,7 +0,0 @@ -Tested Hardware -=============== - -.. toctree:: - :glob: - - hardware-tests/* diff --git a/canopen/sphinx/testing-and-benchmarking.rst b/canopen/sphinx/testing-and-benchmarking.rst deleted file mode 100644 index 602c0f08..00000000 --- a/canopen/sphinx/testing-and-benchmarking.rst +++ /dev/null @@ -1,2 +0,0 @@ -Testing and Benchmarking -======================== diff --git a/canopen/sphinx/motion-controller.rst b/canopen/sphinx/user-guide/cia402-driver.rst similarity index 62% rename from canopen/sphinx/motion-controller.rst rename to canopen/sphinx/user-guide/cia402-driver.rst index 38ad0690..faa4d68e 100644 --- a/canopen/sphinx/motion-controller.rst +++ b/canopen/sphinx/user-guide/cia402-driver.rst @@ -1,14 +1,14 @@ -Motion Controller Driver +Cia402 Driver ======================== -The Motion Controller Driver implements the CIA402 profile and enables setting +The Cia402 Driver implements the CIA402 profile for motion controllers and enables setting the drive status, operation mode and sending target values to the motion controller. Services -------- -.. list-table:: +.. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left @@ -16,12 +16,12 @@ Services * - Services - Type - Description - * - ~/nmt_reset_node + * - ~/nmt_reset_node - Trigger - Resets CANopen Device the Proxy Device Node manages. - * - ~/sdo_read + * - ~/sdo_read - CORead - - Reads an SDO object from the specified index, subindex and datatype of the remote device. + - Reads an SDO object from the specified index, subindex and datatype of the remote device. * - ~/sdo_write - COWrite - Writes data to an SDO object on the specified index, subindex and datatype of the remote device. @@ -49,13 +49,16 @@ Services * - ~/cyclic_velocity_mode - Trigger - Switches to cyclic velocity mode + * - ~/interpolated_position_mode + - Trigger + - Switches to interpolated position mode, only linear mode with fixed time is supported * - ~/target - CODouble - Sets the target value. Only accepted when an operation mode is set. Publishers ---------- -.. list-table:: +.. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left @@ -63,18 +66,15 @@ Publishers * - Publishers - Type - Description - * - ~/actual_position - - Float64 - - Actual position received from motion controller. Units are the units defined on the device. - * - ~/actual_speed - - Float64 - - Actual speed received from motion controller. Units are the units defined on the device. + * - ~/joint_states + - sensor_msgs/msg/JointState + - Joint states of the drive Subscribers ----------- -.. list-table:: +.. list-table:: :widths: 30 20 50 :header-rows: 1 @@ -87,8 +87,10 @@ Subscribers Bus Configuration Parameters ---------------------------- +Additional parameters that can be used in bus.yml for this driver. + -.. list-table:: +.. list-table:: :widths: 30 20 50 :header-rows: 1 @@ -98,3 +100,18 @@ Bus Configuration Parameters * - period - Milliseconds - Refresh period for 402 state machine. Should be similar to sync period of master. + * - switching_state + - see below + - The state to switch the operation mode in. + * - scale_pos_to_dev + - double + - Scaling factor to convert from SI units to device units for position. + * - scale_vel_to_dev + - double + - Scaling factor to convert from SI units to device units for velocity. + * - scale_pos_from_dev + - double + - Scaling factor to convert from device units to SI units for position. + * - scale_vel_from_dev + - double + - Scaling factor to convert from device units to SI units for velocity. diff --git a/canopen/sphinx/configuration.rst b/canopen/sphinx/user-guide/configuration/configuration.rst similarity index 92% rename from canopen/sphinx/configuration.rst rename to canopen/sphinx/user-guide/configuration/configuration.rst index 5b560c43..ce679b0e 100644 --- a/canopen/sphinx/configuration.rst +++ b/canopen/sphinx/user-guide/configuration/configuration.rst @@ -1,7 +1,7 @@ -Bus Configuration +Bus Configuration Reference ============================ -The ros2_canopen stack relies on a YAML configuration file that is used +The ros2_canopen stack relies on a YAML configuration file that is used for configuring the bus topology and specifying configurations for each device. From this configuration file, we generate the device configuration file (DCF) for the CANopen master as well as concise DCF files for master and @@ -16,16 +16,29 @@ The YAML configuration file contains a section for each device on the bus. The s for the master has different configuration options than the section for the slave devices. The file has the following structure. The master section has to be named master. The device sections need be named uniquely. -.. code-block:: +.. code-block:: master: [configuration item]: [value] [...] - + [device_name]: [configuration item]: [value] [...] +Options Section +--------------- +The options section holds general options. Right now these are only the following. + +.. csv-table:: Options Configuration + :header-rows: 1 + :class: longtable + :delim: ; + :widths: 1 1 + + configuration item; description + dcf_path; The directory in which the generated .bin file will be available at runtime. You can set this to "@BUS_CONFIG_PATH@" and it will be auto-generated by cmake. + Master Section -------------- @@ -101,4 +114,4 @@ device. Further references ------------------ -The dcfgen documentation gives more details on the usage of the dcfgen tool for generating DCF: https://opensource.lely.com/canopen/docs/dcf-tools/ \ No newline at end of file +The dcfgen documentation gives more details on the usage of the dcfgen tool for generating DCF: https://opensource.lely.com/canopen/docs/dcf-tools/ diff --git a/canopen/sphinx/master.rst b/canopen/sphinx/user-guide/master.rst similarity index 85% rename from canopen/sphinx/master.rst rename to canopen/sphinx/user-guide/master.rst index aac60dd2..b41b8d2b 100644 --- a/canopen/sphinx/master.rst +++ b/canopen/sphinx/user-guide/master.rst @@ -1,5 +1,5 @@ -Master Node -=========== +Master Driver +============= The master node handles the creation of the necessary can interface and sets up a canopen event loop which drivers can hook onto. In addition, the node offers services for communicating with nodes via nmt and sdo. The device manager will automatically spawn a master node on start-up. The master node is configured by the provided dcf file. @@ -13,15 +13,15 @@ Services * - Services - Type - Description - * - /canopen_master/read_sdo + * - ~/read_sdo - COReadID - Reads an SDO object specified by Index, Subindex and Datatype of the device with the specified nodeid. - * - /canopen_master/write_sdo + * - ~/write_sdo - COWriteID - Writes Data to an SDO object specified by Index, Subindex and Datatype on the device with the specified nodeid. - * - /canopen_master/set_heartbeat + * - ~/set_heartbeat - COHeartbeatID - Sets the heartbeat of the device with the specified nodeid to the heartbeat value (ms) - * - /canopen_master/set_nmt + * - ~/set_nmt - CONmtID - Sends the NMT command to the device with the specified nodeid diff --git a/canopen/sphinx/user-guide/operation/managed-service-interface.rst b/canopen/sphinx/user-guide/operation/managed-service-interface.rst new file mode 100644 index 00000000..c0c858a3 --- /dev/null +++ b/canopen/sphinx/user-guide/operation/managed-service-interface.rst @@ -0,0 +1,52 @@ +Managed Service Interface +============================ + + +Device Container with managed nodes +""""""""""""""""""""""""""""""""""" +The device container implements ROS2 component manager. The load and unload services are disabled. +Devices are loaded based on the Bus Configuration File (bus.yml). The device container provides +the list service, which can be used with ros2cli to check which components have been loaded. + +.. figure:: ../../images/device-manager.png + :alt: Device Manager Concept + + device manager concept + +The device container uses the bus description file to identify the correct drivers for each devices. +On launch it will load the CANopen master node and driver nodes and pass the appropriate configuration +data to the nodes. The nodes are now in unconfigured state. + +When using the default launch files in canopen_core the lifecycle manager node will automatically +be launched. The lifecycle manager takes care of sequencing the lifecycle of the different nodes in the +device container. By bringing the lifecycle_manager to active lifecycle state, all master and driver nodes +will be activated in correct sequence. + +If you choose to write your own lifecycle_manager, you'll need to remember, that the master needs +to be configured and activated before any driver node can be configured or activated. + + +Bus Configuration +""""""""""""""""" +The bus configuration for the managed service interface needs to use the driver classes that are marked as +lifecycle drivers. The master driver indicates whether the bus.yml will be treated as managed or un-managed +service interface. + +.. csv-table:: Available Driver Components + :header: "Package", "Component" + + canopen_master_driver, ros2_canopen::LifecycleMasterDriver + canopen_proxy_driver, ros2_canopen::LifecycleProxyDriver + canopen_402_driver, ros2_canopen::LifecycleCia402Driver + +Launching +""""""""""""" +The device manager has the following configuration parameters. + +.. csv-table:: Parameters + :header: "Parameter", "Type", "Description" + + bus_conf, string, (Mandatory) Path to the bus configuration YAML-file + master_dcf, string, (Mandatory) Path to the DCF file to be used by the master node. Usually generated by dcfgen as master.dcf. + master_bin, string, (Optional) Path to the concise DCF (.bin) file to be used to configure the master. Usually generated by dcfgen as master.bin. (default: "") + can_interface, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) diff --git a/canopen/sphinx/user-guide/operation/operation.rst b/canopen/sphinx/user-guide/operation/operation.rst new file mode 100644 index 00000000..8b1b377d --- /dev/null +++ b/canopen/sphinx/user-guide/operation/operation.rst @@ -0,0 +1,32 @@ +Operation +========= + +The ros2_canopen stack has three different operation modes that should fit for any given operating requirements. +The following operation modes: + +* service interface +* managed service interface +* ros2_control system interface + + +Service interface +"""""""""""""""""" +The service interface has a very simple service interface that enables +using the ros2_canopen stack. Drivers used in this mode cannot be managed +drivers (have Lifecycle in their name). The service interface was developed +for low control frequency and debug purposes. It is not thought for high +freuqency usage. + +Managed service interface +"""""""""""""""""""""""""" +The managed service interface has the same properties as the service interface +with the exception, that it can only use lifecycle drivers (drivers that have +lifecycle in their name). The managed service interface provides more control +over runtime behaviour and recovery options than the service interface. + + +ros2_control system interface +"""""""""""""""""""""""""""""" +The ros2_control system interface provides access to the ros2_canopen stack +via the ros2_control infrastructure. This interface is thought for high frequency +applications. diff --git a/canopen/sphinx/user-guide/operation/ros2-control-interface.rst b/canopen/sphinx/user-guide/operation/ros2-control-interface.rst new file mode 100644 index 00000000..9fa6ccdb --- /dev/null +++ b/canopen/sphinx/user-guide/operation/ros2-control-interface.rst @@ -0,0 +1,77 @@ +ROS2 Control +============================= + + +Hardware Interface +------------------ +This package provides multiple hardware interfaces for testing. Mainly the following: + +- canopen_ros2_control/CanopenSystem: A system interface for ProxyDrivers +- canopen_ros2_control/Cia402System: A system interface for Cia402Drivers +- canopen_ros2_control/Cia402RobotSystem: A system interface for Cia402Drivers in a robot configuration (under development) + + +Robot System Interface +'''''''''''''''''''''' + +The robot system interface takes a number of inputs from the robot description (urdf). +It will make the Cia402Drivers available via the ros2_control hardware interface. +The bus has to still be defined in the bus.yml file. In the urdf you can the choose the +CANopen nodes that have a Cia402Driver attached to them. + +The ros2_control interface only works with non-lifecycle drivers right now. +For each joint in your urdf you can choose the attached CANopen device by using the +``node_id`` parameter. The ``node_id`` parameter is the CANopen node id of the device. + +.. code-block:: xml + + + + canopen_ros2_control/Cia402RobotSystem + [path to bus.yml] + [path to master.dcf] + [can interface to be used] + [master.bin if it exists] + + + 3 + ... + + + 3 + ... + + + +.. note:: + + You can find an example for the configuration in the ``canopen_tests`` package under robot_control. + + +ROS2 Controllers +---------------- +This package provides multiple controllers for testing. Mainly the following: + +- canopen_ros2_controllers/Cia402RobotController: Works with Robot System Interface +- canopen_ros2_controllers/Cia402DeviceController: Works with Cia402System +- canopen_ros2_controllers/CanopenProxyController: Works with CanopenSystem and Cia402System + +Robot Controller +'''''''''''''''' + +The robot controller enables bringing up the different joints of the robot automatically +by using the ros2_controller lifecycle. There is no need for further action, once the +controller is activated, the drives are ready to be used. + +The robot controller can be configured in the ros2_controllers.yaml with the following +parameters: + +.. code-block:: yaml + + robot_controller: + ros__parameters: + joints: # joints that are controlled by the controller + - joint1 + - joint2 + operation_mode: 1 # operation mode of the controller + command_poll_freq: 5 # frequency with which the controller polls for command feedback diff --git a/canopen/sphinx/user-guide/operation/service-interface.rst b/canopen/sphinx/user-guide/operation/service-interface.rst new file mode 100644 index 00000000..f177420b --- /dev/null +++ b/canopen/sphinx/user-guide/operation/service-interface.rst @@ -0,0 +1,54 @@ +Service Interface +================== + + +Device Container +""""""""""""""""" +The device container implements ROS2 component manager. The load and unload services are disabled. +Devices are loaded based on the Bus Configuration File (bus.yml). It provides the list service though. + +.. figure:: ../../images/device-manager.png + :alt: Device Manager Concept + + device manager concept + +The device manager uses the bus description file to identify the correct drivers for each devices. +On launch it will load the CANopen master node and pass the generated DCF files to configure the CANopen master +correctly for you bus configuration. It will the enable the master. Once the master is enabled it will +sequentially load and enable all drivers in the bus configuration. + +Once a CANopen Node comes online (i.e. sends the boot indication) the CANopen master +will configure the node with the parameters and commands specified in the bus configuration for that device. +When the configuration of the device is done, all data send by the device is forwarded +to the appropriate driver. + +All loaded nodes are added to the device manager's executor. + +.. figure:: ../../images/device-manager-usage.png + :alt: Device Manager Usage + + device manager usage + +Bus Configuration +""""""""""""""""" +The bus configuration for the needs to use the driver classes that are marked as +non lifecycle drivers. + +.. csv-table:: Available Driver Components + :header: "Package", "Component" + + canopen_core, ros2_canopen::MasterDriver + canopen_proxy_driver, ros2_canopen::ProxyDriver + canopen_402_driver, ros2_canopen::Cia402Driver + +Launching +""""""""""""" +The device manager has the following configuration parameters. + +.. csv-table:: Parameters + :header: "Parameter", "Type", "Description" + + bus_conf, string, (Mandatory) Path to the bus configuration YAML-file + master_dcf, string, (Mandatory) Path to the DCF file to be used by the master node. Usually generated by dcfgen as master.dcf. + master_bin, string, (Optional) Path to the concise DCF (.bin) file to be used to configure the master. Usually generated by dcfgen as master.bin. (default: "") + can_interface, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) diff --git a/canopen/sphinx/proxy-device.rst b/canopen/sphinx/user-guide/proxy-driver.rst similarity index 80% rename from canopen/sphinx/proxy-device.rst rename to canopen/sphinx/user-guide/proxy-driver.rst index 156e8f48..9103f559 100644 --- a/canopen/sphinx/proxy-device.rst +++ b/canopen/sphinx/user-guide/proxy-driver.rst @@ -5,7 +5,7 @@ A proxy driver which simply forwards CANopen functionality for a specific device Services -------- -.. list-table:: +.. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left @@ -13,12 +13,12 @@ Services * - Services - Type - Description - * - ~/nmt_reset_node + * - ~/nmt_reset_node - Trigger - Resets CANopen Device the Proxy Device Node manages. - * - ~/sdo_read + * - ~/sdo_read - CORead - - Reads an SDO object from the specified index, subindex and datatype of the remote device. + - Reads an SDO object from the specified index, subindex and datatype of the remote device. * - ~/sdo_write - COWrite - Writes data to an SDO object on the specified index, subindex and datatype of the remote device. @@ -27,7 +27,7 @@ Services Publishers ---------- -.. list-table:: +.. list-table:: :widths: 30 20 50 :header-rows: 1 :align: left @@ -35,24 +35,23 @@ Publishers * - Topic - Type - Description - * - ~/nmt_state + * - ~/nmt_state - String - Publishes NMT state on change - * - ~/rpdo + * - ~/rpdo - COData - - Publishes received PDO objects on reception + - Publishes received PDO objects on reception Subscribers ----------- -.. list-table:: +.. list-table:: :widths: 30 20 50 :header-rows: 1 * - Topic - Type - Description - * - ~/tpdo + * - ~/tpdo - COData - Writes received data to remote device if the specified object is RPDO mapped on remote device. - diff --git a/canopen_402_driver/CMakeLists.txt b/canopen_402_driver/CMakeLists.txt index b97bb94a..901dc436 100644 --- a/canopen_402_driver/CMakeLists.txt +++ b/canopen_402_driver/CMakeLists.txt @@ -2,84 +2,120 @@ cmake_minimum_required(VERSION 3.8) project(canopen_402_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) +find_package(canopen_base_driver REQUIRED) +find_package(canopen_core REQUIRED) +find_package(canopen_interfaces REQUIRED) +find_package(canopen_proxy_driver REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(canopen_proxy_driver REQUIRED) -find_package(Boost REQUIRED COMPONENTS container) find_package(rclcpp_lifecycle REQUIRED) -find_package(lifecycle_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +set(dependencies + canopen_base_driver + canopen_core + canopen_interfaces + canopen_proxy_driver + rclcpp + rclcpp_lifecycle + rclcpp_components + sensor_msgs + std_srvs +) -add_library(${PROJECT_NAME} - src/canopen_402_driver.cpp +add_library(lely_motion_controller_bridge src/motor.cpp - ) + src/command.cpp + src/state.cpp + src/default_homing_mode.cpp +) +target_compile_features(lely_motion_controller_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(lely_motion_controller_bridge PUBLIC -Wl,--no-undefined) +target_include_directories(lely_motion_controller_bridge PUBLIC + $ + $) -target_compile_features(canopen_402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_compile_options(canopen_402_driver PUBLIC -Wno-unused-parameter) -target_include_directories(canopen_402_driver PUBLIC +ament_target_dependencies( + lely_motion_controller_bridge + ${dependencies} +) + + +add_library(node_canopen_cia402_driver + src/node_interfaces/node_canopen_402_driver.cpp +) +target_compile_features(node_canopen_cia402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(node_canopen_cia402_driver PUBLIC -Wl,--no-undefined) +target_include_directories(node_canopen_cia402_driver PUBLIC $ $) +target_link_libraries(node_canopen_cia402_driver + lely_motion_controller_bridge +) ament_target_dependencies( - canopen_402_driver - "rclcpp" - "rclcpp_components" - "canopen_interfaces" - "std_msgs" - "std_srvs" - "lely_core_libraries" - "canopen_core" - "canopen_proxy_driver" - "canopen_base_driver" + node_canopen_cia402_driver + ${dependencies} + ) +add_library(lifecycle_cia402_driver + src/lifecycle_cia402_driver.cpp + ) +target_compile_features(lifecycle_cia402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(lifecycle_cia402_driver PUBLIC -Wl,--no-undefined) +target_include_directories(lifecycle_cia402_driver PUBLIC + $ + $) + +target_link_libraries(lifecycle_cia402_driver + node_canopen_cia402_driver + lely_motion_controller_bridge +) +ament_target_dependencies( + lifecycle_cia402_driver + ${dependencies} +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(canopen_402_driver PRIVATE "CANOPEN_402_DRIVER_BUILDING_LIBRARY") +target_compile_definitions(lifecycle_cia402_driver PRIVATE "CANOPEN_402_DRIVER_BUILDING_LIBRARY") + +rclcpp_components_register_nodes(lifecycle_cia402_driver "ros2_canopen::LifecycleCia402Driver") +set(node_plugins "${node_plugins}ros2_canopen::LifecycleCia402Driver;$\n") -rclcpp_components_register_nodes(canopen_402_driver "ros2_canopen::MotionControllerDriver") -set(node_plugins "${node_plugins}ros2_canopen::MotionControllerDriver;$\n") -add_library(lifecycle_canopen_402_driver - src/lifecycle_canopen_402_driver.cpp - src/motor.cpp - ) -target_compile_features(lifecycle_canopen_402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_compile_options(lifecycle_canopen_402_driver PUBLIC -Wno-unused-parameter) -target_include_directories(lifecycle_canopen_402_driver PUBLIC +add_library(cia402_driver + src/cia402_driver.cpp + ) +target_compile_features(cia402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(cia402_driver PUBLIC -Wl,--no-undefined) +target_include_directories(cia402_driver PUBLIC $ $) +target_link_libraries(cia402_driver + node_canopen_cia402_driver + lely_motion_controller_bridge +) + ament_target_dependencies( - lifecycle_canopen_402_driver - "rclcpp" - "rclcpp_components" - "canopen_interfaces" - "std_msgs" - "std_srvs" - "lely_core_libraries" - "canopen_core" - "canopen_proxy_driver" - "canopen_base_driver" - "rclcpp_lifecycle" - "lifecycle_msgs" + cia402_driver + ${dependencies} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(lifecycle_canopen_402_driver PRIVATE "CANOPEN_402_DRIVER_BUILDING_LIBRARY") - -rclcpp_components_register_nodes(lifecycle_canopen_402_driver "ros2_canopen::LifecycleMotionControllerDriver") -set(node_plugins "${node_plugins}ros2_canopen::LifecycleMotionControllerDriver;$\n") +target_compile_definitions(cia402_driver PRIVATE "CANOPEN_cia402_driver_BUILDING_LIBRARY") +rclcpp_components_register_nodes(cia402_driver "ros2_canopen::Cia402Driver") +set(node_plugins "${node_plugins}ros2_canopen::Cia402Driver;$\n") install( DIRECTORY include/ @@ -87,42 +123,32 @@ install( ) install( - TARGETS canopen_402_driver - EXPORT export_canopen_402_driver - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - TARGETS lifecycle_canopen_402_driver - EXPORT export_lifecycle_canopen_402_driver + TARGETS lifecycle_cia402_driver cia402_driver node_canopen_cia402_driver lely_motion_controller_bridge + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - # ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( - canopen_402_driver - lifecycle_canopen_402_driver + lifecycle_cia402_driver + cia402_driver + node_canopen_cia402_driver + lely_motion_controller_bridge ) ament_export_targets( - export_canopen_402_driver - export_lifecycle_canopen_402_driver + export_${PROJECT_NAME} +) +ament_export_dependencies( + ${dependencies} ) ament_package() diff --git a/canopen_402_driver/include/canopen_402_driver/base.hpp b/canopen_402_driver/include/canopen_402_driver/base.hpp index 889eb375..11c33b54 100644 --- a/canopen_402_driver/include/canopen_402_driver/base.hpp +++ b/canopen_402_driver/include/canopen_402_driver/base.hpp @@ -2,80 +2,81 @@ #define CANOPEN_402_BASE_H #include -#include "lely/coapp/master.hpp" #include "lely/coapp/driver.hpp" +#include "lely/coapp/master.hpp" -namespace canopen_402 +namespace ros2_canopen { /** * @brief Motor Base Class - * + * */ -class MotorBase { +class MotorBase +{ protected: - MotorBase() {} -public: - enum OperationMode - { - No_Mode = 0, - Profiled_Position = 1, - Velocity = 2, - Profiled_Velocity = 3, - Profiled_Torque = 4, - Reserved = 5, - Homing = 6, - Interpolated_Position = 7, - Cyclic_Synchronous_Position = 8, - Cyclic_Synchronous_Velocity = 9, - Cyclic_Synchronous_Torque = 10, - }; + MotorBase() {} - /** - * @brief Set target - * - * @param [in] val Target value - * @return true - * @return false - */ - virtual bool setTarget(double val) = 0; +public: + enum OperationMode + { + No_Mode = 0, + Profiled_Position = 1, + Velocity = 2, + Profiled_Velocity = 3, + Profiled_Torque = 4, + Reserved = 5, + Homing = 6, + Interpolated_Position = 7, + Cyclic_Synchronous_Position = 8, + Cyclic_Synchronous_Velocity = 9, + Cyclic_Synchronous_Torque = 10, + }; - /** - * @brief Enter Operation Mode - * - * @param [in] mode Target Mode - * @return true - * @return false - */ - virtual bool enterModeAndWait(uint16_t mode) = 0; + /** + * @brief Set target + * + * @param [in] val Target value + * @return true + * @return false + */ + virtual bool setTarget(double val) = 0; - /** - * @brief Check if Operation Mode is supported - * - * @param [in] mode Operation Mode to be checked - * @return true - * @return false - */ - virtual bool isModeSupported(uint16_t mode) = 0; + /** + * @brief Enter Operation Mode + * + * @param [in] mode Target Mode + * @return true + * @return false + */ + virtual bool enterModeAndWait(uint16_t mode) = 0; - /** - * @brief Get current Mode - * - * @return uint16_t - */ - virtual uint16_t getMode() = 0; + /** + * @brief Check if Operation Mode is supported + * + * @param [in] mode Operation Mode to be checked + * @return true + * @return false + */ + virtual bool isModeSupported(uint16_t mode) = 0; - /** - * @brief Register default Operation Modes - * - */ - virtual void registerDefaultModes() {} + /** + * @brief Get current Mode + * + * @return uint16_t + */ + virtual uint16_t getMode() = 0; - typedef std::shared_ptr MotorBaseSharedPtr; + /** + * @brief Register default Operation Modes + * + */ + virtual void registerDefaultModes() {} + typedef std::shared_ptr MotorBaseSharedPtr; }; typedef MotorBase::MotorBaseSharedPtr MotorBaseSharedPtr; -} +} // namespace ros2_canopen #endif diff --git a/canopen_402_driver/include/canopen_402_driver/canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/canopen_402_driver.hpp deleted file mode 100644 index c4c0aa37..00000000 --- a/canopen_402_driver/include/canopen_402_driver/canopen_402_driver.hpp +++ /dev/null @@ -1,218 +0,0 @@ -#ifndef MC_DEVICE_NODE_HPP -#define MC_DEVICE_NODE_HPP - -#include "canopen_402_driver/visibility_control.h" -#include "std_srvs/srv/trigger.hpp" -#include "std_msgs/msg/float64.hpp" -#include "canopen_interfaces/srv/co_target_double.hpp" -#include "canopen_proxy_driver/canopen_proxy_driver.hpp" -#include "canopen_402_driver/motor.hpp" - -using namespace std::chrono_literals; -using namespace ros2_canopen; -using namespace canopen_402; -namespace ros2_canopen -{ - /** - * @brief ROS2 node for a Motion Controller - * - * This class provides a ros2 node for a CIA 402 - * device. - */ - class MotionControllerDriver : public ProxyDriver - { - private: - std::shared_ptr mc_driver_; - std::shared_ptr motor_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Service::SharedPtr handle_init_service; - rclcpp::Service::SharedPtr handle_halt_service; - rclcpp::Service::SharedPtr handle_recover_service; - rclcpp::Service::SharedPtr handle_set_mode_position_service; - rclcpp::Service::SharedPtr handle_set_mode_torque_service; - rclcpp::Service::SharedPtr handle_set_mode_velocity_service; - rclcpp::Service::SharedPtr handle_set_mode_cyclic_velocity_service; - rclcpp::Service::SharedPtr handle_set_mode_cyclic_position_service; - rclcpp::Service::SharedPtr handle_set_target_service; - rclcpp::Publisher::SharedPtr publish_actual_position; - rclcpp::Publisher::SharedPtr publish_actual_speed; - rclcpp::CallbackGroup::SharedPtr timer_group; - uint32_t period_ms_; - bool intialised; - void register_services(); - - public: - explicit MotionControllerDriver(const rclcpp::NodeOptions &options) - : ProxyDriver(options) - { - intialised = false; - } - - void run() - { - if(!intialised) - { - RCLCPP_INFO(this->get_logger(), "Intitialising Device and Objects"); - timer_->cancel(); - intialised = true; - motor_->registerDefaultModes(); - mc_driver_->validate_objs(); - timer_= this->create_wall_timer( - std::chrono::milliseconds(period_ms_), std::bind(&MotionControllerDriver::run, this), timer_group); - } - - motor_->handleRead(); - motor_->handleWrite(); - //motor_->handleDiag(); - publish(); - } - - void init(ev::Executor &exec, - canopen::AsyncMaster &master, - uint8_t node_id, - std::shared_ptr config) noexcept override; - - protected: - virtual void on_rpdo(COData data) override - { - RCLCPP_INFO(this->get_logger(), "on_rpo not implemented"); - } - - private: - std::atomic active; - - /** - * @brief Service Callback to initialise device - * - * Calls Motor402::handleInit function. Brings motor to enabled - * state and homes it. - * - * @param [in] request - * @param [out] response - */ - void handle_init( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to recover device - * - * Calls Motor402::handleRecover function. Resets faults and brings - * motor to enabled state. - * - * @param [in] request - * @param [out] response - */ - void handle_recover( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to halt device - * - * Calls Motor402::handleHalt function. Calls Quickstop. Resulting - * Motor state depends on devices configuration specifically object - * 0x605A. - * - * @param [in] request - * @param [out] response - */ - void handle_halt( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set profiled position mode - * - * Calls Motor402::enterModeAndWait with Profiled Position Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Profiled Position Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set profiled velocity mode - * - * Calls Motor402::enterModeAndWait with Profiled Velocity Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Profiled Velocity Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set cyclic position mode - * - * Calls Motor402::enterModeAndWait with Cyclic Position Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Cyclic Position Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_cyclic_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - - /** - * @brief Service Callback to set cyclic velocity mode - * - * Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Cyclic Velocity Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_cyclic_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set profiled torque mode - * - * Calls Motor402::enterModeAndWait with Profiled Torque Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Profiled Torque Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_torque( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set target - * - * Calls Motor402::setTarget and sets the requested target value. Note - * that the resulting movement is dependent on the Operation Mode and the - * drives state. - * - * @param [in] request - * @param [out] response - */ - void handle_set_target( - const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, - canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response); - - /** - * @brief Publishes actual position and speed - * - */ - void publish(); - }; - -} - -#endif \ No newline at end of file diff --git a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp new file mode 100644 index 00000000..2eaaa875 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp @@ -0,0 +1,87 @@ +#ifndef CANOPEN_402_DRIVER__402_DRIVER_HPP_ +#define CANOPEN_402_DRIVER__402_DRIVER_HPP_ +#include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" +#include "canopen_core/driver_node.hpp" + +namespace ros2_canopen +{ +/** + * @brief Abstract Class for a CANopen Device Node + * + * This class provides the base functionality for creating a + * CANopen device node. It provides callbacks for nmt and rpdo. + */ +class Cia402Driver : public ros2_canopen::CanopenDriver +{ + std::shared_ptr> node_canopen_402_driver_; + +public: + Cia402Driver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); + + virtual bool reset_node_nmt_command() + { + return node_canopen_402_driver_->reset_node_nmt_command(); + } + + virtual bool start_node_nmt_command() + { + return node_canopen_402_driver_->start_node_nmt_command(); + } + + virtual bool tpdo_transmit(ros2_canopen::COData & data) + { + return node_canopen_402_driver_->tpdo_transmit(data); + } + + virtual bool sdo_write(ros2_canopen::COData & data) + { + return node_canopen_402_driver_->sdo_write(data); + } + + virtual bool sdo_read(ros2_canopen::COData & data) + { + return node_canopen_402_driver_->sdo_read(data); + } + + void register_nmt_state_cb(std::function nmt_state_cb) + { + node_canopen_402_driver_->register_nmt_state_cb(nmt_state_cb); + } + + void register_rpdo_cb(std::function rpdo_cb) + { + node_canopen_402_driver_->register_rpdo_cb(rpdo_cb); + } + + double get_speed() { return node_canopen_402_driver_->get_speed(); } + + double get_position() { return node_canopen_402_driver_->get_position(); } + + bool set_target(double target) { return node_canopen_402_driver_->set_target(target); } + + bool init_motor() { return node_canopen_402_driver_->init_motor(); } + + bool recover_motor() { return node_canopen_402_driver_->recover_motor(); } + + bool halt_motor() { return node_canopen_402_driver_->halt_motor(); } + + bool set_mode_position() { return node_canopen_402_driver_->set_mode_position(); } + + bool set_mode_velocity() { return node_canopen_402_driver_->set_mode_velocity(); } + + bool set_mode_cyclic_position() { return node_canopen_402_driver_->set_mode_cyclic_position(); } + + bool set_mode_cyclic_velocity() { return node_canopen_402_driver_->set_mode_cyclic_velocity(); } + + bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); } + + uint16_t get_mode() { return node_canopen_402_driver_->get_mode(); } + + bool set_operation_mode(uint16_t mode) + { + return node_canopen_402_driver_->set_operation_mode(mode); + } +}; +} // namespace ros2_canopen + +#endif // CANOPEN_402_DRIVER__CANOPEN_402_DRIVER_HPP_ diff --git a/canopen_402_driver/include/canopen_402_driver/command.hpp b/canopen_402_driver/include/canopen_402_driver/command.hpp new file mode 100644 index 00000000..855399ea --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/command.hpp @@ -0,0 +1,66 @@ +#ifndef CANOPEN_402_DRIVER_COMMAND_HPP +#define CANOPEN_402_DRIVER_COMMAND_HPP + +#include +#include +#include +#include "state.hpp" + +namespace ros2_canopen +{ +class Command402 +{ + struct Op + { + uint16_t to_set_; + uint16_t to_reset_; + Op(uint16_t to_set, uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {} + void operator()(uint16_t & val) const { val = (val & ~to_reset_) | to_set_; } + }; + class TransitionTable + { + boost::container::flat_map, Op> + transitions_; + void add(const State402::InternalState & from, const State402::InternalState & to, Op op) + { + transitions_.insert(std::make_pair(std::make_pair(from, to), op)); + } + + public: + TransitionTable(); + const Op & get(const State402::InternalState & from, const State402::InternalState & to) const + { + return transitions_.at(std::make_pair(from, to)); + } + }; + static const TransitionTable transitions_; + static State402::InternalState nextStateForEnabling(State402::InternalState state); + Command402(); + +public: + enum ControlWord + { + CW_Switch_On = 0, + CW_Enable_Voltage = 1, + CW_Quick_Stop = 2, + CW_Enable_Operation = 3, + CW_Operation_mode_specific0 = 4, + CW_Operation_mode_specific1 = 5, + CW_Operation_mode_specific2 = 6, + CW_Fault_Reset = 7, + CW_Halt = 8, + CW_Operation_mode_specific3 = 9, + // CW_Reserved1=10, + CW_Manufacturer_specific0 = 11, + CW_Manufacturer_specific1 = 12, + CW_Manufacturer_specific2 = 13, + CW_Manufacturer_specific3 = 14, + CW_Manufacturer_specific4 = 15, + }; + static bool setTransition( + uint16_t & cw, const State402::InternalState & from, const State402::InternalState & to, + State402::InternalState * next); +}; +} // namespace ros2_canopen + +#endif // CANOPEN_402_DRIVER_COMMAND_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp b/canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp new file mode 100644 index 00000000..079d50af --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp @@ -0,0 +1,43 @@ +#ifndef DEFAULT_HOMING_MODE_HPP +#define DEFAULT_HOMING_MODE_HPP +#include +#include "canopen_base_driver/lely_driver_bridge.hpp" +#include "homing_mode.hpp" + +namespace ros2_canopen +{ + +class DefaultHomingMode : public HomingMode +{ + const uint16_t index = 0x6098; + std::shared_ptr driver; + + std::atomic execute_; + + std::mutex mutex_; + std::condition_variable cond_; + uint16_t status_; + + enum SW_masks + { + MASK_Reached = (1 << State402::SW_Target_reached), + MASK_Attained = (1 << SW_Attained), + MASK_Error = (1 << SW_Error), + }; + bool error(const std::string & msg) + { + execute_ = false; + std::cout << msg << std::endl; + return false; + } + +public: + DefaultHomingMode(std::shared_ptr driver) { this->driver = driver; } + virtual bool start(); + virtual bool read(const uint16_t & sw); + virtual bool write(OpModeAccesser & cw); + + virtual bool executeHoming(); +}; +} // namespace ros2_canopen +#endif // DEFAULT_HOMING_MODE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/homing_mode.hpp b/canopen_402_driver/include/canopen_402_driver/homing_mode.hpp new file mode 100644 index 00000000..31103426 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/homing_mode.hpp @@ -0,0 +1,27 @@ +#ifndef HOMING_MODE_HPP +#define HOMING_MODE_HPP +#include "base.hpp" +#include "mode.hpp" + +namespace ros2_canopen +{ +class HomingMode : public Mode +{ +protected: + enum SW_bits + { + SW_Attained = State402::SW_Operation_mode_specific0, + SW_Error = State402::SW_Operation_mode_specific1, + }; + enum CW_bits + { + CW_StartHoming = Command402::CW_Operation_mode_specific0, + }; + +public: + HomingMode() : Mode(MotorBase::Homing) {} + virtual bool executeHoming() = 0; +}; +} // namespace ros2_canopen + +#endif // HOMING_MODE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/lifecycle_canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/lifecycle_canopen_402_driver.hpp deleted file mode 100644 index f448e1b7..00000000 --- a/canopen_402_driver/include/canopen_402_driver/lifecycle_canopen_402_driver.hpp +++ /dev/null @@ -1,277 +0,0 @@ - -#ifndef MC_DEVICE_NODE_HPP -#define MC_DEVICE_NODE_HPP - -#include "canopen_402_driver/visibility_control.h" -#include "std_srvs/srv/trigger.hpp" -#include "std_msgs/msg/float64.hpp" -#include "canopen_interfaces/srv/co_target_double.hpp" -#include "canopen_proxy_driver/lifecycle_canopen_proxy_driver.hpp" -#include "canopen_402_driver/motor.hpp" - -using namespace std::chrono_literals; -using namespace ros2_canopen; -using namespace canopen_402; -namespace ros2_canopen -{ - /** - * @brief ROS2 node for a Motion Controller - * - * This class provides a ros2 node for a CIA 402 - * device. - */ - class LifecycleMotionControllerDriver : public LifecycleProxyDriver - { - private: - std::shared_ptr mc_driver_; - std::shared_ptr motor_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Service::SharedPtr handle_init_service; - rclcpp::Service::SharedPtr handle_halt_service; - rclcpp::Service::SharedPtr handle_recover_service; - rclcpp::Service::SharedPtr handle_set_mode_position_service; - rclcpp::Service::SharedPtr handle_set_mode_torque_service; - rclcpp::Service::SharedPtr handle_set_mode_velocity_service; - rclcpp::Service::SharedPtr handle_set_mode_cyclic_velocity_service; - rclcpp::Service::SharedPtr handle_set_mode_cyclic_position_service; - rclcpp::Service::SharedPtr handle_set_target_service; - rclcpp::Publisher::SharedPtr publish_actual_position; - rclcpp::Publisher::SharedPtr publish_actual_speed; - uint32_t period_ms_; - virtual bool add() override; - virtual void register_ros_interface() override; - virtual void start_timers() override; - virtual void stop_timers() override; - - public: - explicit LifecycleMotionControllerDriver(const rclcpp::NodeOptions &options) - : LifecycleProxyDriver(options) - { - } - - void run() - { - motor_->handleRead(); - motor_->handleWrite(); - // motor_->handleDiag(); - publish(); - } - - void init() override - { - LifecycleProxyDriver::init(); - period_ms_ = 20; - } - - protected: - // void read_config() override - // { - // RCLCPP_INFO(this->get_logger(), "read_config_start"); - // auto period = this->config_->get_entry(std::string(this->get_name()), std::string("period")); - // RCLCPP_INFO(this->get_logger(), "read_config_start"); - // if (!period.has_value()) - // { - // RCLCPP_ERROR(this->get_logger(), "ERROR: Bus Configuration does not set period for %s", this->get_name()); - // } - // RCLCPP_INFO(this->get_logger(), "read_config_start"); - // period_ms_ = period.value(); - // RCLCPP_INFO(this->get_logger(), "read_config_end"); - // } - - virtual void on_rpdo(COData data) override - { - RCLCPP_INFO(this->get_logger(), "on_rpo not implemented"); - } - - /** - * @brief Configures the driver - * - * Read parameters - * Initialise objects - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &state); - - /** - * @brief Activates the driver - * - * Add driver to masters event loop - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &state); - - /** - * @brief Deactivates the driver - * - * Remove driver from masters event loop - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &state); - - /** - * @brief Cleanup the driver - * - * Delete objects - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &state); - - /** - * @brief Shutdowns the driver - * - * Delete objects - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &state); - - private: - - /** - * @brief Service Callback to initialise device - * - * Calls Motor402::handleInit function. Brings motor to enabled - * state and homes it. - * - * @param [in] request - * @param [out] response - */ - void handle_init( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to recover device - * - * Calls Motor402::handleRecover function. Resets faults and brings - * motor to enabled state. - * - * @param [in] request - * @param [out] response - */ - void handle_recover( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to halt device - * - * Calls Motor402::handleHalt function. Calls Quickstop. Resulting - * Motor state depends on devices configuration specifically object - * 0x605A. - * - * @param [in] request - * @param [out] response - */ - void handle_halt( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set profiled position mode - * - * Calls Motor402::enterModeAndWait with Profiled Position Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Profiled Position Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set profiled velocity mode - * - * Calls Motor402::enterModeAndWait with Profiled Velocity Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Profiled Velocity Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set cyclic position mode - * - * Calls Motor402::enterModeAndWait with Cyclic Position Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Cyclic Position Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_cyclic_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set cyclic velocity mode - * - * Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Cyclic Velocity Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_cyclic_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set profiled torque mode - * - * Calls Motor402::enterModeAndWait with Profiled Torque Mode as - * Target Operation Mode. If successful, the motor was transitioned - * to Profiled Torque Mode. - * - * @param [in] request - * @param [out] response - */ - void handle_set_mode_torque( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - /** - * @brief Service Callback to set target - * - * Calls Motor402::setTarget and sets the requested target value. Note - * that the resulting movement is dependent on the Operation Mode and the - * drives state. - * - * @param [in] request - * @param [out] response - */ - void handle_set_target( - const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, - canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response); - - /** - * @brief Publishes actual position and speed - * - */ - void publish(); - }; - -} - -#endif diff --git a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp new file mode 100644 index 00000000..c92912f2 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp @@ -0,0 +1,82 @@ +#ifndef CANOPEN_402_DRIVER__CANOPEN_LIFECYCLE_402_DRIVER_HPP_ +#define CANOPEN_402_DRIVER__CANOPEN_LIFECYCLE_402_DRIVER_HPP_ + +#include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" +#include "canopen_core/driver_node.hpp" + +namespace ros2_canopen +{ +/** + * @brief Lifecycle 402 Driver + * + * A very basic driver without any functionality. + * + */ +class LifecycleCia402Driver : public ros2_canopen::LifecycleCanopenDriver +{ + std::shared_ptr> + node_canopen_402_driver_; + +public: + LifecycleCia402Driver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); + + virtual bool reset_node_nmt_command() + { + return node_canopen_402_driver_->reset_node_nmt_command(); + } + + virtual bool start_node_nmt_command() + { + return node_canopen_402_driver_->start_node_nmt_command(); + } + + virtual bool tpdo_transmit(ros2_canopen::COData & data) + { + return node_canopen_402_driver_->tpdo_transmit(data); + } + + virtual bool sdo_write(ros2_canopen::COData & data) + { + return node_canopen_402_driver_->sdo_write(data); + } + + virtual bool sdo_read(ros2_canopen::COData & data) + { + return node_canopen_402_driver_->sdo_read(data); + } + + void register_nmt_state_cb(std::function nmt_state_cb) + { + node_canopen_402_driver_->register_nmt_state_cb(nmt_state_cb); + } + + void register_rpdo_cb(std::function rpdo_cb) + { + node_canopen_402_driver_->register_rpdo_cb(rpdo_cb); + } + + double get_speed() { return node_canopen_402_driver_->get_speed(); } + + double get_position() { return node_canopen_402_driver_->get_position(); } + + bool set_target(double target) { return node_canopen_402_driver_->set_target(target); } + + bool init_motor() { return node_canopen_402_driver_->init_motor(); } + + bool recover_motor() { return node_canopen_402_driver_->recover_motor(); } + + bool halt_motor() { return node_canopen_402_driver_->halt_motor(); } + + bool set_mode_position() { return node_canopen_402_driver_->set_mode_position(); } + + bool set_mode_velocity() { return node_canopen_402_driver_->set_mode_velocity(); } + + bool set_mode_cyclic_position() { return node_canopen_402_driver_->set_mode_cyclic_position(); } + + bool set_mode_cyclic_velocity() { return node_canopen_402_driver_->set_mode_cyclic_velocity(); } + + bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); } +}; +} // namespace ros2_canopen + +#endif // CANOPEN_402_DRIVER__CANOPEN_402_DRIVER_HPP_ diff --git a/canopen_402_driver/include/canopen_402_driver/mc_device_driver.hpp b/canopen_402_driver/include/canopen_402_driver/mc_device_driver.hpp deleted file mode 100644 index a3268c0f..00000000 --- a/canopen_402_driver/include/canopen_402_driver/mc_device_driver.hpp +++ /dev/null @@ -1,311 +0,0 @@ -#ifndef MC_DEVICE_DRIVER_HPP -#define MC_DEVICE_DRIVER_HPP -#include -#include -#include -#include -#include - -#include "canopen_402_driver/base.hpp" -#include "canopen_base_driver/lely_bridge.hpp" - -using namespace ros2_canopen; -namespace ros2_canopen -{ - struct RemoteObject - { - uint16_t index; - uint8_t subindex; - uint32_t data; - CODataTypes type; - bool tpdo_mapped; - bool rpdo_mapped; - bool valid; - }; - - /** - * @brief Specialised LelyBridge for MotionControllers - * - * This class provides funtionalities necessary for interacting - * with the canopen_402 stack from ros_canopen. - */ - class MCDeviceDriver : public LelyBridge - { - private: - std::vector> objs; - bool sync; - double speed; - double position; - - public: - /** - * @brief Create a remote obj object - * - * This function registers an object on the remote that is frequently used - * to control the device. A pointer to the object is stored. - * - * @param [in] index Index of remote object - * @param [in] subindex Subindex of remote object - * @param [in] type Type of remote object - * @return std::shared_ptr - */ - std::shared_ptr create_remote_obj(uint16_t index, uint8_t subindex, CODataTypes type) - { - RemoteObject obj = {index, subindex, 0, type, false, false, true}; - for(auto it = objs.begin(); it != objs.end(); ++it) - { - if( - ((*it)->index == index) - && - ((*it)->subindex == subindex) - && - ((*it)->type == type) - ) - { - return *it; - } - } - std::shared_ptr objp = std::make_shared(obj); - objs.push_back(objp); - return objp; - } - - /** - * @brief Update registered objectsa on RPDO write - * - * This function is called when an RPDO write request is received - * from the remote device. The funciton updates the registered objects - * in its remote dictionary. - * - * @param [in] idx Index of written object - * @param [in] subidx Subindex of written object - */ - void OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept override - { - for (auto it = objs.begin(); it != objs.end(); ++it) - { - std::shared_ptr obj = *it; - if (obj->index == idx && obj->subindex == subidx) - { - if(obj->type == CODataTypes::COData8) - obj->data = rpdo_mapped[idx][subidx].Read(); - else if(obj->type == CODataTypes::COData16) - obj->data = rpdo_mapped[idx][subidx].Read(); - else if(obj->type == CODataTypes::COData32) - obj->data = rpdo_mapped[idx][subidx].Read(); - break; - } - } - - if(idx == 0x606C && subidx == 0x0) - { - speed = rpdo_mapped[idx][subidx].Read(); - } - - if(idx == 0x6064 && subidx == 0x0) - { - position = rpdo_mapped[idx][subidx].Read(); - } - } - - /** - * @brief Set the remote obj - * - * Set the data of the remote object. This function will write the - * data passed to it to the local cache and the remote dictionary. - * - * @tparam T Datatype of the object - * @param [in] obj Shared pointer to the object - * @param [in] data Data to be written - */ - template - void set_remote_obj(std::shared_ptr obj, T data) - { - T data_ = data; - std::memcpy(&(obj->data), &data_, sizeof(T)); - - COData d = {obj->index, obj->subindex, obj->data, obj->type}; - if (!obj->tpdo_mapped) - { - - auto f = this->async_sdo_write(d); - f.wait(); - } - else - { - this->tpdo_mapped[obj->index][obj->subindex] = data; - this->tpdo_mapped[obj->index][obj->subindex].WriteEvent(); - } - } - /** - * @brief Get the remote obj - * - * Gets the data stored in the remote object. If the object is - * PDO mapped, the cached data is returned, else the data is fetched - * via SDO request. - * - * @tparam T Datatype of the object - * @param [in] obj Pointer to object - * @return T Data that was read - */ - template - T get_remote_obj(std::shared_ptr obj) - { - if (!obj->rpdo_mapped) - { - COData d = {obj->index, obj->subindex, 0U, obj->type}; - auto f = this->async_sdo_read(d); - f.wait(); - try - { - obj->data = f.get().data_; - } - catch (std::exception &e) - { - obj->valid = false; - } - } - T data; - std::memcpy(&data, &(obj->data), sizeof(T)); - return data; - } - - template - T get_remote_obj_cached(std::shared_ptr obj) - { - T data; - std::memcpy(&data, &(obj->data), sizeof(T)); - return data; - } - - template - void set_remote_obj_cached(std::shared_ptr obj, const T data) - { - T data_ = data; - std::memcpy(&(obj->data), &data_, sizeof(T)); - } - - /** - * @brief Validate objects in dictionary - * - * Validates whether objects exist and whether they are pdo mapped. - * - */ - void validate_objs() - { - for (auto it = objs.begin(); it != objs.end(); ++it) - { - std::shared_ptr obj = *it; - - try - { - switch (obj->type) - { - case CODataTypes::COData8: - obj->rpdo_mapped = this->tpdo_mapped[obj->index][obj->subindex].Read(); - break; - case CODataTypes::COData16: - obj->rpdo_mapped = this->tpdo_mapped[obj->index][obj->subindex].Read(); - break; - case CODataTypes::COData32: - obj->rpdo_mapped = this->tpdo_mapped[obj->index][obj->subindex].Read(); - break; - default: - throw lely::canopen::SdoError( - this->get_id(), - obj->index, - obj->subindex, - std::make_error_code(std::errc::function_not_supported), - "Unkown used, type must be 8, 16 or 32."); - break; - } - obj->tpdo_mapped = true; - } - catch (lely::canopen::SdoError &e) - { - obj->tpdo_mapped = false; - } - - try - { - switch (obj->type) - { - case CODataTypes::COData8: - obj->rpdo_mapped = this->rpdo_mapped[obj->index][obj->subindex].Read(); - break; - case CODataTypes::COData16: - obj->rpdo_mapped = this->rpdo_mapped[obj->index][obj->subindex].Read(); - break; - case CODataTypes::COData32: - obj->rpdo_mapped = this->rpdo_mapped[obj->index][obj->subindex].Read(); - break; - default: - throw lely::canopen::SdoError( - this->get_id(), - obj->index, - obj->subindex, - std::make_error_code(std::errc::function_not_supported), - "Unkown used, type must be 8, 16 or 32."); - break; - } - obj->rpdo_mapped = true; - } - catch (lely::canopen::SdoError &e) - { - obj->rpdo_mapped = false; - } - - switch (obj->type) - { - case CODataTypes::COData8: - obj->data = get_remote_obj(obj); - break; - case CODataTypes::COData16: - obj->data = get_remote_obj(obj); - break; - case CODataTypes::COData32: - obj->data = get_remote_obj(obj); - break; - default: - break; - } - std::cout << "Initialised object :" - << this->get_id() << " " - << std::hex << obj->index << " " - << std::dec << obj->subindex << " " - << obj->data << " " - << "RPDO: " << (obj->rpdo_mapped ? "yes" : "no") << " " - << "TPDO: " << (obj->tpdo_mapped ? "yes" : "no") << " " - << std::endl; - } - } - - /** - * @brief Get the speed object - * - * @return double - */ - double get_speed() - { - return speed; - } - - /** - * @brief Get the position object - * - * @return double - */ - double get_position() - { - return position; - } - - MCDeviceDriver(ev_exec_t *exec, canopen::AsyncMaster &master, uint8_t id) - : LelyBridge(exec, master, id) - { - sync = true; - } - }; - -} -#endif diff --git a/canopen_402_driver/include/canopen_402_driver/mode.hpp b/canopen_402_driver/include/canopen_402_driver/mode.hpp new file mode 100644 index 00000000..e7ccc032 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/mode.hpp @@ -0,0 +1,31 @@ +#ifndef CANOPEN_402_DRIVER_MODE_HPP +#define CANOPEN_402_DRIVER_MODE_HPP + +#include +#include +#include "command.hpp" +#include "state.hpp" +#include "word_accessor.hpp" + +namespace ros2_canopen +{ +class Mode +{ +public: + const uint16_t mode_id_; + Mode(uint16_t id) : mode_id_(id) {} + typedef WordAccessor< + (1 << Command402::CW_Operation_mode_specific0) | + (1 << Command402::CW_Operation_mode_specific1) | + (1 << Command402::CW_Operation_mode_specific2) | (1 << Command402::CW_Operation_mode_specific3)> + OpModeAccesser; + virtual bool start() = 0; + virtual bool read(const uint16_t & sw) = 0; + virtual bool write(OpModeAccesser & cw) = 0; + virtual bool setTarget(const double & val) { return false; } + virtual ~Mode() {} +}; +typedef std::shared_ptr ModeSharedPtr; +} // namespace ros2_canopen + +#endif // CANOPEN_402_DRIVER_MODE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp b/canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp new file mode 100644 index 00000000..216b091b --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp @@ -0,0 +1,41 @@ +#ifndef MODE_FORWARD_HELPER_HPP +#define MODE_FORWARD_HELPER_HPP + +#include +#include +#include "canopen_base_driver/lely_driver_bridge.hpp" +#include "mode_target_helper.hpp" + +namespace ros2_canopen +{ + +template +class ModeForwardHelper : public ModeTargetHelper +{ + std::shared_ptr driver; + +public: + ModeForwardHelper(std::shared_ptr driver) : ModeTargetHelper(ID) + { + this->driver = driver; + } + virtual bool read(const uint16_t & sw) { return true; } + virtual bool write(Mode::OpModeAccesser & cw) + { + if (this->hasTarget()) + { + cw = cw.get() | CW_MASK; + + driver->universal_set_value(OBJ, SUB, this->getTarget()); + return true; + } + else + { + cw = cw.get() & ~CW_MASK; + return false; + } + } +}; +} // namespace ros2_canopen + +#endif // MODE_FORWARD_HELPER_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp b/canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp new file mode 100644 index 00000000..1e08057f --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp @@ -0,0 +1,74 @@ +#ifndef MODE_TARGET_HELPER_HPP +#define MODE_TARGET_HELPER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "canopen_402_driver/mode.hpp" + +namespace ros2_canopen +{ + +template +class ModeTargetHelper : public Mode +{ + T target_; + std::atomic has_target_; + +public: + ModeTargetHelper(uint16_t mode) : Mode(mode) {} + bool hasTarget() { return has_target_; } + T getTarget() { return target_; } + virtual bool setTarget(const double & val) + { + if (std::isnan(val)) + { + // std::cout << "canopen_402 target command is not a number" << std::endl; + RCLCPP_DEBUG(rclcpp::get_logger("canopen_402_target"), "Target command is not a number"); + return false; + } + + using boost::numeric_cast; + using boost::numeric::negative_overflow; + using boost::numeric::positive_overflow; + + try + { + target_ = numeric_cast(val); + } + catch (negative_overflow &) + { + std::cout << "canopen_402 Command " << val + << " does not fit into target, clamping to min limit" << std::endl; + target_ = std::numeric_limits::min(); + } + catch (positive_overflow &) + { + std::cout << "canopen_402 Command " << val + << " does not fit into target, clamping to max limit" << std::endl; + target_ = std::numeric_limits::max(); + } + catch (...) + { + std::cout << "canopen_402 Was not able to cast command " << val << std::endl; + return false; + } + + has_target_ = true; + return true; + } + virtual bool start() + { + has_target_ = false; + return true; + } +}; +} // namespace ros2_canopen + +#endif // MODE_TARGET_HELPER_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/motor.hpp b/canopen_402_driver/include/canopen_402_driver/motor.hpp index 9e44e80f..871a2794 100644 --- a/canopen_402_driver/include/canopen_402_driver/motor.hpp +++ b/canopen_402_driver/include/canopen_402_driver/motor.hpp @@ -1,502 +1,197 @@ #ifndef MOTOR_HPP #define MOTOR_HPP -#include -#include -#include +#include #include +#include +#include #include -#include -#include #include -#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" -#include "canopen_402_driver/mc_device_driver.hpp" -using namespace ros2_canopen; -namespace canopen_402 -{ - class State402 - { - public: - enum StatusWord - { - SW_Ready_To_Switch_On = 0, - SW_Switched_On = 1, - SW_Operation_enabled = 2, - SW_Fault = 3, - SW_Voltage_enabled = 4, - SW_Quick_stop = 5, - SW_Switch_on_disabled = 6, - SW_Warning = 7, - SW_Manufacturer_specific0 = 8, - SW_Remote = 9, - SW_Target_reached = 10, - SW_Internal_limit = 11, - SW_Operation_mode_specific0 = 12, - SW_Operation_mode_specific1 = 13, - SW_Manufacturer_specific1 = 14, - SW_Manufacturer_specific2 = 15 - }; - enum InternalState - { - Unknown = 0, - Start = 0, - Not_Ready_To_Switch_On = 1, - Switch_On_Disabled = 2, - Ready_To_Switch_On = 3, - Switched_On = 4, - Operation_Enable = 5, - Quick_Stop_Active = 6, - Fault_Reaction_Active = 7, - Fault = 8, - }; - InternalState getState(); - InternalState read(uint16_t sw); - bool waitForNewState(const std::chrono::steady_clock::time_point &abstime, InternalState &state); - State402() : state_(Unknown) {} - - private: - std::condition_variable cond_; - std::mutex mutex_; - InternalState state_; - }; - - class Command402 - { - struct Op - { - uint16_t to_set_; - uint16_t to_reset_; - Op(uint16_t to_set, uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {} - void operator()(uint16_t &val) const - { - val = (val & ~to_reset_) | to_set_; - } - }; - class TransitionTable - { - boost::container::flat_map, Op> transitions_; - void add(const State402::InternalState &from, const State402::InternalState &to, Op op) - { - transitions_.insert(std::make_pair(std::make_pair(from, to), op)); - } - - public: - TransitionTable(); - const Op &get(const State402::InternalState &from, const State402::InternalState &to) const - { - return transitions_.at(std::make_pair(from, to)); - } - }; - static const TransitionTable transitions_; - static State402::InternalState nextStateForEnabling(State402::InternalState state); - Command402(); - - public: - enum ControlWord - { - CW_Switch_On = 0, - CW_Enable_Voltage = 1, - CW_Quick_Stop = 2, - CW_Enable_Operation = 3, - CW_Operation_mode_specific0 = 4, - CW_Operation_mode_specific1 = 5, - CW_Operation_mode_specific2 = 6, - CW_Fault_Reset = 7, - CW_Halt = 8, - CW_Operation_mode_specific3 = 9, - // CW_Reserved1=10, - CW_Manufacturer_specific0 = 11, - CW_Manufacturer_specific1 = 12, - CW_Manufacturer_specific2 = 13, - CW_Manufacturer_specific3 = 14, - CW_Manufacturer_specific4 = 15, - }; - static bool setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next); - }; - - template - class WordAccessor - { - uint16_t &word_; - - public: - WordAccessor(uint16_t &word) : word_(word) {} - bool set(uint8_t bit) - { - uint16_t val = MASK & (1 << bit); - word_ |= val; - return val; - } - bool reset(uint8_t bit) - { - uint16_t val = MASK & (1 << bit); - word_ &= ~val; - return val; - } - bool get(uint8_t bit) const { return word_ & (1 << bit); } - uint16_t get() const { return word_ & MASK; } - WordAccessor &operator=(const uint16_t &val) - { - word_ = (word_ & ~MASK) | (val & MASK); - return *this; - } - }; - - class Mode - { - public: - const uint16_t mode_id_; - Mode(uint16_t id) : mode_id_(id) {} - typedef WordAccessor<(1 << Command402::CW_Operation_mode_specific0) | (1 << Command402::CW_Operation_mode_specific1) | (1 << Command402::CW_Operation_mode_specific2) | (1 << Command402::CW_Operation_mode_specific3)> OpModeAccesser; - virtual bool start() = 0; - virtual bool read(const uint16_t &sw) = 0; - virtual bool write(OpModeAccesser &cw) = 0; - virtual bool setTarget(const double &val) { return false; } - virtual ~Mode() {} - }; - typedef std::shared_ptr ModeSharedPtr; - - template - class ModeTargetHelper : public Mode - { - T target_; - std::atomic has_target_; - - public: - ModeTargetHelper(uint16_t mode) : Mode(mode) {} - bool hasTarget() { return has_target_; } - T getTarget() { return target_; } - virtual bool setTarget(const double &val) - { - if (std::isnan(val)) - { - std::cout << "canopen_402 target command is not a number" << std::endl; - return false; - } - - using boost::numeric_cast; - using boost::numeric::negative_overflow; - using boost::numeric::positive_overflow; - - try - { - target_ = numeric_cast(val); - } - catch (negative_overflow &) - { - std::cout << "canopen_402 Command " << val << " does not fit into target, clamping to min limit" << std::endl; - target_ = std::numeric_limits::min(); - } - catch (positive_overflow &) - { - std::cout << "canopen_402 Command " << val << " does not fit into target, clamping to max limit" << std::endl; - target_ = std::numeric_limits::max(); - } - catch (...) - { - std::cout << "canopen_402 Was not able to cast command " << val << std::endl; - return false; - } - - has_target_ = true; - return true; - } - virtual bool start() - { - has_target_ = false; - return true; - } - }; - - template - class ModeForwardHelper : public ModeTargetHelper - { - std::shared_ptr driver; - std::shared_ptr obj; - - public: - ModeForwardHelper(std::shared_ptr driver) : ModeTargetHelper(ID) - { - this->obj = driver->create_remote_obj(OBJ, SUB, TPY); - this->driver = driver; - } - virtual bool read(const uint16_t &sw) { return true; } - virtual bool write(Mode::OpModeAccesser &cw) - { - if (this->hasTarget()) - { - cw = cw.get() | CW_MASK; - - driver->set_remote_obj(obj, this->getTarget()); - return true; - } - else - { - cw = cw.get() & ~CW_MASK; - return false; - } - } - }; - - typedef ModeForwardHelper ProfiledVelocityMode; - typedef ModeForwardHelper ProfiledTorqueMode; - typedef ModeForwardHelper CyclicSynchronousPositionMode; - typedef ModeForwardHelper CyclicSynchronousVelocityMode; - typedef ModeForwardHelper CyclicSynchronousTorqueMode; - typedef ModeForwardHelper VelocityMode; - typedef ModeForwardHelper InterpolatedPositionMode; +#include "canopen_402_driver/default_homing_mode.hpp" +#include "canopen_402_driver/mode_forward_helper.hpp" +#include "canopen_402_driver/profiled_position_mode.hpp" +#include "canopen_base_driver/lely_driver_bridge.hpp" - class ProfiledPositionMode : public ModeTargetHelper - { - const uint16_t index = 0x607A; - std::shared_ptr driver; - std::shared_ptr obj; - - double last_target_; - uint16_t sw_; - - public: - enum SW_masks - { - MASK_Reached = (1 << State402::SW_Target_reached), - MASK_Acknowledged = (1 << State402::SW_Operation_mode_specific0), - MASK_Error = (1 << State402::SW_Operation_mode_specific1), - }; - enum CW_bits - { - CW_NewPoint = Command402::CW_Operation_mode_specific0, - CW_Immediate = Command402::CW_Operation_mode_specific1, - CW_Blending = Command402::CW_Operation_mode_specific3, - }; - ProfiledPositionMode(std::shared_ptr driver) - : ModeTargetHelper(MotorBase::Profiled_Position) - { - this->driver = driver; - obj = driver->create_remote_obj(index, 0U, CODataTypes::COData32); - } - - virtual bool start() - { - sw_ = 0; - last_target_ = std::numeric_limits::quiet_NaN(); - return ModeTargetHelper::start(); - } - virtual bool read(const uint16_t &sw) - { - sw_ = sw; - return (sw & MASK_Error) == 0; - } - virtual bool write(OpModeAccesser &cw) - { - cw.set(CW_Immediate); - if (hasTarget()) - { - int32_t target = getTarget(); - if ((sw_ & MASK_Acknowledged) == 0 && target != last_target_) - { - if (cw.get(CW_NewPoint)) - { - cw.reset(CW_NewPoint); // reset if needed - } - else - { - driver->set_remote_obj(obj, target); - cw.set(CW_NewPoint); - last_target_ = target; - } - } - else if (sw_ & MASK_Acknowledged) - { - cw.reset(CW_NewPoint); - } - return true; - } - return false; - } - }; - - class HomingMode : public Mode - { - protected: - enum SW_bits - { - SW_Attained = State402::SW_Operation_mode_specific0, - SW_Error = State402::SW_Operation_mode_specific1, - }; - enum CW_bits - { - CW_StartHoming = Command402::CW_Operation_mode_specific0, - }; - - public: - HomingMode() : Mode(MotorBase::Homing) {} - virtual bool executeHoming() = 0; - }; - - class DefaultHomingMode : public HomingMode - { - const uint16_t index = 0x6098; - std::shared_ptr driver; - std::shared_ptr obj; - - std::atomic execute_; - - std::mutex mutex_; - std::condition_variable cond_; - uint16_t status_; - - enum SW_masks - { - MASK_Reached = (1 << State402::SW_Target_reached), - MASK_Attained = (1 << SW_Attained), - MASK_Error = (1 << SW_Error), - }; - bool error(const std::string &msg) - { - execute_ = false; - std::cout << msg << std::endl; - return false; - } - - public: - DefaultHomingMode(std::shared_ptr driver) - { - this->driver = driver; - obj = driver->create_remote_obj(index, 0U, CODataTypes::COData32); - } - virtual bool start(); - virtual bool read(const uint16_t &sw); - virtual bool write(OpModeAccesser &cw); - - virtual bool executeHoming(); - }; - - class Motor402 : public MotorBase - { - public: - Motor402(std::shared_ptr driver) : - MotorBase(), - switching_state_(State402::Operation_Enable), - monitor_mode_(true), - state_switch_timeout_(5) - { - this->driver = driver; - status_word_entry_ = driver->create_remote_obj(status_word_entry_index, 0U, CODataTypes::COData16); - control_word_entry_ = driver->create_remote_obj(control_word_entry_index, 0U, CODataTypes::COData16); - op_mode_display_ = driver->create_remote_obj(op_mode_display_index, 0U, CODataTypes::COData8); - op_mode_ = driver->create_remote_obj(op_mode_index, 0U, CODataTypes::COData8); - supported_drive_modes_ = driver->create_remote_obj(supported_drive_modes_index, 0U, CODataTypes::COData32); - } - - virtual bool setTarget(double val); - virtual bool enterModeAndWait(uint16_t mode); - virtual bool isModeSupported(uint16_t mode); - virtual uint16_t getMode(); - bool readState(); - void handleDiag(); - /** - * @brief Initialise the drive - * - * This function intialises the drive. This means, it first - * attempts to bring the device to operational state (CIA402) - * and then executes the chosen homing method. - * - */ - void handleInit(); - void handleRead(); - void handleWrite(); - /** - * @brief Shutdowns the drive - * - * This function shuts down the drive by bringing it into - * SwitchOn disbled state. - * - */ - void handleShutdown(); - /** - * @brief Executes a quickstop - * - * The function executes a quickstop. - * - */ - void handleHalt(); - - /** - * @brief Recovers the device from fault - * - * This function tries to reset faults and - * put the device back to operational state. - * - */ - void handleRecover(); - template - bool registerMode(uint16_t mode, Args &&...args) - { - return mode_allocators_.insert(std::make_pair(mode, [args..., mode, this]() - { - if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...))); })) - .second; - } - - virtual void registerDefaultModes() - { - registerMode(MotorBase::Profiled_Position, driver); - registerMode(MotorBase::Velocity, driver); - registerMode(MotorBase::Profiled_Velocity, driver); - registerMode(MotorBase::Profiled_Torque, driver); - registerMode(MotorBase::Homing, driver); - registerMode(MotorBase::Interpolated_Position, driver); - registerMode(MotorBase::Cyclic_Synchronous_Position, driver); - registerMode(MotorBase::Cyclic_Synchronous_Velocity, driver); - registerMode(MotorBase::Cyclic_Synchronous_Torque, driver); - } - - private: - virtual bool isModeSupportedByDevice(uint16_t mode); - void registerMode(uint16_t id, const ModeSharedPtr &m); - - ModeSharedPtr allocMode(uint16_t mode); - - bool switchMode(uint16_t mode); - bool switchState(const State402::InternalState &target); - - std::atomic status_word_; - uint16_t control_word_; - std::mutex cw_mutex_; - std::atomic start_fault_reset_; - std::atomic target_state_; - - State402 state_handler_; - - std::mutex map_mutex_; - std::unordered_map modes_; - typedef std::function AllocFuncType; - std::unordered_map mode_allocators_; - - ModeSharedPtr selected_mode_; - uint16_t mode_id_; - std::condition_variable mode_cond_; - std::mutex mode_mutex_; - const State402::InternalState switching_state_; - const bool monitor_mode_; - const std::chrono::seconds state_switch_timeout_; - - std::shared_ptr driver; - std::shared_ptr status_word_entry_; - std::shared_ptr control_word_entry_; - std::shared_ptr op_mode_display_; - std::shared_ptr op_mode_; - std::shared_ptr supported_drive_modes_; - const uint16_t status_word_entry_index = 0x6041; - const uint16_t control_word_entry_index = 0x6040; - const uint16_t op_mode_display_index = 0x6061; - const uint16_t op_mode_index = 0x6060; - const uint16_t supported_drive_modes_index = 0x6502; - }; - -} +namespace ros2_canopen +{ +typedef ModeForwardHelper ProfiledVelocityMode; +typedef ModeForwardHelper ProfiledTorqueMode; +typedef ModeForwardHelper + CyclicSynchronousPositionMode; +typedef ModeForwardHelper + CyclicSynchronousVelocityMode; +typedef ModeForwardHelper + CyclicSynchronousTorqueMode; +typedef ModeForwardHelper< + MotorBase::Velocity, int16_t, 0x6042, 0, + (1 << Command402::CW_Operation_mode_specific0) | (1 << Command402::CW_Operation_mode_specific1) | + (1 << Command402::CW_Operation_mode_specific2)> + VelocityMode; +typedef ModeForwardHelper< + MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01, + (1 << Command402::CW_Operation_mode_specific0)> + InterpolatedPositionMode; + +class Motor402 : public MotorBase +{ +public: + Motor402( + std::shared_ptr driver, ros2_canopen::State402::InternalState switching_state) + : MotorBase(), switching_state_(switching_state), monitor_mode_(true), state_switch_timeout_(5) + { + this->driver = driver; + } + + virtual bool setTarget(double val); + virtual bool enterModeAndWait(uint16_t mode); + virtual bool isModeSupported(uint16_t mode); + virtual uint16_t getMode(); + bool readState(); + void handleDiag(); + /** + * @brief Initialise the drive + * + * This function initialises the drive. This means, it first + * attempts to bring the device to operational state (CIA402) + * and then executes the chosen homing method. + * + */ + bool handleInit(); + /** + * @brief Read objects of the drive + * + * This function should be called regularly. It reads the status word + * from the device and translates it into the devices state. + * + */ + void handleRead(); + /** + * @brief Writes objects to the drive + * + * This function should be called regularly. It writes the new command + * word to the drive + * + */ + void handleWrite(); + /** + * @brief Shutdowns the drive + * + * This function shuts down the drive by bringing it into + * SwitchOn disabled state. + * + */ + bool handleShutdown(); + /** + * @brief Executes a quickstop + * + * The function executes a quickstop. + * + */ + bool handleHalt(); + + /** + * @brief Recovers the device from fault + * + * This function tries to reset faults and + * put the device back to operational state. + * + */ + bool handleRecover(); + + /** + * @brief Register a new operation mode for the drive + * + * This function will register an operation mode for the drive. + * It will check if the mode is supported by the drive by reading + * 0x6508 object. + * + * @tparam T + * @tparam Args + * @param mode + * @param args + * @return true + * @return false + */ + template + bool registerMode(uint16_t mode, Args &&... args) + { + return mode_allocators_ + .insert(std::make_pair( + mode, + [args..., mode, this]() + { + if (isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...))); + })) + .second; + } + + /** + * @brief Tries to register the standard operation modes defined in cia402 + * + */ + virtual void registerDefaultModes() + { + registerMode(MotorBase::Profiled_Position, driver); + registerMode(MotorBase::Velocity, driver); + registerMode(MotorBase::Profiled_Velocity, driver); + registerMode(MotorBase::Profiled_Torque, driver); + registerMode(MotorBase::Homing, driver); + registerMode(MotorBase::Interpolated_Position, driver); + registerMode(MotorBase::Cyclic_Synchronous_Position, driver); + registerMode(MotorBase::Cyclic_Synchronous_Velocity, driver); + registerMode(MotorBase::Cyclic_Synchronous_Torque, driver); + } + + double get_speed() const { return (double)this->driver->universal_get_value(0x606C, 0); } + double get_position() const + { + return (double)this->driver->universal_get_value(0x6064, 0); + } + +private: + virtual bool isModeSupportedByDevice(uint16_t mode); + void registerMode(uint16_t id, const ModeSharedPtr & m); + + ModeSharedPtr allocMode(uint16_t mode); + + bool switchMode(uint16_t mode); + bool switchState(const State402::InternalState & target); + + std::atomic status_word_; + uint16_t control_word_; + std::mutex cw_mutex_; + std::atomic start_fault_reset_; + std::atomic target_state_; + + State402 state_handler_; + + std::mutex map_mutex_; + std::unordered_map modes_; + typedef std::function AllocFuncType; + std::unordered_map mode_allocators_; + + ModeSharedPtr selected_mode_; + uint16_t mode_id_; + std::condition_variable mode_cond_; + std::mutex mode_mutex_; + const State402::InternalState switching_state_; + const bool monitor_mode_; + const std::chrono::seconds state_switch_timeout_; + + std::shared_ptr driver; + const uint16_t status_word_entry_index = 0x6041; + const uint16_t control_word_entry_index = 0x6040; + const uint16_t op_mode_display_index = 0x6061; + const uint16_t op_mode_index = 0x6060; + const uint16_t supported_drive_modes_index = 0x6502; +}; + +} // namespace ros2_canopen #endif diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp new file mode 100644 index 00000000..823cd7ad --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp @@ -0,0 +1,332 @@ +#ifndef NODE_CANOPEN_402_DRIVER +#define NODE_CANOPEN_402_DRIVER + +#include "canopen_402_driver/motor.hpp" +#include "canopen_base_driver/lely_driver_bridge.hpp" +#include "canopen_interfaces/srv/co_target_double.hpp" +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +namespace ros2_canopen +{ +namespace node_interfaces +{ + +template +class NodeCanopen402Driver : public NodeCanopenProxyDriver +{ + static_assert( + std::is_base_of::value || + std::is_base_of::value, + "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); + +protected: + std::shared_ptr motor_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Service::SharedPtr handle_init_service; + rclcpp::Service::SharedPtr handle_halt_service; + rclcpp::Service::SharedPtr handle_recover_service; + rclcpp::Service::SharedPtr handle_set_mode_position_service; + rclcpp::Service::SharedPtr handle_set_mode_torque_service; + rclcpp::Service::SharedPtr handle_set_mode_velocity_service; + rclcpp::Service::SharedPtr handle_set_mode_cyclic_velocity_service; + rclcpp::Service::SharedPtr handle_set_mode_cyclic_position_service; + rclcpp::Service::SharedPtr handle_set_mode_interpolated_position_service; + rclcpp::Service::SharedPtr handle_set_target_service; + rclcpp::Publisher::SharedPtr publish_joint_state; + double scale_pos_to_dev_; + double scale_pos_from_dev_; + double scale_vel_to_dev_; + double scale_vel_from_dev_; + ros2_canopen::State402::InternalState switching_state_; + + void publish(); + virtual void poll_timer_callback() override; + +public: + NodeCanopen402Driver(NODETYPE * node); + + virtual void init(bool called_from_base) override; + virtual void configure(bool called_from_base) override; + virtual void activate(bool called_from_base) override; + virtual void deactivate(bool called_from_base) override; + virtual void add_to_master() override; + + virtual double get_speed() { return motor_->get_speed() * scale_vel_from_dev_; } + + virtual double get_position() { return motor_->get_position() * scale_pos_from_dev_; } + + virtual uint16_t get_mode() { return motor_->getMode(); } + + /** + * @brief Service Callback to initialise device + * + * Calls Motor402::handleInit function. Brings motor to enabled + * state and homes it. + * + * @param [in] request + * @param [out] response + */ + void handle_init( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to initialise device + * + * Calls Motor402::handleInit function. Brings motor to enabled + * state and homes it. + * + * @param [in] void + * + * @return bool + * Indicates initialisation procedure result + */ + bool init_motor(); + + /** + * @brief Service Callback to recover device + * + * Calls Motor402::handleRecover function. Resets faults and brings + * motor to enabled state. + * + * @param [in] request + * @param [out] response + */ + void handle_recover( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to recover device + * + * Calls Motor402::handleRecover function. Resets faults and brings + * motor to enabled state. + * + * @param [in] void + * + * @return bool + */ + bool recover_motor(); + + /** + * @brief Service Callback to halt device + * + * Calls Motor402::handleHalt function. Calls Quickstop. Resulting + * Motor state depends on devices configuration specifically object + * 0x605A. + * + * @param [in] request + * @param [out] response + */ + void handle_halt( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to halt device + * + * Calls Motor402::handleHalt function. Calls Quickstop. Resulting + * Motor state depends on devices configuration specifically object + * 0x605A. + * + * @param [in] void + * + * @return bool + */ + bool halt_motor(); + + /** + * @brief Service Callback to set profiled position mode + * + * Calls Motor402::enterModeAndWait with Profiled Position Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Profiled Position Mode. + * + * @param [in] request + * @param [out] response + */ + void handle_set_mode_position( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + bool set_operation_mode(uint16_t mode); + + /** + * @brief Method to set profiled position mode + * + * Calls Motor402::enterModeAndWait with Profiled Position Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Profiled Position Mode. + * + * @param [in] void + * + * @return bool + */ + bool set_mode_position(); + + /** + * @brief Service Callback to set profiled velocity mode + * + * Calls Motor402::enterModeAndWait with Profiled Velocity Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Profiled Velocity Mode. + * + * @param [in] request + * @param [out] response + */ + void handle_set_mode_velocity( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to set profiled velocity mode + * + * Calls Motor402::enterModeAndWait with Profiled Velocity Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Profiled Velocity Mode. + * + * @param [in] void + * + * @return bool + */ + bool set_mode_velocity(); + + /** + * @brief Service Callback to set cyclic position mode + * + * Calls Motor402::enterModeAndWait with Cyclic Position Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Cyclic Position Mode. + * + * @param [in] request + * @param [out] response + */ + void handle_set_mode_cyclic_position( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Service Callback to set interpolated position mode + * + * Calls Motor402::enterModeAndWait with Interpolated Position Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Interpolated Position Mode. This only supports linear mode. + * + * @param [in] request + * @param [out] response + */ + void handle_set_mode_interpolated_position( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to set interpolated position mode + * + * Calls Motor402::enterModeAndWait with Interpolated Position Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Interpolated Position Mode. This only supports linear mode. + * + * @param [in] void + * @param [out] bool + */ + bool set_mode_interpolated_position(); + + /** + * @brief Method to set cyclic position mode + * + * Calls Motor402::enterModeAndWait with Cyclic Position Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Cyclic Position Mode. + * + * @param [in] void + * + * @return bool + */ + bool set_mode_cyclic_position(); + + /** + * @brief Service Callback to set cyclic velocity mode + * + * Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Cyclic Velocity Mode. + * + * @param [in] request + * @param [out] response + */ + void handle_set_mode_cyclic_velocity( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to set cyclic velocity mode + * + * Calls Motor402::enterModeAndWait with Cyclic Velocity Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Cyclic Velocity Mode. + * + * @param [in] void + * + * @return bool + */ + bool set_mode_cyclic_velocity(); + + /** + * @brief Service Callback to set profiled torque mode + * + * Calls Motor402::enterModeAndWait with Profiled Torque Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Profiled Torque Mode. + * + * @param [in] request + * @param [out] response + */ + void handle_set_mode_torque( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to set profiled torque mode + * + * Calls Motor402::enterModeAndWait with Profiled Torque Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Profiled Torque Mode. + * + * @param [in] void + * + * @return bool + */ + bool set_mode_torque(); + + /** + * @brief Service Callback to set target + * + * Calls Motor402::setTarget and sets the requested target value. Note + * that the resulting movement is dependent on the Operation Mode and the + * drives state. + * + * @param [in] request + * @param [out] response + */ + void handle_set_target( + const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, + canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response); + + /** + * @brief Method to set target + * + * Calls Motor402::setTarget and sets the requested target value. Note + * that the resulting movement is dependent on the Operation Mode and the + * drives state. + * + * @param [in] double target value + * + * @return bool + */ + bool set_target(double target); +}; +} // namespace node_interfaces +} // namespace ros2_canopen + +#endif diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp new file mode 100644 index 00000000..120e4a84 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -0,0 +1,605 @@ +#ifndef NODE_CANOPEN_402_DRIVER_IMPL_HPP_ +#define NODE_CANOPEN_402_DRIVER_IMPL_HPP_ + +#include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" +#include "canopen_core/driver_error.hpp" + +#include + +using namespace ros2_canopen::node_interfaces; +using namespace std::placeholders; + +template +NodeCanopen402Driver::NodeCanopen402Driver(NODETYPE * node) +: ros2_canopen::node_interfaces::NodeCanopenProxyDriver(node) +{ +} + +template +void NodeCanopen402Driver::init(bool called_from_base) +{ + RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented."); +} + +template <> +void NodeCanopen402Driver::init(bool called_from_base) +{ + NodeCanopenProxyDriver::init(false); + publish_joint_state = + this->node_->create_publisher("~/joint_states", 1); + handle_init_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/init").c_str(), + std::bind(&NodeCanopen402Driver::handle_init, this, _1, _2)); + + handle_halt_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/halt").c_str(), + std::bind(&NodeCanopen402Driver::handle_halt, this, _1, _2)); + + handle_recover_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/recover").c_str(), + std::bind(&NodeCanopen402Driver::handle_recover, this, _1, _2)); + + handle_set_mode_position_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/position_mode").c_str(), + std::bind(&NodeCanopen402Driver::handle_set_mode_position, this, _1, _2)); + + handle_set_mode_velocity_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/velocity_mode").c_str(), + std::bind(&NodeCanopen402Driver::handle_set_mode_velocity, this, _1, _2)); + + handle_set_mode_cyclic_velocity_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(), + std::bind(&NodeCanopen402Driver::handle_set_mode_cyclic_velocity, this, _1, _2)); + + handle_set_mode_cyclic_position_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(), + std::bind(&NodeCanopen402Driver::handle_set_mode_cyclic_position, this, _1, _2)); + + handle_set_mode_interpolated_position_service = + this->node_->create_service( + std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_interpolated_position, this, _1, _2)); + + handle_set_mode_torque_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/torque_mode").c_str(), + std::bind(&NodeCanopen402Driver::handle_set_mode_torque, this, _1, _2)); + + handle_set_target_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/target").c_str(), + std::bind(&NodeCanopen402Driver::handle_set_target, this, _1, _2)); +} + +template <> +void NodeCanopen402Driver::init(bool called_from_base) +{ + NodeCanopenProxyDriver::init(false); + publish_joint_state = + this->node_->create_publisher("~/joint_states", 10); + handle_init_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/init").c_str(), + std::bind(&NodeCanopen402Driver::handle_init, this, _1, _2)); + + handle_halt_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/halt").c_str(), + std::bind(&NodeCanopen402Driver::handle_halt, this, _1, _2)); + + handle_recover_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/recover").c_str(), + std::bind( + &NodeCanopen402Driver::handle_recover, this, _1, _2)); + + handle_set_mode_position_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/position_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_position, this, _1, + _2)); + + handle_set_mode_velocity_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/velocity_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_velocity, this, _1, + _2)); + + handle_set_mode_cyclic_velocity_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_cyclic_velocity, this, + _1, _2)); + + handle_set_mode_cyclic_position_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_cyclic_position, this, + _1, _2)); + + handle_set_mode_interpolated_position_service = this->node_->create_service< + std_srvs::srv::Trigger>( + std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_interpolated_position, + this, _1, _2)); + + handle_set_mode_torque_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/torque_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_torque, this, _1, + _2)); + + handle_set_target_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/target").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_target, this, _1, _2)); +} + +template <> +void NodeCanopen402Driver::configure(bool called_from_base) +{ + NodeCanopenProxyDriver::configure(false); + std::optional scale_pos_to_dev; + std::optional scale_pos_from_dev; + std::optional scale_vel_to_dev; + std::optional scale_vel_from_dev; + std::optional switching_state; + try + { + scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as()); + } + catch (...) + { + } + try + { + scale_pos_from_dev = std::optional(this->config_["scale_pos_from_dev"].as()); + } + catch (...) + { + } + try + { + scale_vel_to_dev = std::optional(this->config_["scale_vel_to_dev"].as()); + } + catch (...) + { + } + try + { + scale_vel_from_dev = std::optional(this->config_["scale_vel_from_dev"].as()); + } + catch (...) + { + } + try + { + switching_state = std::optional(this->config_["switching_state"].as()); + } + catch (...) + { + } + + // auto period = this->config_["scale_eff_to_dev"].as(); + // auto period = this->config_["scale_eff_from_dev"].as(); + scale_pos_to_dev_ = scale_pos_to_dev.value_or(1000.0); + scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001); + scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0); + scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001); + switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or( + (int)ros2_canopen::State402::InternalState::Operation_Enable); + RCLCPP_INFO( + this->node_->get_logger(), + "scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\n", + scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_); +} + +template <> +void NodeCanopen402Driver::configure(bool called_from_base) +{ + NodeCanopenProxyDriver::configure(false); + std::optional scale_pos_to_dev; + std::optional scale_pos_from_dev; + std::optional scale_vel_to_dev; + std::optional scale_vel_from_dev; + std::optional switching_state; + try + { + scale_pos_to_dev = std::optional(this->config_["scale_pos_to_dev"].as()); + } + catch (...) + { + } + try + { + scale_pos_from_dev = std::optional(this->config_["scale_pos_from_dev"].as()); + } + catch (...) + { + } + try + { + scale_vel_to_dev = std::optional(this->config_["scale_vel_to_dev"].as()); + } + catch (...) + { + } + try + { + scale_vel_from_dev = std::optional(this->config_["scale_vel_from_dev"].as()); + } + catch (...) + { + } + try + { + switching_state = std::optional(this->config_["switching_state"].as()); + } + catch (...) + { + } + + // auto period = this->config_["scale_eff_to_dev"].as(); + // auto period = this->config_["scale_eff_from_dev"].as(); + scale_pos_to_dev_ = scale_pos_to_dev.value_or(1000.0); + scale_pos_from_dev_ = scale_pos_from_dev.value_or(0.001); + scale_vel_to_dev_ = scale_vel_to_dev.value_or(1000.0); + scale_vel_from_dev_ = scale_vel_from_dev.value_or(0.001); + switching_state_ = (ros2_canopen::State402::InternalState)switching_state.value_or( + (int)ros2_canopen::State402::InternalState::Operation_Enable); + RCLCPP_INFO( + this->node_->get_logger(), + "scale_pos_to_dev_ %f\nscale_pos_from_dev_ %f\nscale_vel_to_dev_ %f\nscale_vel_from_dev_ %f\n", + scale_pos_to_dev_, scale_pos_from_dev_, scale_vel_to_dev_, scale_vel_from_dev_); +} + +template +void NodeCanopen402Driver::activate(bool called_from_base) +{ + NodeCanopenProxyDriver::activate(false); + motor_->registerDefaultModes(); +} + +template +void NodeCanopen402Driver::deactivate(bool called_from_base) +{ + NodeCanopenProxyDriver::deactivate(false); + timer_->cancel(); +} + +template +void NodeCanopen402Driver::poll_timer_callback() +{ + NodeCanopenProxyDriver::poll_timer_callback(); + motor_->handleRead(); + motor_->handleWrite(); + publish(); +} + +template +void NodeCanopen402Driver::publish() +{ + sensor_msgs::msg::JointState js_msg; + js_msg.name.push_back(this->node_->get_name()); + js_msg.position.push_back(motor_->get_position() * scale_pos_from_dev_); + js_msg.velocity.push_back(motor_->get_speed() * scale_vel_from_dev_); + js_msg.effort.push_back(0.0); + publish_joint_state->publish(js_msg); +} + +template +void NodeCanopen402Driver::add_to_master() +{ + NodeCanopenProxyDriver::add_to_master(); + motor_ = std::make_shared(this->lely_driver_, switching_state_); +} + +template +void NodeCanopen402Driver::handle_init( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (this->activated_.load()) + { + bool temp = motor_->handleInit(); + response->success = temp; + } +} +template +void NodeCanopen402Driver::handle_recover( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (this->activated_.load()) + { + response->success = motor_->handleRecover(); + } +} +template +void NodeCanopen402Driver::handle_halt( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (this->activated_.load()) + { + response->success = motor_->handleHalt(); + } +} +template +void NodeCanopen402Driver::handle_set_mode_position( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = set_mode_position(); +} + +template +void NodeCanopen402Driver::handle_set_mode_velocity( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = set_mode_velocity(); +} + +template +void NodeCanopen402Driver::handle_set_mode_cyclic_position( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = set_mode_cyclic_position(); +} + +template +void NodeCanopen402Driver::handle_set_mode_interpolated_position( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = set_mode_interpolated_position(); +} + +template +void NodeCanopen402Driver::handle_set_mode_cyclic_velocity( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = set_mode_cyclic_velocity(); +} +template +void NodeCanopen402Driver::handle_set_mode_torque( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = set_mode_torque(); +} + +template +void NodeCanopen402Driver::handle_set_target( + const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, + canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response) +{ + if (this->activated_.load()) + { + auto mode = motor_->getMode(); + double target; + if ( + (mode == MotorBase::Profiled_Position) or (mode == MotorBase::Cyclic_Synchronous_Position) or + (mode == MotorBase::Interpolated_Position)) + { + target = request->target * scale_pos_to_dev_; + } + else if ( + (mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or + (mode == MotorBase::Cyclic_Synchronous_Velocity)) + { + target = request->target * scale_vel_to_dev_; + } + else + { + target = request->target; + } + + response->success = motor_->setTarget(target); + } +} + +template +bool NodeCanopen402Driver::init_motor() +{ + if (this->activated_.load()) + { + bool temp = motor_->handleInit(); + return temp; + } + else + { + RCLCPP_INFO(this->node_->get_logger(), "Initialisation failed."); + return false; + } +} + +template +bool NodeCanopen402Driver::recover_motor() +{ + if (this->activated_.load()) + { + return motor_->handleRecover(); + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::halt_motor() +{ + if (this->activated_.load()) + { + return motor_->handleHalt(); + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::set_operation_mode(uint16_t mode) +{ + if (this->activated_.load()) + { + return motor_->enterModeAndWait(mode); + } + return false; +} + +template +bool NodeCanopen402Driver::set_mode_position() +{ + if (this->activated_.load()) + { + if (motor_->getMode() != MotorBase::Profiled_Position) + { + return motor_->enterModeAndWait(MotorBase::Profiled_Position); + } + else + { + return false; + } + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::set_mode_interpolated_position() +{ + if (this->activated_.load()) + { + if (motor_->getMode() != MotorBase::Interpolated_Position) + { + return motor_->enterModeAndWait(MotorBase::Interpolated_Position); + } + else + { + return false; + } + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::set_mode_velocity() +{ + if (this->activated_.load()) + { + if (motor_->getMode() != MotorBase::Profiled_Velocity) + { + return motor_->enterModeAndWait(MotorBase::Profiled_Velocity); + } + else + { + return false; + } + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::set_mode_cyclic_position() +{ + if (this->activated_.load()) + { + if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Position) + { + return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Position); + } + else + { + return false; + } + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::set_mode_cyclic_velocity() +{ + if (this->activated_.load()) + { + if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Velocity) + { + return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Velocity); + } + else + { + return false; + } + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::set_mode_torque() +{ + if (this->activated_.load()) + { + if (motor_->getMode() != MotorBase::Profiled_Torque) + { + return motor_->enterModeAndWait(MotorBase::Profiled_Torque); + } + else + { + return false; + } + } + else + { + return false; + } +} + +template +bool NodeCanopen402Driver::set_target(double target) +{ + if (this->activated_.load()) + { + auto mode = motor_->getMode(); + double scaled_target; + if ( + (mode == MotorBase::Profiled_Position) or (mode == MotorBase::Cyclic_Synchronous_Position) or + (mode == MotorBase::Interpolated_Position)) + { + scaled_target = target * scale_pos_to_dev_; + } + else if ( + (mode == MotorBase::Velocity) or (mode == MotorBase::Profiled_Velocity) or + (mode == MotorBase::Cyclic_Synchronous_Velocity)) + { + scaled_target = target * scale_vel_to_dev_; + } + else + { + scaled_target = target; + } + // RCLCPP_INFO(this->node_->get_logger(), "Scaled target %f", scaled_target); + return motor_->setTarget(scaled_target); + } + else + { + return false; + } +} + +#endif diff --git a/canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp b/canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp new file mode 100644 index 00000000..ca56eaa8 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp @@ -0,0 +1,79 @@ +#ifndef PROFILED_POSITION_MODE_HPP +#define PROFILED_POSITION_MODE_HPP + +#include +#include +#include "canopen_base_driver/lely_driver_bridge.hpp" +#include "mode_target_helper.hpp" + +namespace ros2_canopen +{ +class ProfiledPositionMode : public ModeTargetHelper +{ + const uint16_t index = 0x607A; + std::shared_ptr driver; + + double last_target_; + uint16_t sw_; + +public: + enum SW_masks + { + MASK_Reached = (1 << State402::SW_Target_reached), + MASK_Acknowledged = (1 << State402::SW_Operation_mode_specific0), + MASK_Error = (1 << State402::SW_Operation_mode_specific1), + }; + enum CW_bits + { + CW_NewPoint = Command402::CW_Operation_mode_specific0, + CW_Immediate = Command402::CW_Operation_mode_specific1, + CW_Blending = Command402::CW_Operation_mode_specific3, + }; + ProfiledPositionMode(std::shared_ptr driver) + : ModeTargetHelper(MotorBase::Profiled_Position) + { + this->driver = driver; + } + + virtual bool start() + { + sw_ = 0; + last_target_ = std::numeric_limits::quiet_NaN(); + return ModeTargetHelper::start(); + } + virtual bool read(const uint16_t & sw) + { + sw_ = sw; + return (sw & MASK_Error) == 0; + } + virtual bool write(OpModeAccesser & cw) + { + cw.set(CW_Immediate); + if (hasTarget()) + { + int32_t target = getTarget(); + if ((sw_ & MASK_Acknowledged) == 0 && target != last_target_) + { + if (cw.get(CW_NewPoint)) + { + cw.reset(CW_NewPoint); // reset if needed + } + else + { + driver->universal_set_value(index, 0x0, target); + cw.set(CW_NewPoint); + last_target_ = target; + } + } + else if (sw_ & MASK_Acknowledged) + { + cw.reset(CW_NewPoint); + } + return true; + } + return false; + } +}; +} // namespace ros2_canopen + +#endif // PROFILED_POSITION_MODE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/state.hpp b/canopen_402_driver/include/canopen_402_driver/state.hpp new file mode 100644 index 00000000..4a146118 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/state.hpp @@ -0,0 +1,58 @@ +#ifndef CANOPEN_402_DRIVER_STATE_HPP +#define CANOPEN_402_DRIVER_STATE_HPP + +#include +#include +#include + +namespace ros2_canopen +{ +class State402 +{ +public: + enum StatusWord + { + SW_Ready_To_Switch_On = 0, + SW_Switched_On = 1, + SW_Operation_enabled = 2, + SW_Fault = 3, + SW_Voltage_enabled = 4, + SW_Quick_stop = 5, + SW_Switch_on_disabled = 6, + SW_Warning = 7, + SW_Manufacturer_specific0 = 8, + SW_Remote = 9, + SW_Target_reached = 10, + SW_Internal_limit = 11, + SW_Operation_mode_specific0 = 12, + SW_Operation_mode_specific1 = 13, + SW_Manufacturer_specific1 = 14, + SW_Manufacturer_specific2 = 15 + }; + enum InternalState + { + Unknown = 0, + Start = 0, + Not_Ready_To_Switch_On = 1, + Switch_On_Disabled = 2, + Ready_To_Switch_On = 3, + Switched_On = 4, + Operation_Enable = 5, + Quick_Stop_Active = 6, + Fault_Reaction_Active = 7, + Fault = 8, + }; + InternalState getState(); + InternalState read(uint16_t sw); + bool waitForNewState( + const std::chrono::steady_clock::time_point & abstime, InternalState & state); + State402() : state_(Unknown) {} + +private: + std::condition_variable cond_; + std::mutex mutex_; + InternalState state_; +}; +} // namespace ros2_canopen + +#endif // CANOPEN_402_DRIVER_STATE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/visibility_control.h b/canopen_402_driver/include/canopen_402_driver/visibility_control.h index aa7a2952..231c9eea 100644 --- a/canopen_402_driver/include/canopen_402_driver/visibility_control.h +++ b/canopen_402_driver/include/canopen_402_driver/visibility_control.h @@ -5,31 +5,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define CANOPEN_402_DRIVER_EXPORT __attribute__ ((dllexport)) - #define CANOPEN_402_DRIVER_IMPORT __attribute__ ((dllimport)) - #else - #define CANOPEN_402_DRIVER_EXPORT __declspec(dllexport) - #define CANOPEN_402_DRIVER_IMPORT __declspec(dllimport) - #endif - #ifdef CANOPEN_402_DRIVER_BUILDING_LIBRARY - #define CANOPEN_402_DRIVER_PUBLIC CANOPEN_402_DRIVER_EXPORT - #else - #define CANOPEN_402_DRIVER_PUBLIC CANOPEN_402_DRIVER_IMPORT - #endif - #define CANOPEN_402_DRIVER_PUBLIC_TYPE CANOPEN_402_DRIVER_PUBLIC - #define CANOPEN_402_DRIVER_LOCAL +#ifdef __GNUC__ +#define CANOPEN_402_DRIVER_EXPORT __attribute__((dllexport)) +#define CANOPEN_402_DRIVER_IMPORT __attribute__((dllimport)) #else - #define CANOPEN_402_DRIVER_EXPORT __attribute__ ((visibility("default"))) - #define CANOPEN_402_DRIVER_IMPORT - #if __GNUC__ >= 4 - #define CANOPEN_402_DRIVER_PUBLIC __attribute__ ((visibility("default"))) - #define CANOPEN_402_DRIVER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define CANOPEN_402_DRIVER_PUBLIC - #define CANOPEN_402_DRIVER_LOCAL - #endif - #define CANOPEN_402_DRIVER_PUBLIC_TYPE +#define CANOPEN_402_DRIVER_EXPORT __declspec(dllexport) +#define CANOPEN_402_DRIVER_IMPORT __declspec(dllimport) +#endif +#ifdef CANOPEN_402_DRIVER_BUILDING_LIBRARY +#define CANOPEN_402_DRIVER_PUBLIC CANOPEN_402_DRIVER_EXPORT +#else +#define CANOPEN_402_DRIVER_PUBLIC CANOPEN_402_DRIVER_IMPORT +#endif +#define CANOPEN_402_DRIVER_PUBLIC_TYPE CANOPEN_402_DRIVER_PUBLIC +#define CANOPEN_402_DRIVER_LOCAL +#else +#define CANOPEN_402_DRIVER_EXPORT __attribute__((visibility("default"))) +#define CANOPEN_402_DRIVER_IMPORT +#if __GNUC__ >= 4 +#define CANOPEN_402_DRIVER_PUBLIC __attribute__((visibility("default"))) +#define CANOPEN_402_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else +#define CANOPEN_402_DRIVER_PUBLIC +#define CANOPEN_402_DRIVER_LOCAL +#endif +#define CANOPEN_402_DRIVER_PUBLIC_TYPE #endif #endif // CANOPEN_402_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/canopen_402_driver/include/canopen_402_driver/word_accessor.hpp b/canopen_402_driver/include/canopen_402_driver/word_accessor.hpp new file mode 100644 index 00000000..34e54914 --- /dev/null +++ b/canopen_402_driver/include/canopen_402_driver/word_accessor.hpp @@ -0,0 +1,37 @@ +#ifndef WORD_ACCESSOR_HPP +#define WORD_ACCESSOR_HPP + +#include + +namespace ros2_canopen +{ +template +class WordAccessor +{ + uint16_t & word_; + +public: + WordAccessor(uint16_t & word) : word_(word) {} + bool set(uint8_t bit) + { + uint16_t val = MASK & (1 << bit); + word_ |= val; + return val; + } + bool reset(uint8_t bit) + { + uint16_t val = MASK & (1 << bit); + word_ &= ~val; + return val; + } + bool get(uint8_t bit) const { return word_ & (1 << bit); } + uint16_t get() const { return word_ & MASK; } + WordAccessor & operator=(const uint16_t & val) + { + word_ = (word_ & ~MASK) | (val & MASK); + return *this; + } +}; +} // namespace ros2_canopen + +#endif // WORD_ACCESSOR_HPP diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index 722db6d4..d8980e21 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -9,12 +9,16 @@ ament_cmake_ros + boost + canopen_base_driver + canopen_core + canopen_interfaces + canopen_proxy_driver rclcpp rclcpp_components - std_msgs + rclcpp_lifecycle + sensor_msgs std_srvs - canopen_proxy_driver - boost ament_lint_auto diff --git a/canopen_402_driver/src/canopen_402_driver.cpp b/canopen_402_driver/src/canopen_402_driver.cpp deleted file mode 100644 index 7c121123..00000000 --- a/canopen_402_driver/src/canopen_402_driver.cpp +++ /dev/null @@ -1,203 +0,0 @@ -#include "canopen_402_driver/canopen_402_driver.hpp" - -using namespace ros2_canopen; -using namespace std::placeholders; - -void MotionControllerDriver::handle_init( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - motor_->handleInit(); - mc_driver_->validate_objs(); - response->success = true; - } -} - -void MotionControllerDriver::handle_recover( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - motor_->handleRecover(); - response->success = true; - } -} - -void MotionControllerDriver::handle_halt( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - motor_->handleHalt(); - } -} - -void MotionControllerDriver::handle_set_mode_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - if (motor_->getMode() != MotorBase::Profiled_Position) - { - response->success = motor_->enterModeAndWait(MotorBase::Profiled_Position); - return; - } - - response->success = false; - } -} - -void MotionControllerDriver::handle_set_mode_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - if (motor_->getMode() != MotorBase::Profiled_Velocity) - { - response->success = motor_->enterModeAndWait(MotorBase::Profiled_Velocity); - return; - } - response->success = false; - } -} - -void MotionControllerDriver::handle_set_mode_cyclic_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Position) - { - response->success = motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Position); - return; - } - response->success = false; - } -} - -void MotionControllerDriver::handle_set_mode_cyclic_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Velocity) - { - response->success = motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Velocity); - return; - } - response->success = false; - } -} - -void MotionControllerDriver::handle_set_mode_torque( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (active.load()) - { - if (motor_->getMode() != MotorBase::Profiled_Torque) - { - response->success = motor_->enterModeAndWait(MotorBase::Profiled_Torque); - } - response->success = true; - } -} - -void MotionControllerDriver::handle_set_target( - const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, - canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response) -{ - if (active.load()) - { - response->success = motor_->setTarget(request->target); - } -} - -void MotionControllerDriver::publish(){ - std_msgs::msg::Float64 pos_msg; - std_msgs::msg::Float64 speed_msg; - pos_msg.data = mc_driver_->get_position(); - speed_msg.data = mc_driver_->get_speed(); - publish_actual_position->publish(pos_msg); - publish_actual_speed->publish(speed_msg); -} - - -void MotionControllerDriver::register_services() -{ - publish_actual_position = this->create_publisher("~/actual_position", 10);; - publish_actual_speed = this->create_publisher("~/actual_speed", 10); - handle_init_service = this->create_service( - std::string(this->get_name()).append("/init").c_str(), - std::bind(&MotionControllerDriver::handle_init, this, _1, _2)); - - handle_halt_service = this->create_service( - std::string(this->get_name()).append("/halt").c_str(), - std::bind(&MotionControllerDriver::handle_halt, this, _1, _2)); - - handle_recover_service = this->create_service( - std::string(this->get_name()).append("/recover").c_str(), - std::bind(&MotionControllerDriver::handle_recover, this, _1, _2)); - - handle_set_mode_position_service = this->create_service( - std::string(this->get_name()).append("/position_mode").c_str(), - std::bind(&MotionControllerDriver::handle_set_mode_position, this, _1, _2)); - - handle_set_mode_velocity_service = this->create_service( - std::string(this->get_name()).append("/velocity_mode").c_str(), - std::bind(&MotionControllerDriver::handle_set_mode_velocity, this, _1, _2)); - - handle_set_mode_cyclic_velocity_service = this->create_service( - std::string(this->get_name()).append("/cyclic_velocity_mode").c_str(), - std::bind(&MotionControllerDriver::handle_set_mode_cyclic_velocity, this, _1, _2)); - - handle_set_mode_cyclic_position_service = this->create_service( - std::string(this->get_name()).append("/cyclic_position_mode").c_str(), - std::bind(&MotionControllerDriver::handle_set_mode_cyclic_position, this, _1, _2)); - - handle_set_mode_torque_service = this->create_service( - std::string(this->get_name()).append("/torque_mode").c_str(), - std::bind(&MotionControllerDriver::handle_set_mode_torque, this, _1, _2)); - - handle_set_target_service = this->create_service( - std::string(this->get_name()).append("/target").c_str(), - std::bind(&MotionControllerDriver::handle_set_target, this, _1, _2)); -} - -void MotionControllerDriver::init(ev::Executor &exec, - canopen::AsyncMaster &master, - uint8_t node_id, - std::shared_ptr config) noexcept -{ - ProxyDriver::init(exec, master, node_id, config); - auto period = this->config_->get_entry(std::string(this->get_name()), std::string("period")); - if(!period.has_value()) - { - RCLCPP_ERROR(this->get_logger(), "ERROR: Bus Configuration does not set period for %s", this->get_name()); - return; - } - period_ms_ = period.value(); - driver.reset(); - mc_driver_ = std::make_shared(exec, master, node_id); - driver = std::static_pointer_cast(mc_driver_); - motor_ = std::make_shared(mc_driver_); - register_services(); - - timer_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - timer_ = this->create_wall_timer( - 2000ms, std::bind(&MotionControllerDriver::run, this), timer_group); - driver->Boot(); - active.store(true); -} - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::MotionControllerDriver) \ No newline at end of file diff --git a/canopen_402_driver/src/cia402_driver.cpp b/canopen_402_driver/src/cia402_driver.cpp new file mode 100644 index 00000000..2d07f9ea --- /dev/null +++ b/canopen_402_driver/src/cia402_driver.cpp @@ -0,0 +1,14 @@ +#include "canopen_402_driver/cia402_driver.hpp" + +using namespace ros2_canopen; + +Cia402Driver::Cia402Driver(rclcpp::NodeOptions node_options) : CanopenDriver(node_options) +{ + node_canopen_402_driver_ = + std::make_shared>(this); + node_canopen_driver_ = + std::static_pointer_cast(node_canopen_402_driver_); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::Cia402Driver) diff --git a/canopen_402_driver/src/command.cpp b/canopen_402_driver/src/command.cpp new file mode 100644 index 00000000..7bb13169 --- /dev/null +++ b/canopen_402_driver/src/command.cpp @@ -0,0 +1,102 @@ +#include "canopen_402_driver/command.hpp" +using namespace ros2_canopen; + +const Command402::TransitionTable Command402::transitions_; + +Command402::TransitionTable::TransitionTable() +{ + typedef State402 s; + + transitions_.reserve(32); + + Op disable_voltage(0, (1 << CW_Fault_Reset) | (1 << CW_Enable_Voltage)); + /* 7*/ add(s::Ready_To_Switch_On, s::Switch_On_Disabled, disable_voltage); + /* 9*/ add(s::Operation_Enable, s::Switch_On_Disabled, disable_voltage); + /*10*/ add(s::Switched_On, s::Switch_On_Disabled, disable_voltage); + /*12*/ add(s::Quick_Stop_Active, s::Switch_On_Disabled, disable_voltage); + + Op automatic(0, 0); + /* 0*/ add(s::Start, s::Not_Ready_To_Switch_On, automatic); + /* 1*/ add(s::Not_Ready_To_Switch_On, s::Switch_On_Disabled, automatic); + /*14*/ add(s::Fault_Reaction_Active, s::Fault, automatic); + + Op shutdown( + (1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage), (1 << CW_Fault_Reset) | (1 << CW_Switch_On)); + /* 2*/ add(s::Switch_On_Disabled, s::Ready_To_Switch_On, shutdown); + /* 6*/ add(s::Switched_On, s::Ready_To_Switch_On, shutdown); + /* 8*/ add(s::Operation_Enable, s::Ready_To_Switch_On, shutdown); + + Op switch_on( + (1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage) | (1 << CW_Switch_On), + (1 << CW_Fault_Reset) | (1 << CW_Enable_Operation)); + /* 3*/ add(s::Ready_To_Switch_On, s::Switched_On, switch_on); + /* 5*/ add(s::Operation_Enable, s::Switched_On, switch_on); + + Op enable_operation( + (1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage) | (1 << CW_Switch_On) | + (1 << CW_Enable_Operation), + (1 << CW_Fault_Reset)); + /* 4*/ add(s::Switched_On, s::Operation_Enable, enable_operation); + /*16*/ add(s::Quick_Stop_Active, s::Operation_Enable, enable_operation); + + Op quickstop((1 << CW_Enable_Voltage), (1 << CW_Fault_Reset) | (1 << CW_Quick_Stop)); + /* 7*/ add( + s::Ready_To_Switch_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled + /*10*/ add(s::Switched_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled + /*11*/ add(s::Operation_Enable, s::Quick_Stop_Active, quickstop); + + // fault reset + /*15*/ add(s::Fault, s::Switch_On_Disabled, Op((1 << CW_Fault_Reset), 0)); +} +State402::InternalState Command402::nextStateForEnabling(State402::InternalState state) +{ + switch (state) + { + case State402::Start: + return State402::Not_Ready_To_Switch_On; + + case State402::Fault: + case State402::Not_Ready_To_Switch_On: + return State402::Switch_On_Disabled; + + case State402::Switch_On_Disabled: + return State402::Ready_To_Switch_On; + + case State402::Ready_To_Switch_On: + return State402::Switched_On; + + case State402::Switched_On: + case State402::Quick_Stop_Active: + case State402::Operation_Enable: + return State402::Operation_Enable; + + case State402::Fault_Reaction_Active: + return State402::Fault; + } + throw std::out_of_range("state value is illegal"); +} + +bool Command402::setTransition( + uint16_t & cw, const State402::InternalState & from, const State402::InternalState & to, + State402::InternalState * next) +{ + try + { + if (from != to) + { + State402::InternalState hop = to; + if (next) + { + if (to == State402::Operation_Enable) hop = nextStateForEnabling(from); + *next = hop; + } + transitions_.get(from, hop)(cw); + } + return true; + } + catch (...) + { + /// @todo Print error here. + } + return false; +} diff --git a/canopen_402_driver/src/default_homing_mode.cpp b/canopen_402_driver/src/default_homing_mode.cpp new file mode 100644 index 00000000..4f8de6f8 --- /dev/null +++ b/canopen_402_driver/src/default_homing_mode.cpp @@ -0,0 +1,106 @@ +#include "canopen_402_driver/default_homing_mode.hpp" +using namespace ros2_canopen; + +template +struct masked_status_not_equal +{ + uint16_t & status_; + masked_status_not_equal(uint16_t & status) : status_(status) {} + bool operator()() const { return (status_ & mask) != not_equal; } +}; +bool DefaultHomingMode::start() +{ + execute_ = false; + return read(0); +} +bool DefaultHomingMode::read(const uint16_t & sw) +{ + std::scoped_lock lock(mutex_); + uint16_t old = status_; + status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error); + if (old != status_) + { + cond_.notify_all(); + } + return true; +} +bool DefaultHomingMode::write(Mode::OpModeAccesser & cw) +{ + cw = 0; + if (execute_) + { + cw.set(CW_StartHoming); + return true; + } + return false; +} + +bool DefaultHomingMode::executeHoming() +{ + int hmode = driver->universal_get_value(index, 0x0); + if (hmode == 0) + { + return true; + } + /// @ get abs time from canopen_master + std::chrono::steady_clock::time_point prepare_time = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + // ensure homing is not running + std::unique_lock lock(mutex_); + if (!cond_.wait_until( + lock, prepare_time, masked_status_not_equal(status_))) + { + return error("could not prepare homing"); + } + if (status_ & MASK_Error) + { + return error("homing error before start"); + } + + execute_ = true; + + // ensure start + if (!cond_.wait_until( + lock, prepare_time, + masked_status_not_equal(status_))) + { + return error("homing did not start"); + } + if (status_ & MASK_Error) + { + return error("homing error at start"); + } + + std::chrono::steady_clock::time_point finish_time = + std::chrono::steady_clock::now() + std::chrono::seconds(10); // + + // wait for attained + if (!cond_.wait_until( + lock, finish_time, masked_status_not_equal(status_))) + { + return error("homing not attained"); + } + if (status_ & MASK_Error) + { + return error("homing error during process"); + } + + // wait for motion stop + if (!cond_.wait_until( + lock, finish_time, masked_status_not_equal(status_))) + { + return error("homing did not stop"); + } + if (status_ & MASK_Error) + { + return error("homing error during stop"); + } + + if ((status_ & MASK_Reached) && (status_ & MASK_Attained)) + { + execute_ = false; + return true; + } + + return error("something went wrong while homing"); +} diff --git a/canopen_402_driver/src/lifecycle_canopen_402_driver.cpp b/canopen_402_driver/src/lifecycle_canopen_402_driver.cpp deleted file mode 100644 index a8a93467..00000000 --- a/canopen_402_driver/src/lifecycle_canopen_402_driver.cpp +++ /dev/null @@ -1,264 +0,0 @@ -#include "canopen_402_driver/lifecycle_canopen_402_driver.hpp" - -using namespace ros2_canopen; -using namespace std::placeholders; - -void LifecycleMotionControllerDriver::handle_init( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - motor_->handleInit(); - mc_driver_->validate_objs(); - response->success = true; - } -} - -void LifecycleMotionControllerDriver::handle_recover( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - motor_->handleRecover(); - response->success = true; - } -} - -void LifecycleMotionControllerDriver::handle_halt( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - motor_->handleHalt(); - } -} - -void LifecycleMotionControllerDriver::handle_set_mode_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - if (motor_->getMode() != MotorBase::Profiled_Position) - { - response->success = motor_->enterModeAndWait(MotorBase::Profiled_Position); - return; - } - - response->success = false; - } -} - -void LifecycleMotionControllerDriver::handle_set_mode_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - if (motor_->getMode() != MotorBase::Profiled_Velocity) - { - response->success = motor_->enterModeAndWait(MotorBase::Profiled_Velocity); - return; - } - response->success = false; - } -} - -void LifecycleMotionControllerDriver::handle_set_mode_cyclic_position( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Position) - { - response->success = motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Position); - return; - } - response->success = false; - } -} - -void LifecycleMotionControllerDriver::handle_set_mode_cyclic_velocity( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Velocity) - { - response->success = motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Velocity); - return; - } - response->success = false; - } -} - -void LifecycleMotionControllerDriver::handle_set_mode_torque( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (activated_.load()) - { - if (motor_->getMode() != MotorBase::Profiled_Torque) - { - response->success = motor_->enterModeAndWait(MotorBase::Profiled_Torque); - } - response->success = true; - } -} - -void LifecycleMotionControllerDriver::handle_set_target( - const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, - canopen_interfaces::srv::COTargetDouble::Response::SharedPtr response) -{ - if (activated_.load()) - { - response->success = motor_->setTarget(request->target); - } -} - -void LifecycleMotionControllerDriver::publish() -{ - std_msgs::msg::Float64 pos_msg; - std_msgs::msg::Float64 speed_msg; - pos_msg.data = mc_driver_->get_position(); - speed_msg.data = mc_driver_->get_speed(); - publish_actual_position->publish(pos_msg); - publish_actual_speed->publish(speed_msg); -} - -void LifecycleMotionControllerDriver::register_ros_interface() -{ - RCLCPP_INFO(this->get_logger(), "register_ros_interface"); - LifecycleProxyDriver::register_ros_interface(); - publish_actual_position = this->create_publisher("~/actual_position", 10); - - publish_actual_speed = this->create_publisher("~/actual_speed", 10); - handle_init_service = this->create_service( - std::string(this->get_name()).append("/init").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_init, this, _1, _2)); - - handle_halt_service = this->create_service( - std::string(this->get_name()).append("/halt").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_halt, this, _1, _2)); - - handle_recover_service = this->create_service( - std::string(this->get_name()).append("/recover").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_recover, this, _1, _2)); - - handle_set_mode_position_service = this->create_service( - std::string(this->get_name()).append("/position_mode").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_set_mode_position, this, _1, _2)); - - handle_set_mode_velocity_service = this->create_service( - std::string(this->get_name()).append("/velocity_mode").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_set_mode_velocity, this, _1, _2)); - - handle_set_mode_cyclic_velocity_service = this->create_service( - std::string(this->get_name()).append("/cyclic_velocity_mode").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_set_mode_cyclic_velocity, this, _1, _2)); - - handle_set_mode_cyclic_position_service = this->create_service( - std::string(this->get_name()).append("/cyclic_position_mode").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_set_mode_cyclic_position, this, _1, _2)); - - handle_set_mode_torque_service = this->create_service( - std::string(this->get_name()).append("/torque_mode").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_set_mode_torque, this, _1, _2)); - - handle_set_target_service = this->create_service( - std::string(this->get_name()).append("/target").c_str(), - std::bind(&LifecycleMotionControllerDriver::handle_set_target, this, _1, _2)); -} - -void LifecycleMotionControllerDriver::start_timers() -{ - ros2_canopen::LifecycleProxyDriver::start_timers(); - RCLCPP_INFO(this->get_logger(), "start_timers_start"); - std::this_thread::sleep_for(10ms); - motor_->registerDefaultModes(); - mc_driver_->validate_objs(); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(period_ms_), - std::bind(&LifecycleMotionControllerDriver::run, - this), - this->timer_cbg_); - RCLCPP_INFO(this->get_logger(), "start_timers_end"); -} - -void LifecycleMotionControllerDriver::stop_timers() -{ - timer_->cancel(); -} - -bool LifecycleMotionControllerDriver::add() -{ - RCLCPP_INFO(this->get_logger(), "add_start"); - std::shared_ptr>> prom; - prom = std::make_shared>>(); - std::future> f = prom->get_future(); - master_->GetExecutor().post( - [this, prom]() - { - RCLCPP_INFO(this->get_logger(), "in_executor_start"); - std::scoped_lock lock(this->driver_mutex_); - mc_driver_ = std::make_shared(*exec_, *master_, node_id_); - RCLCPP_INFO(this->get_logger(), "inexecutor_boot"); - mc_driver_->Boot(); - RCLCPP_INFO(this->get_logger(), "inexecutor_set"); - prom->set_value(mc_driver_); - RCLCPP_INFO(this->get_logger(), "in_executor_end"); - }); - RCLCPP_INFO(this->get_logger(), "wait_future"); - auto future_status = f.wait_for(this->non_transmit_timeout_); - if (future_status != std::future_status::ready) - { - RCLCPP_INFO(this->get_logger(), "timed_out"); - return false; - } - RCLCPP_INFO(this->get_logger(), "get_future"); - mc_driver_ = f.get(); - RCLCPP_INFO(this->get_logger(), "set_driver"); - motor_ = std::make_shared(mc_driver_); - driver_ = std::static_pointer_cast(mc_driver_); - RCLCPP_INFO(this->get_logger(), "add_end"); - return true; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleMotionControllerDriver::on_configure(const rclcpp_lifecycle::State &state) -{ - return LifecycleProxyDriver::on_configure(state); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleMotionControllerDriver::on_activate(const rclcpp_lifecycle::State &state) -{ - return LifecycleProxyDriver::on_activate(state); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleMotionControllerDriver::on_deactivate(const rclcpp_lifecycle::State &state) -{ - return LifecycleProxyDriver::on_deactivate(state); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleMotionControllerDriver::on_cleanup(const rclcpp_lifecycle::State &state) -{ - return LifecycleProxyDriver::on_cleanup(state); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleMotionControllerDriver::on_shutdown(const rclcpp_lifecycle::State &state) -{ - return LifecycleProxyDriver::on_shutdown(state); -} - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleMotionControllerDriver) diff --git a/canopen_402_driver/src/lifecycle_cia402_driver.cpp b/canopen_402_driver/src/lifecycle_cia402_driver.cpp new file mode 100644 index 00000000..56b2c02e --- /dev/null +++ b/canopen_402_driver/src/lifecycle_cia402_driver.cpp @@ -0,0 +1,15 @@ +#include "canopen_402_driver/lifecycle_cia402_driver.hpp" + +using namespace ros2_canopen; + +LifecycleCia402Driver::LifecycleCia402Driver(rclcpp::NodeOptions node_options) +: LifecycleCanopenDriver(node_options) +{ + node_canopen_402_driver_ = + std::make_shared>(this); + node_canopen_driver_ = + std::static_pointer_cast(node_canopen_402_driver_); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleCia402Driver) diff --git a/canopen_402_driver/src/motor.cpp b/canopen_402_driver/src/motor.cpp index 166fec01..b2a98889 100644 --- a/canopen_402_driver/src/motor.cpp +++ b/canopen_402_driver/src/motor.cpp @@ -1,668 +1,391 @@ #include "canopen_402_driver/motor.hpp" using namespace ros2_canopen; -namespace canopen_402 +bool Motor402::setTarget(double val) { + if (state_handler_.getState() == State402::Operation_Enable) + { + std::scoped_lock lock(mode_mutex_); + return selected_mode_ && selected_mode_->setTarget(val); + } + return false; +} +bool Motor402::isModeSupported(uint16_t mode) +{ + return mode != MotorBase::Homing && allocMode(mode); +} + +bool Motor402::enterModeAndWait(uint16_t mode) +{ + bool okay = mode != MotorBase::Homing && switchMode(mode); + return okay; +} + +uint16_t Motor402::getMode() +{ + std::scoped_lock lock(mode_mutex_); + return selected_mode_ ? selected_mode_->mode_id_ : (uint16_t)MotorBase::No_Mode; +} + +bool Motor402::isModeSupportedByDevice(uint16_t mode) +{ + uint32_t supported_modes = + driver->universal_get_value(supported_drive_modes_index, 0x0); + bool supported = supported_modes & (1 << (mode - 1)); + bool below_max = mode <= 32; + bool above_min = mode > 0; + return below_max && above_min && supported; +} +void Motor402::registerMode(uint16_t id, const ModeSharedPtr & m) +{ + std::scoped_lock map_lock(map_mutex_); + if (m && m->mode_id_ == id) modes_.insert(std::make_pair(id, m)); +} - State402::InternalState State402::getState() +ModeSharedPtr Motor402::allocMode(uint16_t mode) +{ + ModeSharedPtr res; + if (isModeSupportedByDevice(mode)) + { + std::scoped_lock map_lock(map_mutex_); + std::unordered_map::iterator it = modes_.find(mode); + if (it != modes_.end()) { - std::scoped_lock lock(mutex_); - return state_; + res = it->second; } + } + return res; +} - State402::InternalState State402::read(uint16_t sw) - { - static const uint16_t r = (1 << SW_Ready_To_Switch_On); - static const uint16_t s = (1 << SW_Switched_On); - static const uint16_t o = (1 << SW_Operation_enabled); - static const uint16_t f = (1 << SW_Fault); - static const uint16_t q = (1 << SW_Quick_stop); - static const uint16_t d = (1 << SW_Switch_on_disabled); - - InternalState new_state = Unknown; - - uint16_t state = sw & (d | q | f | o | s | r); - switch (state) - { - // ( d | q | f | o | s | r ): - case (0 | 0 | 0 | 0 | 0 | 0): - case (0 | q | 0 | 0 | 0 | 0): - //std::cout << "Not_Ready_To_Switch_On" << std::endl; - new_state = Not_Ready_To_Switch_On; - break; - - case (d | 0 | 0 | 0 | 0 | 0): - case (d | q | 0 | 0 | 0 | 0): - //std::cout << "Switch_On_Disabled" << std::endl; - new_state = Switch_On_Disabled; - break; - - case (0 | q | 0 | 0 | 0 | r): - //std::cout << "Ready_To_Switch_On" << std::endl; - new_state = Ready_To_Switch_On; - break; - - case (0 | q | 0 | 0 | s | r): - new_state = Switched_On; - break; - - case (0 | q | 0 | o | s | r): - new_state = Operation_Enable; - break; - - case (0 | 0 | 0 | o | s | r): - new_state = Quick_Stop_Active; - break; - - case (0 | 0 | f | o | s | r): - case (0 | q | f | o | s | r): - new_state = Fault_Reaction_Active; - break; - - case (0 | 0 | f | 0 | 0 | 0): - case (0 | q | f | 0 | 0 | 0): - new_state = Fault; - break; - - default: - /// @todo Throw error here. - break; - } - std::scoped_lock lock(mutex_); - if (new_state != state_) - { - state_ = new_state; - cond_.notify_all(); - } - return state_; +bool Motor402::switchMode(uint16_t mode) +{ + if (mode == MotorBase::No_Mode) + { + std::scoped_lock lock(mode_mutex_); + selected_mode_.reset(); + try + { // try to set mode + driver->universal_set_value(op_mode_index, 0x0, mode); } - bool State402::waitForNewState(const std::chrono::steady_clock::time_point &abstime, State402::InternalState &state) + catch (...) { - std::unique_lock lock(mutex_); - while (state_ == state && cond_.wait_until(lock, abstime) == std::cv_status::no_timeout) - { - } - bool res = state != state_; - state = state_; - return res; } - - const Command402::TransitionTable Command402::transitions_; - - Command402::TransitionTable::TransitionTable() + return true; + } + + ModeSharedPtr next_mode = allocMode(mode); + if (!next_mode) + { + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode is not supported."); + return false; + } + + if (!next_mode->start()) + { + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Could not start mode."); + return false; + } + + { // disable mode handler + std::scoped_lock lock(mode_mutex_); + + if (mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode) { - typedef State402 s; - - transitions_.reserve(32); - - Op disable_voltage(0, (1 << CW_Fault_Reset) | (1 << CW_Enable_Voltage)); - /* 7*/ add(s::Ready_To_Switch_On, s::Switch_On_Disabled, disable_voltage); - /* 9*/ add(s::Operation_Enable, s::Switch_On_Disabled, disable_voltage); - /*10*/ add(s::Switched_On, s::Switch_On_Disabled, disable_voltage); - /*12*/ add(s::Quick_Stop_Active, s::Switch_On_Disabled, disable_voltage); - - Op automatic(0, 0); - /* 0*/ add(s::Start, s::Not_Ready_To_Switch_On, automatic); - /* 1*/ add(s::Not_Ready_To_Switch_On, s::Switch_On_Disabled, automatic); - /*14*/ add(s::Fault_Reaction_Active, s::Fault, automatic); - - Op shutdown((1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage), (1 << CW_Fault_Reset) | (1 << CW_Switch_On)); - /* 2*/ add(s::Switch_On_Disabled, s::Ready_To_Switch_On, shutdown); - /* 6*/ add(s::Switched_On, s::Ready_To_Switch_On, shutdown); - /* 8*/ add(s::Operation_Enable, s::Ready_To_Switch_On, shutdown); + // nothing to do + return true; + } - Op switch_on((1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage) | (1 << CW_Switch_On), (1 << CW_Fault_Reset) | (1 << CW_Enable_Operation)); - /* 3*/ add(s::Ready_To_Switch_On, s::Switched_On, switch_on); - /* 5*/ add(s::Operation_Enable, s::Switched_On, switch_on); + selected_mode_.reset(); + } - Op enable_operation((1 << CW_Quick_Stop) | (1 << CW_Enable_Voltage) | (1 << CW_Switch_On) | (1 << CW_Enable_Operation), (1 << CW_Fault_Reset)); - /* 4*/ add(s::Switched_On, s::Operation_Enable, enable_operation); - /*16*/ add(s::Quick_Stop_Active, s::Operation_Enable, enable_operation); + if (!switchState(switching_state_)) return false; - Op quickstop((1 << CW_Enable_Voltage), (1 << CW_Fault_Reset) | (1 << CW_Quick_Stop)); - /* 7*/ add(s::Ready_To_Switch_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled - /*10*/ add(s::Switched_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled - /*11*/ add(s::Operation_Enable, s::Quick_Stop_Active, quickstop); + driver->universal_set_value(op_mode_index, 0x0, mode); - // fault reset - /*15*/ add(s::Fault, s::Switch_On_Disabled, Op((1 << CW_Fault_Reset), 0)); - } - State402::InternalState Command402::nextStateForEnabling(State402::InternalState state) - { - switch (state) - { - case State402::Start: - return State402::Not_Ready_To_Switch_On; - - case State402::Fault: - case State402::Not_Ready_To_Switch_On: - return State402::Switch_On_Disabled; - - case State402::Switch_On_Disabled: - return State402::Ready_To_Switch_On; - - case State402::Ready_To_Switch_On: - return State402::Switched_On; - - case State402::Switched_On: - case State402::Quick_Stop_Active: - case State402::Operation_Enable: - return State402::Operation_Enable; - - case State402::Fault_Reaction_Active: - return State402::Fault; - } - throw std::out_of_range("state value is illegal"); - } + bool okay = false; - bool Command402::setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next) - { - try - { - if (from != to) - { - State402::InternalState hop = to; - if (next) - { - if (to == State402::Operation_Enable) - hop = nextStateForEnabling(from); - *next = hop; - } - transitions_.get(from, hop)(cw); - } - return true; - } - catch (...) - { - /// @todo Print error here. - } - return false; - } + { // wait for switch + std::unique_lock lock(mode_mutex_); - template - struct masked_status_not_equal - { - uint16_t &status_; - masked_status_not_equal(uint16_t &status) : status_(status) {} - bool operator()() const { return (status_ & mask) != not_equal; } - }; - bool DefaultHomingMode::start() + std::chrono::steady_clock::time_point abstime = + std::chrono::steady_clock::now() + std::chrono::seconds(5); + if (monitor_mode_) { - execute_ = false; - return read(0); + while (mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == std::cv_status::no_timeout) + { + } } - bool DefaultHomingMode::read(const uint16_t &sw) + else { - std::scoped_lock lock(mutex_); - uint16_t old = status_; - status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error); - if (old != status_) - { - cond_.notify_all(); - } - return true; - } - bool DefaultHomingMode::write(Mode::OpModeAccesser &cw) - { - cw = 0; - if (execute_) - { - cw.set(CW_StartHoming); - return true; - } - return false; + while (mode_id_ != mode && std::chrono::steady_clock::now() < abstime) + { + lock.unlock(); // unlock inside loop + driver->universal_get_value(op_mode_display_index, 0x0); // poll + std::this_thread::sleep_for(std::chrono::milliseconds(20)); // wait some time + lock.lock(); + } } - bool DefaultHomingMode::executeHoming() + if (mode_id_ == mode) { - int hmode = driver->get_remote_obj(obj); - if ( hmode == 0) - { - return true; - } - /// @ get abs time from canopen_master - std::chrono::steady_clock::time_point prepare_time = std::chrono::steady_clock::now() + std::chrono::seconds(1); - // ensure homing is not running - std::unique_lock lock(mutex_); - if (!cond_.wait_until(lock, prepare_time, masked_status_not_equal(status_))) - { - return error("could not prepare homing"); - } - if (status_ & MASK_Error) - { - return error("homing error before start"); - } - - execute_ = true; - - // ensure start - if (!cond_.wait_until(lock, prepare_time, masked_status_not_equal(status_))) - { - return error("homing did not start"); - } - if (status_ & MASK_Error) - { - return error("homing error at start"); - } - - std::chrono::steady_clock::time_point finish_time = std::chrono::steady_clock::now() + std::chrono::seconds(10); // - - // wait for attained - if (!cond_.wait_until(lock, finish_time, masked_status_not_equal(status_))) - { - return error("homing not attained"); - } - if (status_ & MASK_Error) - { - return error("homing error during process"); - } - - // wait for motion stop - if (!cond_.wait_until(lock, finish_time, masked_status_not_equal(status_))) - { - return error("homing did not stop"); - } - if (status_ & MASK_Error) - { - return error("homing error during stop"); - } - - if ((status_ & MASK_Reached) && (status_ & MASK_Attained)) - { - execute_ = false; - return true; - } - - return error("something went wrong while homing"); + selected_mode_ = next_mode; + okay = true; } - - bool Motor402::setTarget(double val) + else { - if (state_handler_.getState() == State402::Operation_Enable) - { - std::scoped_lock lock(mode_mutex_); - return selected_mode_ && selected_mode_->setTarget(val); - } - return false; + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode switch timed out."); + driver->universal_set_value(op_mode_index, 0x0, mode_id_); } - bool Motor402::isModeSupported(uint16_t mode) { return mode != MotorBase::Homing && allocMode(mode); } + } - bool Motor402::enterModeAndWait(uint16_t mode) - { - bool okay = mode != MotorBase::Homing && switchMode(mode); - return okay; - } + if (!switchState(State402::Operation_Enable)) return false; - uint16_t Motor402::getMode() - { - std::scoped_lock lock(mode_mutex_); - return selected_mode_ ? selected_mode_->mode_id_ : (uint16_t)MotorBase::No_Mode; - } + return okay; +} - bool Motor402::isModeSupportedByDevice(uint16_t mode) +bool Motor402::switchState(const State402::InternalState & target) +{ + std::chrono::steady_clock::time_point abstime = + std::chrono::steady_clock::now() + state_switch_timeout_; + State402::InternalState state = state_handler_.getState(); + target_state_ = target; + while (state != target_state_) + { + std::unique_lock lock(cw_mutex_); + State402::InternalState next = State402::Unknown; + bool success = Command402::setTransition(control_word_, state, target_state_, &next); + if (!success) { - if (!supported_drive_modes_->valid) - { - //THROW_EXCEPTION(std::runtime_error("Supported drive modes (object 6502) is not valid")); - } - uint32_t supported_modes = driver->get_remote_obj_cached(supported_drive_modes_); - bool supported = supported_modes & (1 << (mode - 1)); - bool below_max = mode <= 32; - bool above_min = mode > 0; - return below_max && above_min && supported; + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Could not set transition."); + return false; } - void Motor402::registerMode(uint16_t id, const ModeSharedPtr &m) + lock.unlock(); + if (state != next && !state_handler_.waitForNewState(abstime, state)) { - std::scoped_lock map_lock(map_mutex_); - if (m && m->mode_id_ == id) - modes_.insert(std::make_pair(id, m)); + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Transition timed out."); + return false; } + } + return state == target; +} - ModeSharedPtr Motor402::allocMode(uint16_t mode) - { - ModeSharedPtr res; - if (isModeSupportedByDevice(mode)) - { - std::scoped_lock map_lock(map_mutex_); - std::unordered_map::iterator it = modes_.find(mode); - if (it != modes_.end()) - { - res = it->second; - } - } - return res; - } +bool Motor402::readState() +{ + uint16_t old_sw, sw = driver->universal_get_value( + status_word_entry_index, 0x0); // TODO: added error handling + old_sw = status_word_.exchange(sw); - bool Motor402::switchMode(uint16_t mode) - { + state_handler_.read(sw); - if (mode == MotorBase::No_Mode) - { - std::scoped_lock lock(mode_mutex_); - selected_mode_.reset(); - try - { // try to set mode - driver->set_remote_obj(op_mode_, mode); - } - catch (...) - { - } - return true; - } - - ModeSharedPtr next_mode = allocMode(mode); - if (!next_mode) - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode is not supported."); - return false; - } - - if (!next_mode->start()) - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Could not start mode."); - return false; - } - - { // disable mode handler - std::scoped_lock lock(mode_mutex_); - - if (mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode) - { - // nothing to do - return true; - } - - selected_mode_.reset(); - } - - if (!switchState(switching_state_)) - return false; - - driver->set_remote_obj(op_mode_, mode); - - bool okay = false; - - { // wait for switch - std::unique_lock lock(mode_mutex_); - - std::chrono::steady_clock::time_point abstime = std::chrono::steady_clock::now() + std::chrono::seconds(5); - if (monitor_mode_) - { - while (mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == std::cv_status::no_timeout) - { - } - } - else - { - while (mode_id_ != mode && std::chrono::steady_clock::now() < abstime) - { - lock.unlock(); // unlock inside loop - driver->get_remote_obj(op_mode_display_); // poll - std::this_thread::sleep_for(std::chrono::milliseconds(20)); // wait some time - lock.lock(); - } - } - - if (mode_id_ == mode) - { - selected_mode_ = next_mode; - okay = true; - } - else - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode switch timed out."); - driver->set_remote_obj(op_mode_, mode_id_); - } - } - - if (!switchState(State402::Operation_Enable)) - return false; - - return okay; - } + std::unique_lock lock(mode_mutex_); + uint16_t new_mode; + new_mode = driver->universal_get_value(op_mode_display_index, 0x0); + // RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode %hhi",new_mode); - bool Motor402::switchState(const State402::InternalState &target) + if (selected_mode_ && selected_mode_->mode_id_ == new_mode) + { + if (!selected_mode_->read(sw)) { - std::chrono::steady_clock::time_point abstime = std::chrono::steady_clock::now() + state_switch_timeout_; - State402::InternalState state = state_handler_.getState(); - target_state_ = target; - while (state != target_state_) - { - std::unique_lock lock(cw_mutex_); - State402::InternalState next = State402::Unknown; - bool success = Command402::setTransition(control_word_, state, target_state_, &next); - if (!success) - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Could not set transition."); - return false; - } - lock.unlock(); - if (state != next && !state_handler_.waitForNewState(abstime, state)) - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Transition timed out."); - return false; - } - } - return state == target; + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode handler has error."); } - - bool Motor402::readState() + } + if (new_mode != mode_id_) + { + mode_id_ = new_mode; + mode_cond_.notify_all(); + } + if (selected_mode_ && selected_mode_->mode_id_ != new_mode) + { + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode does not match."); + } + if (sw & (1 << State402::SW_Internal_limit)) + { + if (old_sw & (1 << State402::SW_Internal_limit)) { - - uint16_t old_sw, sw = driver->get_remote_obj(status_word_entry_); // TODO: added error handling - old_sw = status_word_.exchange(sw); - - state_handler_.read(sw); - - std::unique_lock lock(mode_mutex_); - uint16_t new_mode; - new_mode = driver->get_remote_obj(op_mode_display_); - //RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode %hhi",new_mode); - - if (selected_mode_ && selected_mode_->mode_id_ == new_mode) - { - if (!selected_mode_->read(sw)) - { - - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode handler has error."); - } - } - if (new_mode != mode_id_) - { - mode_id_ = new_mode; - mode_cond_.notify_all(); - } - if (selected_mode_ && selected_mode_->mode_id_ != new_mode) - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode does not match."); - } - if (sw & (1 << State402::SW_Internal_limit)) - { - if (old_sw & (1 << State402::SW_Internal_limit)) - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active"); - } - else - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active"); - } - } - - return true; + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active"); } - void Motor402::handleRead() + else { - readState(); + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active"); } - void Motor402::handleWrite() - { + } - std::scoped_lock lock(cw_mutex_); - control_word_ |= (1 << Command402::CW_Halt); - if (state_handler_.getState() == State402::Operation_Enable) - { - std::scoped_lock lock(mode_mutex_); - Mode::OpModeAccesser cwa(control_word_); - bool okay = false; - if (selected_mode_ && selected_mode_->mode_id_ == mode_id_) - { - okay = selected_mode_->write(cwa); - } - else - { - cwa = 0; - } - if (okay) - { - control_word_ &= ~(1 << Command402::CW_Halt); - } - } - if (start_fault_reset_.exchange(false)) - { - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Fault reset"); - this->driver->set_remote_obj(control_word_entry_,control_word_ & ~(1 << Command402::CW_Fault_Reset)); - } - else - { - //RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Control Word %s", std::bitset<16>{control_word_}.to_string()); - this->driver->set_remote_obj(control_word_entry_, control_word_); - } - } - void Motor402::handleDiag() + return true; +} +void Motor402::handleRead() { readState(); } +void Motor402::handleWrite() +{ + std::scoped_lock lock(cw_mutex_); + control_word_ |= (1 << Command402::CW_Halt); + if (state_handler_.getState() == State402::Operation_Enable) + { + std::scoped_lock lock(mode_mutex_); + Mode::OpModeAccesser cwa(control_word_); + bool okay = false; + if (selected_mode_ && selected_mode_->mode_id_ == mode_id_) { - uint16_t sw = status_word_; - State402::InternalState state = state_handler_.getState(); - - switch (state) - { - case State402::Not_Ready_To_Switch_On: - case State402::Switch_On_Disabled: - case State402::Ready_To_Switch_On: - case State402::Switched_On: - std::cout << "Motor operation is not enabled" << std::endl; - case State402::Operation_Enable: - break; - - case State402::Quick_Stop_Active: - std::cout << "Quick stop is active" << std::endl; - break; - case State402::Fault: - case State402::Fault_Reaction_Active: - std::cout << "Motor has fault" << std::endl; - break; - case State402::Unknown: - std::cout << "State is unknown" << std::endl; - break; - } - - if (sw & (1 << State402::SW_Warning)) - { - std::cout << "Warning bit is set" << std::endl; - } - if (sw & (1 << State402::SW_Internal_limit)) - { - std::cout << "Internal limit active" << std::endl; - } + okay = selected_mode_->write(cwa); } - void Motor402::handleInit() + else { - for (std::unordered_map::iterator it = mode_allocators_.begin(); it != mode_allocators_.end(); ++it) - { - (it->second)(); - } - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Read State"); - if (!readState()) - { - std::cout <<"Could not read motor state" << std::endl; - return; - } - { - std::scoped_lock lock(cw_mutex_); - control_word_ = 0; - start_fault_reset_ = true; - } - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Enable"); - if (!switchState(State402::Operation_Enable)) - { - std::cout <<"Could not enable motor" << std::endl; - return; - } - - ModeSharedPtr m = allocMode(MotorBase::Homing); - if (!m) - { - return; // homing not supported - } - - HomingMode *homing = dynamic_cast(m.get()); - - if (!homing) - { - std::cout <<"Homing mode has incorrect handler" << std::endl; - return; - } - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Switch to homing"); - if (!switchMode(MotorBase::Homing)) - { - std::cout <<"Could not enter homing mode" << std::endl; - return; - } - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Execute homing"); - if (!homing->executeHoming()) - { - std::cout <<"Homing failed" << std::endl; - return; - } - RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Switch no mode"); - if (!switchMode(MotorBase::No_Mode)) - { - std::cout <<"Could not enter no mode" << std::endl; - return; - } + cwa = 0; } - void Motor402::handleShutdown() + if (okay) { - switchMode(MotorBase::No_Mode); - switchState(State402::Switch_On_Disabled); + control_word_ &= ~(1 << Command402::CW_Halt); } - void Motor402::handleHalt() + } + if (start_fault_reset_.exchange(false)) + { + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Fault reset"); + this->driver->universal_set_value( + control_word_entry_index, 0x0, control_word_ & ~(1 << Command402::CW_Fault_Reset)); + } + else + { + // RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Control Word %s", + // std::bitset<16>{control_word_}.to_string()); + this->driver->universal_set_value(control_word_entry_index, 0x0, control_word_); + } +} +void Motor402::handleDiag() +{ + uint16_t sw = status_word_; + State402::InternalState state = state_handler_.getState(); + + switch (state) + { + case State402::Not_Ready_To_Switch_On: + case State402::Switch_On_Disabled: + case State402::Ready_To_Switch_On: + case State402::Switched_On: + std::cout << "Motor operation is not enabled" << std::endl; + case State402::Operation_Enable: + break; + + case State402::Quick_Stop_Active: + std::cout << "Quick stop is active" << std::endl; + break; + case State402::Fault: + case State402::Fault_Reaction_Active: + std::cout << "Motor has fault" << std::endl; + break; + case State402::Unknown: + std::cout << "State is unknown" << std::endl; + break; + } + + if (sw & (1 << State402::SW_Warning)) + { + std::cout << "Warning bit is set" << std::endl; + } + if (sw & (1 << State402::SW_Internal_limit)) + { + std::cout << "Internal limit active" << std::endl; + } +} +bool Motor402::handleInit() +{ + for (std::unordered_map::iterator it = mode_allocators_.begin(); + it != mode_allocators_.end(); ++it) + { + (it->second)(); + } + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Read State"); + if (!readState()) + { + std::cout << "Could not read motor state" << std::endl; + return false; + } + { + std::scoped_lock lock(cw_mutex_); + control_word_ = 0; + start_fault_reset_ = true; + } + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Enable"); + if (!switchState(State402::Operation_Enable)) + { + std::cout << "Could not enable motor" << std::endl; + return false; + } + + ModeSharedPtr m = allocMode(MotorBase::Homing); + if (!m) + { + std::cout << "Homeing mode not supported" << std::endl; + return true; // homing not supported + } + + HomingMode * homing = dynamic_cast(m.get()); + + if (!homing) + { + std::cout << "Homing mode has incorrect handler" << std::endl; + return false; + } + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Switch to homing"); + if (!switchMode(MotorBase::Homing)) + { + std::cout << "Could not enter homing mode" << std::endl; + return false; + } + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Execute homing"); + if (!homing->executeHoming()) + { + std::cout << "Homing failed" << std::endl; + return false; + } + RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Init: Switch no mode"); + if (!switchMode(MotorBase::No_Mode)) + { + std::cout << "Could not enter no mode" << std::endl; + return false; + } + return true; +} +bool Motor402::handleShutdown() +{ + switchMode(MotorBase::No_Mode); + return switchState(State402::Switch_On_Disabled); +} +bool Motor402::handleHalt() +{ + State402::InternalState state = state_handler_.getState(); + std::scoped_lock lock(cw_mutex_); + + // do not demand quickstop in case of fault + if (state == State402::Fault_Reaction_Active || state == State402::Fault) return false; + + if (state != State402::Operation_Enable) + { + target_state_ = state; + } + else + { + target_state_ = State402::Quick_Stop_Active; + if (!Command402::setTransition(control_word_, state, State402::Quick_Stop_Active, 0)) { - State402::InternalState state = state_handler_.getState(); - std::scoped_lock lock(cw_mutex_); - - // do not demand quickstop in case of fault - if (state == State402::Fault_Reaction_Active || state == State402::Fault) - return; - - if (state != State402::Operation_Enable) - { - target_state_ = state; - } - else - { - target_state_ = State402::Quick_Stop_Active; - if (!Command402::setTransition(control_word_, state, State402::Quick_Stop_Active, 0)) - { - std::cout << "Could not quick stop" << std::endl; - } - } + std::cout << "Could not quick stop" << std::endl; + return false; } - void Motor402::handleRecover() + } + return true; +} +bool Motor402::handleRecover() +{ + start_fault_reset_ = true; + { + std::scoped_lock lock(mode_mutex_); + if (selected_mode_ && !selected_mode_->start()) { - start_fault_reset_ = true; - { - std::scoped_lock lock(mode_mutex_); - if (selected_mode_ && !selected_mode_->start()) - { - std::cout <<"Could not restart mode." << std::endl; - return; - } - } - if (!switchState(State402::Operation_Enable)) - { - std::cout <<"Could not enable motor" << std::endl; - return; - } + std::cout << "Could not restart mode." << std::endl; + return false; } - -} // namespace + } + if (!switchState(State402::Operation_Enable)) + { + std::cout << "Could not enable motor" << std::endl; + return false; + } + return true; +} diff --git a/canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp b/canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp new file mode 100644 index 00000000..c1ccc77a --- /dev/null +++ b/canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp @@ -0,0 +1,8 @@ +#include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" +#include "canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp" +#include "canopen_core/driver_error.hpp" + +using namespace ros2_canopen::node_interfaces; + +template class ros2_canopen::node_interfaces::NodeCanopen402Driver; +template class ros2_canopen::node_interfaces::NodeCanopen402Driver; diff --git a/canopen_402_driver/src/state.cpp b/canopen_402_driver/src/state.cpp new file mode 100644 index 00000000..ce3f6d60 --- /dev/null +++ b/canopen_402_driver/src/state.cpp @@ -0,0 +1,87 @@ +#include "canopen_402_driver/state.hpp" +#include +using namespace ros2_canopen; + +State402::InternalState State402::getState() +{ + std::scoped_lock lock(mutex_); + return state_; +} + +State402::InternalState State402::read(uint16_t sw) +{ + static const uint16_t r = (1 << SW_Ready_To_Switch_On); + static const uint16_t s = (1 << SW_Switched_On); + static const uint16_t o = (1 << SW_Operation_enabled); + static const uint16_t f = (1 << SW_Fault); + static const uint16_t q = (1 << SW_Quick_stop); + static const uint16_t d = (1 << SW_Switch_on_disabled); + + InternalState new_state = Unknown; + + uint16_t state = sw & (d | q | f | o | s | r); + switch (state) + { + // ( d | q | f | o | s | r ): + case (0 | 0 | 0 | 0 | 0 | 0): + case (0 | q | 0 | 0 | 0 | 0): + // std::cout << "Not_Ready_To_Switch_On" << std::endl; + new_state = Not_Ready_To_Switch_On; + break; + + case (d | 0 | 0 | 0 | 0 | 0): + case (d | q | 0 | 0 | 0 | 0): + // std::cout << "Switch_On_Disabled" << std::endl; + new_state = Switch_On_Disabled; + break; + + case (0 | q | 0 | 0 | 0 | r): + // std::cout << "Ready_To_Switch_On" << std::endl; + new_state = Ready_To_Switch_On; + break; + + case (0 | q | 0 | 0 | s | r): + new_state = Switched_On; + break; + + case (0 | q | 0 | o | s | r): + new_state = Operation_Enable; + break; + + case (0 | 0 | 0 | o | s | r): + new_state = Quick_Stop_Active; + break; + + case (0 | 0 | f | o | s | r): + case (0 | q | f | o | s | r): + new_state = Fault_Reaction_Active; + break; + + case (0 | 0 | f | 0 | 0 | 0): + case (0 | q | f | 0 | 0 | 0): + new_state = Fault; + break; + + default: + /// @todo Throw error here. + break; + } + std::scoped_lock lock(mutex_); + if (new_state != state_) + { + state_ = new_state; + cond_.notify_all(); + } + return state_; +} +bool State402::waitForNewState( + const std::chrono::steady_clock::time_point & abstime, State402::InternalState & state) +{ + std::unique_lock lock(mutex_); + while (state_ == state && cond_.wait_until(lock, abstime) == std::cv_status::no_timeout) + { + } + bool res = state != state_; + state = state_; + return res; +} diff --git a/canopen_402_driver/test/CMakeLists.txt b/canopen_402_driver/test/CMakeLists.txt new file mode 100644 index 00000000..046f3ffc --- /dev/null +++ b/canopen_402_driver/test/CMakeLists.txt @@ -0,0 +1,9 @@ +ament_add_gtest(test_driver_component +test_driver_component.cpp +) +ament_target_dependencies(test_driver_component + ${dependencies} +) +target_include_directories(test_driver_component PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) diff --git a/canopen_402_driver/test/master.dcf b/canopen_402_driver/test/master.dcf new file mode 100644 index 00000000..4f6f975e --- /dev/null +++ b/canopen_402_driver/test/master.dcf @@ -0,0 +1,694 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=2 +NrOfTxPDO=2 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=32 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1401 +15=0x1600 +16=0x1601 +17=0x1800 +18=0x1801 +19=0x1A00 +20=0x1A01 +21=0x1F25 +22=0x1F55 +23=0x1F80 +24=0x1F81 +25=0x1F82 +26=0x1F84 +27=0x1F85 +28=0x1F86 +29=0x1F87 +30=0x1F88 +31=0x1F89 +32=0x1F8A + +[ManufacturerObjects] +SupportedObjects=12 +1=0x2000 +2=0x2001 +3=0x2200 +4=0x2201 +5=0x5800 +6=0x5801 +7=0x5A00 +8=0x5A01 +9=0x5C00 +10=0x5C01 +11=0x5E00 +12=0x5E01 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=2 +2=0x00000082 +3=0x00000083 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1401] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1401sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1401sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000183 + +[1401sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1401sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1401sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1401sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1601] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1601Value] +NrOfEntries=1 +1=0x20010120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1801] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1801sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1801sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000203 + +[1801sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1801sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1801sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1A01] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A01Value] +NrOfEntries=1 +1=0x22010120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=2 +2=0x00000005 +3=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=2 +2=0x00000360 +3=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=proxy_device_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2001] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 2 +ObjectType=0x09 + +[2001sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2001sub1] +ParameterName=proxy_device_2: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=proxy_device_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2201] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 2 +ObjectType=0x09 + +[2201sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2201sub1] +ParameterName=proxy_device_2: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5801] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5A01] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A01Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5C01] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 + +[5E01] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E01Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_402_driver/test/test_driver_component.cpp b/canopen_402_driver/test/test_driver_component.cpp new file mode 100644 index 00000000..dc2ebb73 --- /dev/null +++ b/canopen_402_driver/test/test_driver_component.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "gtest/gtest.h" +using namespace rclcpp_components; + +TEST(ComponentLoad, test_load_component_1) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_402_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[0]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} + +TEST(ComponentLoad, test_load_component_2) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_402_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[1]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} diff --git a/canopen_base_driver/CMakeLists.txt b/canopen_base_driver/CMakeLists.txt index 8920a4ad..7b850aff 100644 --- a/canopen_base_driver/CMakeLists.txt +++ b/canopen_base_driver/CMakeLists.txt @@ -2,117 +2,151 @@ cmake_minimum_required(VERSION 3.8) project(canopen_base_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) +find_package(canopen_core REQUIRED) +find_package(canopen_interfaces REQUIRED) +find_package(lely_core_libraries REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(canopen_interfaces REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(lely_core_libraries REQUIRED) -find_package(canopen_core REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -add_library(${PROJECT_NAME} - SHARED - src/canopen_base_driver.cpp - src/lely_bridge.cpp +set(dependencies + canopen_core + canopen_interfaces + lely_core_libraries + rclcpp + rclcpp_components + rclcpp_lifecycle + std_msgs + std_srvs +) + + +add_library(lely_driver_bridge + src/lely_driver_bridge.cpp ) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_compile_options(${PROJECT_NAME} PUBLIC -Wno-unused-parameter) -target_include_directories(${PROJECT_NAME} PUBLIC +target_compile_features(lely_driver_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(lely_driver_bridge PUBLIC -Wl,--no-undefined) +target_include_directories(lely_driver_bridge PUBLIC $ $) ament_target_dependencies( - ${PROJECT_NAME} - "rclcpp" - "rclcpp_components" - "canopen_interfaces" - "std_msgs" - "std_srvs" - "lely_core_libraries" - "canopen_core" + lely_driver_bridge + ${dependencies} +) + + +add_library(node_canopen_base_driver + src/node_interfaces/node_canopen_base_driver.cpp ) +target_compile_features(node_canopen_base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(node_canopen_base_driver PUBLIC -Wl,--no-undefined) +target_include_directories(node_canopen_base_driver PUBLIC + $ + $) +target_link_libraries(node_canopen_base_driver + lely_driver_bridge +) + +ament_target_dependencies( + node_canopen_base_driver + ${dependencies} +) +message(STATUS "node_canopen_base_driver: ${lely_core_libraries_LIBRARIES}") + +add_library(lifecycle_base_driver + src/lifecycle_base_driver.cpp + ) +target_compile_features(lifecycle_base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(lifecycle_base_driver PUBLIC -Wl,--no-undefined) +target_include_directories(lifecycle_base_driver PUBLIC + $ + $) + +target_link_libraries(lifecycle_base_driver + node_canopen_base_driver + lely_driver_bridge +) +ament_target_dependencies( + lifecycle_base_driver + ${dependencies} +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "CANOPEN_BASE_DRIVER_BUILDING_LIBRARY") +target_compile_definitions(lifecycle_base_driver PRIVATE "CANOPEN_base_DRIVER_BUILDING_LIBRARY") +rclcpp_components_register_nodes(lifecycle_base_driver "ros2_canopen::LifecycleBaseDriver") +set(node_plugins "${node_plugins}ros2_canopen::LifecycleBaseDriver;$\n") -add_library(lifecycle_canopen_base_driver - SHARED - src/lifecycle_canopen_base_driver.cpp - src/lely_bridge.cpp -) -target_compile_features(lifecycle_canopen_base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_compile_options(${PROJECT_NAME} PUBLIC -Wno-unused-parameter) -target_include_directories(lifecycle_canopen_base_driver PUBLIC + +add_library(base_driver + src/base_driver.cpp + ) +target_compile_features(base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(base_driver PUBLIC -Wl,--no-undefined) +target_include_directories(base_driver PUBLIC $ $) +target_link_libraries(base_driver + node_canopen_base_driver + lely_driver_bridge +) + ament_target_dependencies( - lifecycle_canopen_base_driver - "rclcpp" - "rclcpp_components" - "canopen_interfaces" - "std_msgs" - "std_srvs" - "lely_core_libraries" - "canopen_core" - "rclcpp_lifecycle" + base_driver + ${dependencies} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(lifecycle_canopen_base_driver PRIVATE "CANOPEN_BASE_DRIVER_BUILDING_LIBRARY") +target_compile_definitions(base_driver PRIVATE "CANOPEN_base_DRIVER_BUILDING_LIBRARY") + +rclcpp_components_register_nodes(base_driver "ros2_canopen::BaseDriver") +set(node_plugins "${node_plugins}ros2_canopen::BaseDriver;$\n") install( DIRECTORY include/ DESTINATION include ) -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) install( - TARGETS lifecycle_canopen_base_driver - EXPORT export_lifecycle_canopen_base_driver + TARGETS lifecycle_base_driver base_driver node_canopen_base_driver lely_driver_bridge + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( - ${PROJECT_NAME} - lifecycle_canopen_base_driver + lifecycle_base_driver + base_driver + node_canopen_base_driver + lely_driver_bridge ) ament_export_targets( export_${PROJECT_NAME} - export_lifecycle_canopen_base_driver ) - ament_export_dependencies( - canopen_core + ${dependencies} ) - - ament_package() diff --git a/canopen_base_driver/include/canopen_base_driver/base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/base_driver.hpp new file mode 100644 index 00000000..c0fb9cd0 --- /dev/null +++ b/canopen_base_driver/include/canopen_base_driver/base_driver.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ +#define CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ + +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "canopen_core/driver_node.hpp" + +namespace ros2_canopen +{ +/** + * @brief Abstract Class for a CANopen Device Node + * + * This class provides the base functionality for creating a + * CANopen device node. It provides callbacks for nmt and rpdo. + */ +class BaseDriver : public ros2_canopen::CanopenDriver +{ + std::shared_ptr> node_canopen_base_driver_; + +public: + BaseDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); + + void register_nmt_state_cb(std::function nmt_state_cb) + { + node_canopen_base_driver_->register_nmt_state_cb(nmt_state_cb); + } + + void register_rpdo_cb(std::function rpdo_cb) + { + node_canopen_base_driver_->register_rpdo_cb(rpdo_cb); + } +}; +} // namespace ros2_canopen + +#endif // CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ diff --git a/canopen_base_driver/include/canopen_base_driver/canopen_base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/canopen_base_driver.hpp deleted file mode 100644 index 4b56c89c..00000000 --- a/canopen_base_driver/include/canopen_base_driver/canopen_base_driver.hpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ -#define CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ -#include - -#include "canopen_base_driver/visibility_control.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/publisher.hpp" -#include "std_msgs/msg/string.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include "canopen_base_driver/lely_bridge.hpp" -#include "canopen_core/device.hpp" -#include "canopen_interfaces/msg/co_data.hpp" -#include "canopen_interfaces/srv/co_read.hpp" -#include "canopen_interfaces/srv/co_write.hpp" - -namespace ros2_canopen -{ - /** - * @brief Abstract Class for a CANopen Device Node - * - * This class provides the base functionality for creating a - * CANopen device node. It provides callbacks for nmt and rpdo. - */ - class BaseDriver : public DriverInterface - { - private: - std::future nmt_state_publisher_future; - std::future rpdo_publisher_future; - - void nmt_listener(); - void rdpo_listener(); - - protected: - std::shared_ptr driver; - std::shared_ptr config_; - - /** - * @brief NMT State Change Callback - * - * This function is called, when the NMT State of the - * associated LelyBridge changes, - * - * @param [in] nmt_state New NMT state - */ - virtual void on_nmt(canopen::NmtState nmt_state) - { - RCLCPP_INFO(this->get_logger(), "New NMT state %d", (int)nmt_state); - } - - /** - * @brief RPDO Callback - * - * This funciton is called when the associated - * LelyBridge detects a change - * on a specific object, due to an RPDO event. - * - * @param [in] data Changed object - */ - virtual void on_rpdo(COData data) - { - RCLCPP_INFO( - this->get_logger(), - "Received PDO index %hu subindex %hhu data %u", - data.index_, - data.subindex_, - data.data_); - } - - explicit BaseDriver( - const rclcpp::NodeOptions &options) - : DriverInterface("base_driver", options) {} - - public: - /** - * @brief Initializer for the driver - * - * Initializes the driver, adds it to the CANopen Master. - * This function needs to be executed inside the masters - * event loop or the masters thread! - * - * @param [in] exec The executor to be used for the driver - * @param [in] master The master the driver should be added to - * @param [in] node_id The nodeid of the device the driver commands - */ - void init( - ev::Executor &exec, - canopen::AsyncMaster &master, - uint8_t node_id, - std::shared_ptr config) noexcept override; - /** - * @brief Removes the driver from master - * - * @todo Check interface! - * - * @param [in] exec - * @param [in] master - * @param [in] node_id - */ - void remove( - ev::Executor &exec, - canopen::AsyncMaster &master, - uint8_t node_id) noexcept override; - }; -} // namespace ros2_canopen - -#endif // CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ \ No newline at end of file diff --git a/canopen_base_driver/include/canopen_base_driver/lely_bridge.hpp b/canopen_base_driver/include/canopen_base_driver/lely_bridge.hpp deleted file mode 100644 index 9fa0f14c..00000000 --- a/canopen_base_driver/include/canopen_base_driver/lely_bridge.hpp +++ /dev/null @@ -1,234 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ -#define CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "canopen_core/exchange.hpp" - -using namespace std::chrono_literals; -using namespace lely; - -namespace ros2_canopen -{ - -/** - * @brief Lely Driver Bridge - * - * This class provides functionalities for bridging between - * Lelycore drivers and standard C++ functions. This means - * it provides async and sync functions for interacting with - * CANopen devices using synchronisation functionalities from C++ standard - * library. - * - */ -class LelyBridge : public canopen::FiberDriver -{ - class TPDOWriteTask : public ev::CoTask - { - public: - COData data; - LelyBridge * driver; - std::mutex mtx; - explicit TPDOWriteTask(ev_exec_t * exec) - : ev::CoTask(exec) - { - // Lock and load - mtx.lock(); - } - void operator()() noexcept - { - std::scoped_lock lk(driver->tpdo_event_mutex); - switch (data.type_) { - case CODataTypes::COData8: - driver->tpdo_mapped[data.index_][data.subindex_] = - static_cast(data.data_); - break; - case CODataTypes::COData16: - driver->tpdo_mapped[data.index_][data.subindex_] = - static_cast(data.data_); - break; - case CODataTypes::COData32: - driver->tpdo_mapped[data.index_][data.subindex_] = - static_cast(data.data_); - break; - default: - break; - } - driver->master.TpdoWriteEvent(driver->id(), data.index_, data.subindex_); - // Unlock when done - mtx.unlock(); - } - }; - -private: - // SDO Read synchronisation items - std::shared_ptr> sdo_read_data_promise; - std::shared_ptr> sdo_write_data_promise; - std::mutex sdo_mutex; - bool running; - std::condition_variable sdo_cond; - - // NMT synchronisation items - std::promise nmt_state_promise; - std::atomic nmt_state_is_set; - std::mutex nmt_mtex; - - // RPDO synchronisation items - std::promise rpdo_promise; - std::atomic rpdo_is_set; - std::mutex pdo_mtex; - - std::vector> tpdo_tasks; - uint8_t nodeid; - - /** - * @brief OnState Callback - * - * This callback function is called when an Nmt state - * change is detected on the connected device. - * - * @param [in] state NMT State - */ - void - OnState(canopen::NmtState state) noexcept override; - - /** - * @brief OnRpdoWrite Callback - * - * This callback function is called when an RPDO - * write request is received from the connected device. - * - * @param [in] idx Object Index - * @param [in] subidx Object Subindex - */ - void - OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept override; - -public: - using FiberDriver::FiberDriver; - - /** - * @brief Construct a new Lely Bridge object - * - * @param [in] exec Executor to use - * @param [in] master Master to use - * @param [in] id NodeId to connect to - */ - LelyBridge(ev_exec_t * exec, canopen::AsyncMaster & master, uint8_t id) - : FiberDriver(exec, master, id) - { - nodeid = id; - running = false; - } - - /** - * @brief Asynchronous SDO Write - * - * Writes the data passed to the function via SDO to - * the connected device. - * - * @param [in] data Data to written. - * - * @return std::future - * Returns an std::future that is fulfilled - * when the write request was done. An error is - * stored when the write request was unsuccesful. - */ - std::future async_sdo_write(COData data); - - /** - * @brief Aynchronous SDO Read - * - * Reads the indicated SDO object from the connected - * device. - * - * @param [in] data Data to be read, the data entry is not used. - * @return std::future - * Returns an std::future that is fulfilled - * when the read request was done. The result of the request - * is stored in the future. An error is stored when the read - * request was unsuccesful. - */ - std::future async_sdo_read(COData data); - - /** - * @brief Asynchronous request for NMT - * - * Waits for an NMT state change to occur. The new - * state is stored in the future returned by the function. - * - * @return std::future - * The returned future is set when NMT State changes. - */ - std::future async_request_nmt(); - - /** - * @brief Asynchronous request for RPDO - * - * Waits for an RPDO write request to be received from - * the slave. The content of the request are stored in - * the returned future. - * - * @return std::future - * The returned future is set when an RPDO event is detected. - */ - std::future async_request_rpdo(); - - /** - * @brief Executes a TPDO transmission - * - * This funciton executes a TPDO transmission. The - * object specified in the input data is sent if it - * is registered as a TPDO with the master. - * - * @param [in] data Object and data to be written - */ - void tpdo_transmit(COData data); - - /** - * @brief Executes a NMT Command - * - * This function sends the NMT command specified as - * parameter. - * - * @param [in] command NMT Command to execute - */ - void nmt_command(canopen::NmtCommand command); - - /** - * @brief Get the nodeid - * - * @return uint8_t - */ - uint8_t get_id(); -}; - -} // namespace ros2_canopen - - -#endif // CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ diff --git a/canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp b/canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp new file mode 100644 index 00000000..f25993c2 --- /dev/null +++ b/canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp @@ -0,0 +1,849 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ +#define CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "canopen_core/exchange.hpp" + +using namespace std::chrono_literals; +using namespace lely; + +namespace ros2_canopen +{ +struct pdo_mapping +{ + bool is_tpdo; + bool is_rpdo; +}; + +typedef std::map> PDOMap; +class DriverDictionary : public lely::CODev +{ +public: + DriverDictionary(std::string eds_file) : lely::CODev(eds_file.c_str()) {} + ~DriverDictionary() + { + // lely::CODev::~CODev(); + } + + std::shared_ptr createPDOMapping() + { + std::shared_ptr pdo_map = std::make_shared(); + fetchRPDO(pdo_map); + fetchTPDO(pdo_map); + return pdo_map; + // COObj * first = co_dev_first_obj((lely::CODev *)this); + // COObj * last = co_dev_last_obj((lely::CODev *)this); + // if (first == nullptr || last == nullptr) + // { + // std::cout << "No objects found in dictionary" << std::endl; + // return pdo_map; + // } + // COObj * current = first; + // while (current != last) + // { + // uint16_t index = co_obj_get_idx(current); + // auto subids = current->getSubidx(); + // bool created = false; + // for (auto subid : subids) + // { + // bool is_rpdo = checkObjRPDO(index, subid); + // bool is_tpdo = checkObjTPDO(index, subid); + // std::cout << "Found subobject: index=" << std::hex << (int)index + // << " subindex=" << (int)subid << (is_rpdo ? " rpdo" : "") + // << (is_tpdo ? " tpdo" : "") << std::endl; + // if (is_rpdo || is_tpdo) + // { + // pdo_mapping mapping; + // mapping.is_rpdo = is_rpdo; + // mapping.is_tpdo = is_tpdo; + // if (!created) + // { + // pdo_map->emplace(index, std::map()); + // created = true; + // } + // (*pdo_map)[index].emplace(subid, mapping); + // } + // } + // current = co_obj_next(current); + // } + // return pdo_map; + } + + void fetchRPDO(std::shared_ptr map) + { + for (int index = 0; index < 256; index++) + { + for (int subindex = 1; subindex < 9; subindex++) + { + auto obj = find(0x1600 + index, subindex); + if (obj == nullptr) + { + continue; + } + uint32_t data; + { + data = obj->getVal(); + } + uint8_t tmps = (data >> 8) & 0xFF; + uint16_t tmpi = (data >> 16) & 0xFFFF; + if (tmpi == 0U) + { + continue; + } + pdo_mapping mapping; + mapping.is_rpdo = true; + mapping.is_tpdo = false; + (*map)[tmpi][tmps] = mapping; + std::cout << "Found rpdo mapped object: index=" << std::hex << (int)tmpi + << " subindex=" << (int)tmps << std::endl; + } + } + } + void fetchTPDO(std::shared_ptr map) + { + for (int index = 0; index < 256; index++) + { + for (int subindex = 1; subindex < 9; subindex++) + { + auto obj = find(0x1A00 + index, subindex); + if (obj == nullptr) + { + continue; + } + uint32_t data; + { + data = obj->getVal(); + } + uint8_t tmps = (data >> 8) & 0xFF; + uint16_t tmpi = (data >> 16) & 0xFFFF; + if (tmpi == 0U) + { + continue; + } + + pdo_mapping mapping; + mapping.is_rpdo = false; + mapping.is_tpdo = true; + (*map)[tmpi][tmps] = mapping; + std::cout << "Found tpdo mapped object: index=" << std::hex << (int)tmpi + << " subindex=" << (int)tmps << std::endl; + } + } + } + + bool checkObjRPDO(uint16_t idx, uint8_t subidx) + { + // std::cout << "Checking for rpo mapping of object: index=" << std::hex << (int)idx + // << " subindex=" << (int)subidx << std::endl; + for (int i = 0; i < 256; i++) + { + if (this->checkObjInPDO(i, 0x1600, idx, subidx)) + { + return true; + } + } + return false; + } + + bool checkObjTPDO(uint16_t idx, uint8_t subidx) + { + // std::cout << "Checking for rpo mapping of object: index=" << std::hex << (int)idx + // << " subindex=" << (int)subidx << std::endl; + for (int i = 0; i < 256; i++) + { + if (this->checkObjInPDO(i, 0x1A00, idx, subidx)) + { + return true; + } + } + return false; + } + + bool checkObjInPDO(uint8_t pdo, uint16_t mapping_idx, uint16_t idx, uint8_t subindex) + { + for (int i = 1; i < 9; i++) + { + auto obj = find(mapping_idx + pdo, i); + if (obj == nullptr) + { + return false; + } + uint32_t data; + { + data = obj->getVal(); + } + uint8_t tmps = (data >> 8) & 0xFF; + uint16_t tmpi = (data >> 16) & 0xFFFF; + + if (tmps == subindex && tmpi == idx) + { + std::cout << "Found object in pdo: " << (int)pdo << std::endl; + return true; + } + } + + return false; + } +}; + +enum class LelyBridgeErrc +{ + NotListedDevice = 'A', + NoResponseOnDeviceType = 'B', + DeviceTypeDifference = 'C', + VendorIdDifference = 'D', + HeartbeatIssue = 'E', + NodeGuardingIssue = 'F', + InconsistentProgramDownload = 'G', + SoftwareUpdateRequired = 'H', + SoftwareDownloadFailed = 'I', + ConfigurationDownloadFailed = 'J', + StartErrorControlFailed = 'K', + NmtSlaveInitiallyOperational = 'L', + ProductCodeDifference = 'M', + RevisionCodeDifference = 'N', + SerialNumberDifference = 'O' +}; + +struct LelyBridgeErrCategory : std::error_category +{ + const char * name() const noexcept override; + std::string message(int ev) const override; +}; +} // namespace ros2_canopen + +namespace std +{ +template <> +struct is_error_code_enum : true_type +{ +}; +std::error_code make_error_code(ros2_canopen::LelyBridgeErrc); +} // namespace std + +namespace ros2_canopen +{ +/** + * @brief Lely Driver Bridge + * + * This class provides functionalities for bridging between + * Lelycore drivers and standard C++ functions. This means + * it provides async and sync functions for interacting with + * CANopen devices using synchronisation functionalities from C++ standard + * library. + * + */ +class LelyDriverBridge : public canopen::FiberDriver +{ +protected: + // Dictionary for driver based on DCF and BIN files. + std::unique_ptr dictionary_; + std::mutex dictionary_mutex_; + std::shared_ptr pdo_map_; + + // SDO Read synchronisation items + std::shared_ptr> sdo_read_data_promise; + std::shared_ptr> sdo_write_data_promise; + std::mutex sdo_mutex; + bool running; + std::condition_variable sdo_cond; + + // NMT synchronisation items + std::promise nmt_state_promise; + std::atomic nmt_state_is_set; + std::mutex nmt_mtex; + + // RPDO synchronisation items + std::promise rpdo_promise; + std::atomic rpdo_is_set; + std::mutex pdo_mtex; + std::shared_ptr> rpdo_queue; + + // EMCY synchronisation items + std::promise emcy_promise; + std::atomic emcy_is_set; + std::mutex emcy_mtex; + std::shared_ptr> emcy_queue; + + // BOOT synchronisation items + std::atomic booted; + char boot_status; + std::string boot_what; + canopen::NmtState boot_state; + std::condition_variable boot_cond; + std::mutex boot_mtex; + + uint8_t nodeid; + std::string name_; + + std::function on_sync_function_; + + // void set_sync_function(std::function on_sync_function) + // { + // on_sync_function_ = on_sync_function; + // } + + // void unset_sync_function() + // { + // on_sync_function_ = std::function(); + // } + + void OnSync(uint8_t cnt, const time_point & t) noexcept override + { + if (on_sync_function_ != nullptr) + { + try + { + on_sync_function_(); + } + catch (...) + { + } + } + } + + /** + * @brief OnState Callback + * + * This callback function is called when an Nmt state + * change is detected on the connected device. + * + * @param [in] state NMT State + */ + void OnState(canopen::NmtState state) noexcept override; + + /** + * @brief OnBoot Callback + * This callback is called when the Boot process of the + * slave that was initiated by the master has been success + * fully finished. + * + * @param st + * @param es + * @param what + */ + virtual void OnBoot(canopen::NmtState st, char es, const ::std::string & what) noexcept override; + + /** + * @brief OnRpdoWrite Callback + * + * This callback function is called when an RPDO + * write request is received from the connected device. + * @todo This function should use a threadsafe queue not the icky implementation we have now. + * + * @param [in] idx Object Index + * @param [in] subidx Object Subindex + */ + void OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept override; + + /** + * The function invoked when an EMCY message is received from the remote node. + * @todo This function should use a threadsafe queue not the icky implementation we have now. + * + * @param eec the emergency error code. + * @param er the error register. + * @param msef the manufacturer-specific error code. + */ + void OnEmcy(uint16_t eec, uint8_t er, uint8_t msef[5]) noexcept override; + +public: + using FiberDriver::FiberDriver; + + /** + * @brief Construct a new Lely Bridge object + * + * @param [in] exec Executor to use + * @param [in] master Master to use + * @param [in] id NodeId to connect to + * @param [in] eds EDS file + * @param [in] bin BIN file (concise dcf) + * + */ + LelyDriverBridge( + ev_exec_t * exec, canopen::AsyncMaster & master, uint8_t id, std::string name, std::string eds, + std::string bin) + : FiberDriver(exec, master, id), + rpdo_queue(new SafeQueue()), + emcy_queue(new SafeQueue()) + { + nodeid = id; + running = false; + name_ = name; + dictionary_ = std::make_unique(eds.c_str()); + struct stat buffer; + if (stat(bin.c_str(), &buffer) == 0) + { + co_unsigned16_t * a = NULL; + co_unsigned16_t * b = NULL; + dictionary_->readDCF(a, b, bin.c_str()); + } + pdo_map_ = dictionary_->createPDOMapping(); + } + + /** + * @brief Asynchronous SDO Write + * + * Writes the data passed to the function via SDO to + * the connected device. + * + * @param [in] data Data to written. + * + * @return std::future + * Returns an std::future that is fulfilled + * when the write request was done. An error is + * stored when the write request was unsuccessful. + */ + std::future async_sdo_write(COData data); + + template + std::future async_sdo_write_typed(uint16_t idx, uint8_t subidx, T value) + { + std::unique_lock lck(sdo_mutex); + if (running) + { + sdo_cond.wait(lck); + } + running = true; + + auto prom = std::make_shared>(); + lely::COSub * sub = this->dictionary_->find(idx, subidx); + if (sub == nullptr) + { + std::cout << "async_sdo_write_typed: id=" << (unsigned int)this->get_id() << " index=0x" + << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx + << " object does not exist" << std::endl; + prom->set_value(false); + this->running = false; + this->sdo_cond.notify_one(); + return prom->get_future(); + } + + this->SubmitWrite( + idx, subidx, value, + [this, value, prom](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable + { + if (ec) + { + prom->set_exception( + lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload")); + } + else + { + std::scoped_lock lck(this->dictionary_mutex_); + this->dictionary_->setVal(idx, subidx, value); + prom->set_value(true); + } + std::unique_lock lck(this->sdo_mutex); + this->running = false; + this->sdo_cond.notify_one(); + }, + 20ms); + return prom->get_future(); + } + + template + bool sync_sdo_write_typed( + uint16_t idx, uint8_t subidx, T value, std::chrono::milliseconds timeout) + { + auto fut = async_sdo_write_typed(idx, subidx, value); + auto wait_res = fut.wait_for(timeout); + if (wait_res == std::future_status::timeout) + { + std::cout << "sync_sdo_write_typed: id=" << (unsigned int)this->get_id() << " index=0x" + << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx + << " timed out." << std::endl; + return false; + } + bool res = false; + try + { + res = fut.get(); + } + catch (std::exception & e) + { + RCLCPP_ERROR(rclcpp::get_logger(name_), e.what()); + } + return res; + } + + /** + * @brief Aynchronous SDO Read + * + * Reads the indicated SDO object from the connected + * device. + * + * @param [in] data Data to be read, the data entry is not used. + * @return std::future + * Returns an std::future that is fulfilled + * when the read request was done. The result of the request + * is stored in the future. An error is stored when the read + * request was unsuccessful. + */ + std::future async_sdo_read(COData data); + + template + std::future async_sdo_read_typed(uint16_t idx, uint8_t subidx) + { + std::unique_lock lck(sdo_mutex); + if (running) + { + sdo_cond.wait(lck); + } + running = true; + + auto prom = std::make_shared>(); + lely::COSub * sub = this->dictionary_->find(idx, subidx); + if (sub == nullptr) + { + std::cout << "async_sdo_read: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex + << (unsigned int)idx << " subindex=" << (unsigned int)subidx + << " object does not exist" << std::endl; + try + { + throw lely::canopen::SdoError(this->get_id(), idx, subidx, lely::canopen::SdoErrc::NO_OBJ); + } + catch (...) + { + prom->set_exception(std::current_exception()); + } + this->running = false; + this->sdo_cond.notify_one(); + return prom->get_future(); + } + this->SubmitRead( + idx, subidx, + [this, prom](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable + { + if (ec) + { + prom->set_exception( + lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload")); + } + else + { + std::scoped_lock lck(this->dictionary_mutex_); + this->dictionary_->setVal(idx, subidx, value); + prom->set_value(value); + } + std::unique_lock lck(this->sdo_mutex); + this->running = false; + this->sdo_cond.notify_one(); + }, + 20ms); + return prom->get_future(); + } + + template + bool sync_sdo_read_typed( + uint16_t idx, uint8_t subidx, T & value, std::chrono::milliseconds timeout) + { + auto fut = async_sdo_read_typed(idx, subidx); + auto wait_res = fut.wait_for(timeout); + if (wait_res == std::future_status::timeout) + { + std::cout << "sync_sdo_read_typed: id=" << (unsigned int)this->get_id() << " index=0x" + << std::hex << (unsigned int)idx << " subindex=" << (unsigned int)subidx + << " timed out." << std::endl; + return false; + } + bool res = false; + try + { + value = fut.get(); + res = true; + } + catch (std::exception & e) + { + RCLCPP_ERROR(rclcpp::get_logger(name_), e.what()); + res = false; + } + return res; + } + + /** + * @brief Asynchronous request for NMT + * + * Waits for an NMT state change to occur. The new + * state is stored in the future returned by the function. + * + * @return std::future + * The returned future is set when NMT State changes. + */ + std::future async_request_nmt(); + + /** + * @brief Asynchronous request for RPDO + * + * Waits for an RPDO write request to be received from + * the slave. The content of the request are stored in + * the returned future. + * @todo This function should use a threadsafe queue not the icky implementation we have now. + * + * @return std::future + * The returned future is set when an RPDO event is detected. + */ + std::shared_ptr> get_rpdo_queue(); + + /** + * @brief Asynchronous request for EMCY + * @todo This function should use a threadsafe queue not the icky implementation we have now. + * + * @return std::future + * The returned future is set when an EMCY event is detected. + */ + std::shared_ptr> get_emcy_queue(); + + /** + * @brief Executes a TPDO transmission + * + * This function executes a TPDO transmission. The{false, true} + * object specified in the input data is sent if it + * is registered as a TPDO with the master. + * + * @param [in] data Object and data to be written + */ + void tpdo_transmit(COData data); + + /** + * @brief Executes a NMT Command + * + * This function sends the NMT command specified as + * parameter. + * + * @param [in] command NMT Command to execute + */ + void nmt_command(canopen::NmtCommand command); + + /** + * @brief Get the nodeid + * + * @return uint8_t + */ + uint8_t get_id(); + + /** + * @brief Wait for device to be booted + * + * @return true + * @return false + */ + bool wait_for_boot() + { + if (booted.load()) + { + return true; + } + std::unique_lock lck(boot_mtex); + boot_cond.wait(lck); + if ((boot_status != 0) && (boot_status != 'L')) + { + throw std::system_error(boot_status, LelyBridgeErrCategory(), "Boot Issue"); + } + else + { + booted.store(true); + return true; + } + return false; + } + + void set_sync_function(std::function on_sync_function) + { + on_sync_function_ = on_sync_function; + } + + void unset_sync_function() { on_sync_function_ = std::function(); } + + /** + * @brief Request master to boot device + * + */ + void Boot() + { + booted.store(false); + FiberDriver::Boot(); + } + + /** + * @brief Indicates if Device is booted + * + * @return true + * @return false + */ + bool is_booted() { return booted.load(); } + + template + void submit_write(COData data) + { + T value = 0; + std::memcpy(&value, &data.data_, sizeof(value)); + + this->SubmitWrite( + data.index_, data.subindex_, value, + [this, value](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable + { + if (ec) + { + this->sdo_write_data_promise->set_exception( + lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload")); + } + else + { + std::scoped_lock lck(this->dictionary_mutex_); + this->dictionary_->setVal(idx, subidx, value); + this->sdo_write_data_promise->set_value(true); + } + std::unique_lock lck(this->sdo_mutex); + this->running = false; + this->sdo_cond.notify_one(); + }, + 20ms); + } + + template + void submit_read(COData data) + { + this->SubmitRead( + data.index_, data.subindex_, + [this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable + { + if (ec) + { + this->sdo_read_data_promise->set_exception( + lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload")); + } + else + { + std::scoped_lock lck(this->dictionary_mutex_); + this->dictionary_->setVal(idx, subidx, value); + COData d = {idx, subidx, 0}; + std::memcpy(&d.data_, &value, sizeof(T)); + this->sdo_read_data_promise->set_value(d); + } + std::unique_lock lck(this->sdo_mutex); + this->running = false; + this->sdo_cond.notify_one(); + }, + 20ms); + } + + template + const T universal_get_value(uint16_t index, uint8_t subindex) + { + T value = 0; + bool is_tpdo = false; + if (this->pdo_map_->find(index) != this->pdo_map_->end()) + { + auto object = this->pdo_map_->at(index); + if (object.find(subindex) != object.end()) + { + auto entry = object.at(subindex); + is_tpdo = entry.is_tpdo; + } + } + if (!is_tpdo) + { + if (sync_sdo_read_typed(index, subindex, value, std::chrono::milliseconds(20))) + { + return value; + } + } + + std::scoped_lock lck(this->dictionary_mutex_); + if (typeid(T) == typeid(uint8_t)) + { + value = this->dictionary_->getVal(index, subindex); + } + if (typeid(T) == typeid(uint16_t)) + { + value = this->dictionary_->getVal(index, subindex); + } + if (typeid(T) == typeid(uint32_t)) + { + value = this->dictionary_->getVal(index, subindex); + } + if (typeid(T) == typeid(int8_t)) + { + value = this->dictionary_->getVal(index, subindex); + } + if (typeid(T) == typeid(int16_t)) + { + value = this->dictionary_->getVal(index, subindex); + } + if (typeid(T) == typeid(int32_t)) + { + value = this->dictionary_->getVal(index, subindex); + } + + return value; + } + + template + void universal_set_value(uint16_t index, uint8_t subindex, T value) + { + bool is_rpdo = false; + if (this->pdo_map_->find(index) != this->pdo_map_->end()) + { + auto object = this->pdo_map_->at(index); + if (object.find(subindex) != object.end()) + { + auto entry = object.at(subindex); + is_rpdo = entry.is_rpdo; + } + } + if (is_rpdo) + { + std::scoped_lock lck(this->dictionary_mutex_); + this->dictionary_->setVal(index, subindex, value); + this->tpdo_mapped[index][subindex] = value; + this->tpdo_mapped[index][subindex].WriteEvent(); + } + else + { + sync_sdo_write_typed(index, subindex, value, std::chrono::milliseconds(20)); + } + } +}; + +} // namespace ros2_canopen + +#endif // CANOPEN_BASE_DRIVER__LELY_BRIDGE_HPP_ diff --git a/canopen_base_driver/include/canopen_base_driver/lifecycle_base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/lifecycle_base_driver.hpp new file mode 100644 index 00000000..179fa596 --- /dev/null +++ b/canopen_base_driver/include/canopen_base_driver/lifecycle_base_driver.hpp @@ -0,0 +1,48 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ +#define CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ + +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "canopen_core/driver_node.hpp" + +namespace ros2_canopen +{ +/** + * @brief Lifecycle Base Driver + * + * A very basic driver without any functionality. + * + */ +class LifecycleBaseDriver : public ros2_canopen::LifecycleCanopenDriver +{ + std::shared_ptr> + node_canopen_base_driver_; + +public: + LifecycleBaseDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); + + void register_nmt_state_cb(std::function nmt_state_cb) + { + node_canopen_base_driver_->register_nmt_state_cb(nmt_state_cb); + } + + void register_rpdo_cb(std::function rpdo_cb) + { + node_canopen_base_driver_->register_rpdo_cb(rpdo_cb); + } +}; +} // namespace ros2_canopen + +#endif // CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ diff --git a/canopen_base_driver/include/canopen_base_driver/lifecycle_canopen_base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/lifecycle_canopen_base_driver.hpp deleted file mode 100644 index 11bc335f..00000000 --- a/canopen_base_driver/include/canopen_base_driver/lifecycle_canopen_base_driver.hpp +++ /dev/null @@ -1,151 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ -#define CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ -#include -#include -#include - -#include "canopen_base_driver/visibility_control.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/publisher.hpp" -#include "std_msgs/msg/string.hpp" -#include "std_srvs/srv/trigger.hpp" -#include "lifecycle_msgs/msg/state.hpp" - -#include "canopen_base_driver/lely_bridge.hpp" -#include "canopen_core/device.hpp" -#include "canopen_interfaces/msg/co_data.hpp" -#include "canopen_interfaces/srv/co_read.hpp" -#include "canopen_interfaces/srv/co_write.hpp" -#include "canopen_interfaces/srv/co_node.hpp" - -namespace ros2_canopen -{ - /** - * @brief Abstract Class for a CANopen Device Node - * - * This class provides the base functionality for creating a - * CANopen device node. It provides callbacks for nmt and rpdo. - */ - class LifecycleBaseDriver : public LifecycleDriverInterface - { - protected: - std::thread nmt_state_publisher_thread_; - std::thread rpdo_publisher_thread_; - - - void nmt_listener(); - void rdpo_listener(); - - std::mutex driver_mutex_; - std::shared_ptr driver_; - - virtual void start_threads() override - { - nmt_state_publisher_thread_ = - std::thread(std::bind(&ros2_canopen::LifecycleBaseDriver::nmt_listener, this)); - - rpdo_publisher_thread_ = - std::thread(std::bind(&ros2_canopen::LifecycleBaseDriver::rdpo_listener, this)); - } - - virtual void join_threads() override - { - nmt_state_publisher_thread_.join(); - rpdo_publisher_thread_.join(); - } - - /** - * @brief Configures the driver - * - * Read parameters - * Initialise objects - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &state); - - /** - * @brief Activates the driver - * - * Add driver to masters event loop - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &state); - - /** - * @brief Deactivates the driver - * - * Remove driver from masters event loop - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &state); - - /** - * @brief Cleanup the driver - * - * Delete objects - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &state); - - /** - * @brief NMT State Change Callback - * - * Drivers that use the BaseDriver should implement this - * function to handle NMT State changes signaled by the - * device. - * - * @param [in] nmt_state New NMT state - */ - virtual void on_nmt(canopen::NmtState nmt_state) = 0; - - /** - * @brief RPDO Callback - * - * Drivers that use the BaseDriver should implement this - * function to handle PDOs sent from the device. - * - * @param [in] data Changed object - */ - virtual void on_rpdo(COData data) = 0; - - explicit LifecycleBaseDriver( - const rclcpp::NodeOptions &options) - : LifecycleDriverInterface("base_driver", options) - { - } - - public: - virtual bool add() override; - virtual bool remove() override; - }; -} // namespace ros2_canopen - -#endif // CANOPEN_BASE_DRIVER__CANOPEN_BASE_DRIVER_HPP_ diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp new file mode 100644 index 00000000..31ee7041 --- /dev/null +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp @@ -0,0 +1,118 @@ +#ifndef NODE_CANOPEN_BASE_DRIVER +#define NODE_CANOPEN_BASE_DRIVER + +#include "canopen_base_driver/lely_driver_bridge.hpp" +#include "canopen_core/node_interfaces/node_canopen_driver.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include "canopen_interfaces/msg/co_data.hpp" +#include "canopen_interfaces/srv/co_read.hpp" +#include "canopen_interfaces/srv/co_write.hpp" + +namespace ros2_canopen +{ +namespace node_interfaces +{ +template +class NodeCanopenBaseDriver : public NodeCanopenDriver +{ + static_assert( + std::is_base_of::value || + std::is_base_of::value, + "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); + +protected: + std::thread nmt_state_publisher_thread_; + std::thread rpdo_publisher_thread_; + std::thread emcy_publisher_thread_; + std::mutex driver_mutex_; + std::shared_ptr lely_driver_; + uint32_t period_ms_; + bool polling_; + + // nmt state callback + std::function nmt_state_cb_; + // rpdo callback + std::function rpdo_cb_; + // emcy callback + std::function emcy_cb_; + + std::shared_ptr> emcy_queue_; + std::shared_ptr> rpdo_queue_; + rclcpp::TimerBase::SharedPtr poll_timer_; + virtual void poll_timer_callback(); + void nmt_listener(); + virtual void on_nmt(canopen::NmtState nmt_state); + void rdpo_listener(); + virtual void on_rpdo(COData data); + void emcy_listener(); + virtual void on_emcy(COEmcy emcy); + +public: + NodeCanopenBaseDriver(NODETYPE * node); + + virtual ~NodeCanopenBaseDriver() + { + if (nmt_state_publisher_thread_.joinable()) + { + nmt_state_publisher_thread_.join(); + } + if (rpdo_publisher_thread_.joinable()) + { + rpdo_publisher_thread_.join(); + } + } + + virtual void init(bool called_from_base); + + virtual void configure(bool called_from_base); + + virtual void activate(bool called_from_base); + + virtual void deactivate(bool called_from_base); + + virtual void cleanup(bool called_from_base); + + virtual void shutdown(bool called_from_base); + + virtual void add_to_master(); + + virtual void remove_from_master(); + + /** + * @brief Register a callback for NMT state change + * + * @param nmt_state_cb + */ + void register_nmt_state_cb(std::function nmt_state_cb) + { + nmt_state_cb_ = std::move(nmt_state_cb); + } + + /** + * @brief Register a callback for RPDO + * + * @param rpdo_cb + */ + void register_rpdo_cb(std::function rpdo_cb) + { + rpdo_cb_ = std::move(rpdo_cb); + } + + /** + * @brief Register a callback for EMCY + * + * @param emcy_cb + */ + void register_emcy_cb(std::function emcy_cb) + { + emcy_cb_ = std::move(emcy_cb); + } +}; +typedef NodeCanopenBaseDriver NCBDNode; +typedef NodeCanopenBaseDriver NCBDLifecycleNode; +} // namespace node_interfaces +} // namespace ros2_canopen + +#endif diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp new file mode 100644 index 00000000..21444ddf --- /dev/null +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -0,0 +1,317 @@ +#ifndef NODE_CANOPEN_BASE_DRIVER_IMPL +#define NODE_CANOPEN_BASE_DRIVER_IMPL +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "canopen_core/driver_error.hpp" + +using namespace ros2_canopen::node_interfaces; + +template +NodeCanopenBaseDriver::NodeCanopenBaseDriver(NODETYPE * node) +: ros2_canopen::node_interfaces::NodeCanopenDriver(node) +{ +} + +template +void NodeCanopenBaseDriver::init(bool called_from_base) +{ +} + +template <> +void NodeCanopenBaseDriver::configure(bool called_from_base) +{ + try + { + polling_ = this->config_["polling"].as(); + } + catch (...) + { + RCLCPP_ERROR(this->node_->get_logger(), "Could not polling from config, setting to true."); + polling_ = true; + } + if (polling_) + { + try + { + period_ms_ = this->config_["period"].as(); + } + catch (...) + { + RCLCPP_ERROR(this->node_->get_logger(), "Could not read period from config, setting to 10ms"); + period_ms_ = 10; + } + } +} +template <> +void NodeCanopenBaseDriver::configure(bool called_from_base) +{ + try + { + polling_ = this->config_["polling"].as(); + } + catch (...) + { + RCLCPP_ERROR(this->node_->get_logger(), "Could not polling from config, setting to true."); + polling_ = true; + } + if (polling_) + { + try + { + period_ms_ = this->config_["period"].as(); + } + catch (...) + { + RCLCPP_ERROR(this->node_->get_logger(), "Could not read period from config, setting to 10ms"); + period_ms_ = 10; + } + } +} + +template +void NodeCanopenBaseDriver::activate(bool called_from_base) +{ + nmt_state_publisher_thread_ = + std::thread(std::bind(&NodeCanopenBaseDriver::nmt_listener, this)); + emcy_queue_ = this->lely_driver_->get_emcy_queue(); + rpdo_queue_ = this->lely_driver_->get_rpdo_queue(); + if (polling_) + { + RCLCPP_INFO(this->node_->get_logger(), "Starting with polling mode."); + poll_timer_ = this->node_->create_wall_timer( + std::chrono::milliseconds(period_ms_), + std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this), this->timer_cbg_); + } + else + { + RCLCPP_INFO(this->node_->get_logger(), "Starting with event mode."); + this->lely_driver_->set_sync_function( + std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this)); + } +} + +template +void NodeCanopenBaseDriver::deactivate(bool called_from_base) +{ + nmt_state_publisher_thread_.join(); + poll_timer_->cancel(); + this->lely_driver_->unset_sync_function(); +} + +template +void NodeCanopenBaseDriver::cleanup(bool called_from_base) +{ +} + +template +void NodeCanopenBaseDriver::shutdown(bool called_from_base) +{ +} + +template +void NodeCanopenBaseDriver::add_to_master() +{ + RCLCPP_INFO(this->node_->get_logger(), "eds file %s", this->eds_.c_str()); + RCLCPP_INFO(this->node_->get_logger(), "bin file %s", this->bin_.c_str()); + std::shared_ptr>> prom; + prom = std::make_shared>>(); + std::future> f = prom->get_future(); + this->exec_->post( + [this, prom]() + { + std::scoped_lock lock(this->driver_mutex_); + this->lely_driver_ = std::make_shared( + *(this->exec_), *(this->master_), this->node_id_, this->node_->get_name(), this->eds_, + this->bin_); + this->driver_ = std::static_pointer_cast(this->lely_driver_); + prom->set_value(lely_driver_); + }); + + auto future_status = f.wait_for(this->non_transmit_timeout_); + if (future_status != std::future_status::ready) + { + RCLCPP_ERROR(this->node_->get_logger(), "Adding timed out."); + throw DriverException("add_to_master: adding timed out"); + } + this->lely_driver_ = f.get(); + this->driver_ = std::static_pointer_cast(this->lely_driver_); + if (!this->lely_driver_->IsReady()) + { + RCLCPP_WARN(this->node_->get_logger(), "Wait for device to boot."); + try + { + this->lely_driver_->wait_for_boot(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(this->node_->get_logger(), e.what()); + } + } + RCLCPP_INFO(this->node_->get_logger(), "Driver booted and ready."); +} + +template +void NodeCanopenBaseDriver::remove_from_master() +{ + std::shared_ptr> prom = std::make_shared>(); + auto f = prom->get_future(); + this->exec_->post( + [this, prom]() + { + this->driver_.reset(); + this->lely_driver_.reset(); + prom->set_value(); + }); + + auto future_status = f.wait_for(this->non_transmit_timeout_); + if (future_status != std::future_status::ready) + { + throw DriverException("remove_from_master: removing timed out"); + } +} +template +void NodeCanopenBaseDriver::nmt_listener() +{ + while (rclcpp::ok()) + { + std::future f; + { + std::scoped_lock lock(this->driver_mutex_); + f = this->lely_driver_->async_request_nmt(); + } + while (f.wait_for(this->non_transmit_timeout_) != std::future_status::ready) + { + if (!this->activated_.load()) return; + } + try + { + auto state = f.get(); + if (nmt_state_cb_) + { + nmt_state_cb_(state, this->lely_driver_->get_id()); + } + on_nmt(state); + } + catch (const std::future_error & e) + { + break; + } + } +} +template +void NodeCanopenBaseDriver::on_nmt(canopen::NmtState nmt_state) +{ +} + +template +void NodeCanopenBaseDriver::on_rpdo(COData data) +{ +} + +template +void NodeCanopenBaseDriver::on_emcy(COEmcy emcy) +{ +} + +template +void NodeCanopenBaseDriver::rdpo_listener() +{ + RCLCPP_INFO(this->node_->get_logger(), "Starting RPDO Listener"); + auto q = lely_driver_->get_rpdo_queue(); + while (rclcpp::ok()) + { + ros2_canopen::COData rpdo; + while (!q->wait_and_pop_for(this->non_transmit_timeout_, rpdo)) + { + if (!this->activated_.load()) return; + } + try + { + if (rpdo_cb_) + { + rpdo_cb_(rpdo, this->lely_driver_->get_id()); + } + on_rpdo(rpdo); + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "RPDO Listener error: " << e.what()); + break; + } + } +} +template +void NodeCanopenBaseDriver::poll_timer_callback() +{ + for (int i = 0; i < 10; i++) + { + auto opt = emcy_queue_->try_pop(); + if (!opt.has_value()) + { + break; + } + try + { + if (emcy_cb_) + { + emcy_cb_(opt.value(), this->lely_driver_->get_id()); + } + on_emcy(opt.value()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "EMCY poll error: " << e.what()); + break; + } + } + for (int i = 0; i < 10; i++) + { + auto opt = rpdo_queue_->try_pop(); + if (!opt.has_value()) + { + break; + } + try + { + if (rpdo_cb_) + { + rpdo_cb_(opt.value(), this->lely_driver_->get_id()); + } + on_rpdo(opt.value()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "RPDO Poll error: " << e.what()); + break; + } + } +} + +template +void NodeCanopenBaseDriver::emcy_listener() +{ + RCLCPP_INFO(this->node_->get_logger(), "Starting EMCY Listener"); + auto q = lely_driver_->get_emcy_queue(); + while (rclcpp::ok()) + { + ros2_canopen::COEmcy emcy; + while (!q->wait_and_pop_for(this->non_transmit_timeout_, emcy)) + { + if (!this->activated_.load()) return; + } + try + { + if (emcy_cb_) + { + emcy_cb_(emcy, this->lely_driver_->get_id()); + } + on_emcy(emcy); + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM(this->node_->get_logger(), "EMCY Listener error: " << e.what()); + break; + } + } +} + +#endif diff --git a/canopen_base_driver/include/canopen_base_driver/visibility_control.h b/canopen_base_driver/include/canopen_base_driver/visibility_control.h index a24ad7cc..36d8ae10 100644 --- a/canopen_base_driver/include/canopen_base_driver/visibility_control.h +++ b/canopen_base_driver/include/canopen_base_driver/visibility_control.h @@ -19,31 +19,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define CANOPEN_BASE_DRIVER_EXPORT __attribute__ ((dllexport)) - #define CANOPEN_BASE_DRIVER_IMPORT __attribute__ ((dllimport)) - #else - #define CANOPEN_BASE_DRIVER_EXPORT __declspec(dllexport) - #define CANOPEN_BASE_DRIVER_IMPORT __declspec(dllimport) - #endif - #ifdef CANOPEN_BASE_DRIVER_BUILDING_LIBRARY - #define CANOPEN_BASE_DRIVER_PUBLIC CANOPEN_BASE_DRIVER_EXPORT - #else - #define CANOPEN_BASE_DRIVER_PUBLIC CANOPEN_BASE_DRIVER_IMPORT - #endif - #define CANOPEN_BASE_DRIVER_PUBLIC_TYPE CANOPEN_BASE_DRIVER_PUBLIC - #define CANOPEN_BASE_DRIVER_LOCAL +#ifdef __GNUC__ +#define CANOPEN_BASE_DRIVER_EXPORT __attribute__((dllexport)) +#define CANOPEN_BASE_DRIVER_IMPORT __attribute__((dllimport)) #else - #define CANOPEN_BASE_DRIVER_EXPORT __attribute__ ((visibility("default"))) - #define CANOPEN_BASE_DRIVER_IMPORT - #if __GNUC__ >= 4 - #define CANOPEN_BASE_DRIVER_PUBLIC __attribute__ ((visibility("default"))) - #define CANOPEN_BASE_DRIVER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define CANOPEN_BASE_DRIVER_PUBLIC - #define CANOPEN_BASE_DRIVER_LOCAL - #endif - #define CANOPEN_BASE_DRIVER_PUBLIC_TYPE +#define CANOPEN_BASE_DRIVER_EXPORT __declspec(dllexport) +#define CANOPEN_BASE_DRIVER_IMPORT __declspec(dllimport) +#endif +#ifdef CANOPEN_BASE_DRIVER_BUILDING_LIBRARY +#define CANOPEN_BASE_DRIVER_PUBLIC CANOPEN_BASE_DRIVER_EXPORT +#else +#define CANOPEN_BASE_DRIVER_PUBLIC CANOPEN_BASE_DRIVER_IMPORT +#endif +#define CANOPEN_BASE_DRIVER_PUBLIC_TYPE CANOPEN_BASE_DRIVER_PUBLIC +#define CANOPEN_BASE_DRIVER_LOCAL +#else +#define CANOPEN_BASE_DRIVER_EXPORT __attribute__((visibility("default"))) +#define CANOPEN_BASE_DRIVER_IMPORT +#if __GNUC__ >= 4 +#define CANOPEN_BASE_DRIVER_PUBLIC __attribute__((visibility("default"))) +#define CANOPEN_BASE_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else +#define CANOPEN_BASE_DRIVER_PUBLIC +#define CANOPEN_BASE_DRIVER_LOCAL +#endif +#define CANOPEN_BASE_DRIVER_PUBLIC_TYPE #endif #endif // CANOPEN_BASE_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index c4cd8025..a47416a6 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -9,11 +9,15 @@ ament_cmake_ros + canopen_core + canopen_interfaces + lely_core_libraries rclcpp rclcpp_components + rclcpp_lifecycle std_msgs std_srvs - canopen_core + boost ament_lint_auto diff --git a/canopen_base_driver/src/base_driver.cpp b/canopen_base_driver/src/base_driver.cpp new file mode 100644 index 00000000..a43ef069 --- /dev/null +++ b/canopen_base_driver/src/base_driver.cpp @@ -0,0 +1,27 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "canopen_base_driver/base_driver.hpp" + +using namespace ros2_canopen; + +BaseDriver::BaseDriver(rclcpp::NodeOptions node_options) : CanopenDriver(node_options) +{ + node_canopen_base_driver_ = + std::make_shared>(this); + node_canopen_driver_ = std::static_pointer_cast( + node_canopen_base_driver_); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::BaseDriver) diff --git a/canopen_base_driver/src/canopen_base_driver.cpp b/canopen_base_driver/src/canopen_base_driver.cpp deleted file mode 100644 index 97488006..00000000 --- a/canopen_base_driver/src/canopen_base_driver.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include -#include "canopen_base_driver/canopen_base_driver.hpp" - -using namespace lely; -using namespace ros2_canopen; - -void BaseDriver::nmt_listener() -{ - while (rclcpp::ok()) { - auto f = driver->async_request_nmt(); - f.wait(); - on_nmt(f.get()); - } -} - -void BaseDriver::rdpo_listener() -{ - while (rclcpp::ok()) { - auto f = driver->async_request_rpdo(); - f.wait(); - on_rpdo(f.get()); - } -} - -void BaseDriver::init( - ev::Executor & exec, - canopen::AsyncMaster & master, - uint8_t node_id, - std::shared_ptr config) noexcept -{ - config_ = config; - driver = - std::make_shared(exec, master, node_id); - nmt_state_publisher_future = - std::async( - std::launch::async, - std::bind(&ros2_canopen::BaseDriver::nmt_listener, this) - ); - rpdo_publisher_future = - std::async( - std::launch::async, - std::bind(&ros2_canopen::BaseDriver::rdpo_listener, this) - ); - driver->Boot(); -} - -void BaseDriver::remove( - ev::Executor & exec, - canopen::AsyncMaster & master, - uint8_t node_id) noexcept -{ - driver.reset(); -} \ No newline at end of file diff --git a/canopen_base_driver/src/lely_bridge.cpp b/canopen_base_driver/src/lely_bridge.cpp deleted file mode 100644 index e899b86c..00000000 --- a/canopen_base_driver/src/lely_bridge.cpp +++ /dev/null @@ -1,244 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -#include -#include "canopen_base_driver/lely_bridge.hpp" - -namespace ros2_canopen -{ - -void LelyBridge::OnState(canopen::NmtState state) noexcept -{ - canopen::NmtState st = state; - // We assume 1F80 bit 2 is false. All slaves are put into Operational after boot-up. - // Lelycore does not track NMT states in this mode except BOOTUP. - if (st == canopen::NmtState::BOOTUP) { - st = canopen::NmtState::START; - } - - if (!nmt_state_is_set.load()) { - // We do not care so much about missing a message, rather push them through. - std::unique_lock lk(nmt_mtex, std::defer_lock); - if (lk.try_lock()) { - nmt_state_is_set.store(true); - nmt_state_promise.set_value(st); - } - } -} - -void LelyBridge::OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept -{ - uint32_t data = (uint32_t)rpdo_mapped[idx][subidx]; - COData codata = {idx, subidx, data, CODataTypes::CODataUnkown}; - - // We do not care so much about missing a message, rather push them through. - std::unique_lock lk(pdo_mtex, std::defer_lock); - if (lk.try_lock()) { - if (!rpdo_is_set.load()) { - rpdo_is_set.store(true); - rpdo_promise.set_value(codata); - } - } -} - -std::future LelyBridge::async_sdo_write(COData data) -{ - std::unique_lock lck(sdo_mutex); - if (running) { - sdo_cond.wait(lck); - } - running = true; - - sdo_write_data_promise = std::make_shared>(); - if (data.type_ == CODataTypes::COData8) { - this->SubmitWrite( - data.index_, data.subindex_, (uint8_t)data.data_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec) mutable - { - if (ec) { - this->sdo_write_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, - idx, - subidx, - ec, - "AsyncDownload") - ); - } else { - this->sdo_write_data_promise->set_value(true); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } else if (data.type_ == CODataTypes::COData16) { - this->SubmitWrite( - data.index_, data.subindex_, (uint16_t)data.data_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec) mutable - { - if (ec) { - this->sdo_write_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncDownload") - ); - } else { - this->sdo_write_data_promise->set_value(true); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } else if (data.type_ == CODataTypes::COData32) { - this->SubmitWrite( - data.index_, data.subindex_, (uint32_t)data.data_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec) mutable - { - if (ec) { - this->sdo_write_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncDownload") - ); - } else { - this->sdo_write_data_promise->set_value(true); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - - return sdo_write_data_promise->get_future(); -} - -std::future LelyBridge::async_sdo_read(COData data) -{ - std::unique_lock lck(sdo_mutex); - if (running) { - sdo_cond.wait(lck); - } - running = true; - - sdo_read_data_promise = std::make_shared>(); - if (data.type_ == CODataTypes::COData8) { - this->SubmitRead( - data.index_, data.subindex_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec, uint8_t value) mutable - { - if (ec) { - this->sdo_read_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncUpload") - ); - } else { - COData d = {idx, subidx, value, CODataTypes::COData16}; - this->sdo_read_data_promise->set_value(d); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } else if (data.type_ == CODataTypes::COData16) { - this->SubmitRead( - data.index_, data.subindex_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec, uint16_t value) mutable - { - if (ec) { - this->sdo_read_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncUpload") - ); - } else { - COData d = {idx, subidx, value, CODataTypes::COData16}; - this->sdo_read_data_promise->set_value(d); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } else if (data.type_ == CODataTypes::COData32) { - this->SubmitRead( - data.index_, data.subindex_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec, uint32_t value) mutable - { - if (ec) { - this->sdo_read_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncUpload") - ); - } else { - COData d = {idx, subidx, value, CODataTypes::COData16}; - this->sdo_read_data_promise->set_value(d); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - return sdo_read_data_promise->get_future(); -} - -std::future LelyBridge::async_request_nmt() -{ - std::scoped_lock lk(nmt_mtex); - nmt_state_is_set.store(false); - nmt_state_promise = std::promise(); - return nmt_state_promise.get_future(); -} - -std::future LelyBridge::async_request_rpdo() -{ - std::scoped_lock lk(pdo_mtex); - rpdo_is_set.store(false); - rpdo_promise = std::promise(); - return rpdo_promise.get_future(); -} - -void LelyBridge::tpdo_transmit(COData data) -{ - TPDOWriteTask task(this->GetStrand()); - task.driver = this; - task.data = data; - { - this->master.GetExecutor().post( - [&task, this] - { - this->GetStrand().post(task); - }); - } - // Wait for task to finish. - std::scoped_lock lk(task.mtx); -} - -void LelyBridge::nmt_command(canopen::NmtCommand command) -{ - this->master.Command(command, nodeid); -} - -uint8_t LelyBridge::get_id() -{ - return nodeid; -} -} // namespace ros2_canopen diff --git a/canopen_base_driver/src/lely_driver_bridge.cpp b/canopen_base_driver/src/lely_driver_bridge.cpp new file mode 100644 index 00000000..2da1b2aa --- /dev/null +++ b/canopen_base_driver/src/lely_driver_bridge.cpp @@ -0,0 +1,399 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +#include "canopen_base_driver/lely_driver_bridge.hpp" +#include +#include + +const ros2_canopen::LelyBridgeErrCategory LelyBridgeErrCategoryInstance{}; + +std::error_code std::make_error_code(ros2_canopen::LelyBridgeErrc e) +{ + return {static_cast(e), LelyBridgeErrCategoryInstance}; +} + +namespace ros2_canopen +{ + +const char * LelyBridgeErrCategory::name() const noexcept { return "LelyBridgeError"; } + +std::string LelyBridgeErrCategory::message(int ev) const +{ + switch (static_cast(ev)) + { + case LelyBridgeErrc::NotListedDevice: + return "The CANopen device is not listed in object 1F81."; + case LelyBridgeErrc::NoResponseOnDeviceType: + return "No response received for upload request of object 1000."; + case LelyBridgeErrc::DeviceTypeDifference: + return "Value of object 1000 from CANopen device is different to value in object 1F84 " + "(Device type)."; + case LelyBridgeErrc::VendorIdDifference: + return "Value of object 1018:01 from CANopen device is different to value in object 1F85 " + "(Vendor-ID)."; + case LelyBridgeErrc::HeartbeatIssue: + return "Heartbeat event. No heartbeat message received from CANopen device."; + case LelyBridgeErrc::NodeGuardingIssue: + return "Node guarding event. No confirmation for guarding request received from CANopen " + "device."; + case LelyBridgeErrc::InconsistentProgramDownload: + return "Objects for program download are not configured or inconsistent."; + case LelyBridgeErrc::SoftwareUpdateRequired: + return "Software update is required, but not allowed because of configuration or current " + "status."; + case LelyBridgeErrc::SoftwareDownloadFailed: + return "Software update is required, but program download failed."; + case LelyBridgeErrc::ConfigurationDownloadFailed: + return "Configuration download failed."; + case LelyBridgeErrc::StartErrorControlFailed: + return "Heartbeat event during start error control service. No heartbeat message received " + "from CANopen device during start error control service."; + case LelyBridgeErrc::NmtSlaveInitiallyOperational: + return "NMT slave was initially operational. (CANopen manager may resume operation with " + "other CANopen devices)"; + case LelyBridgeErrc::ProductCodeDifference: + return "Value of object 1018:02 from CANopen device is different to value in object 1F86 " + "(Product code)."; + case LelyBridgeErrc::RevisionCodeDifference: + return "Value of object 1018:03 from CANopen device is different to value in object 1F87 " + "(Revision number)."; + case LelyBridgeErrc::SerialNumberDifference: + return "Value of object 1018:04 from CANopen device is different to value in object 1F88 " + "(Serial number)."; + default: + return "(unrecognized error)"; + } +} + +void LelyDriverBridge::OnState(canopen::NmtState state) noexcept +{ + canopen::NmtState st = state; + // We assume 1F80 bit 2 is false. All slaves are put into Operational after boot-up. + // Lelycore does not track NMT states in this mode except BOOTUP. + if (st == canopen::NmtState::BOOTUP) + { + st = canopen::NmtState::START; + } + + if (!nmt_state_is_set.load()) + { + // We do not care so much about missing a message, rather push them through. + std::unique_lock lk(nmt_mtex, std::defer_lock); + if (lk.try_lock()) + { + nmt_state_is_set.store(true); + nmt_state_promise.set_value(st); + } + } +} + +void LelyDriverBridge::OnBoot(canopen::NmtState st, char es, const ::std::string & what) noexcept +{ + FiberDriver::OnBoot(st, es, what); + if (es == 0) + { + booted.store(true); + } + std::unique_lock lck(boot_mtex); + this->boot_state = st; + this->boot_status = es; + this->boot_what = what; + boot_cond.notify_all(); +} + +void LelyDriverBridge::OnRpdoWrite(uint16_t idx, uint8_t subidx) noexcept +{ + lely::COSub * sub = this->dictionary_->find(idx, subidx); + if (sub == nullptr) + { + std::cout << "OnRpdoWrite: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex + << (unsigned int)idx << " subindex=" << (unsigned int)subidx + << " object does not exist" << std::endl; + return; + } + uint8_t co_def = (uint8_t)sub->getType(); + uint32_t data = 0; + if (co_def == CO_DEFTYPE_UNSIGNED8) + { + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal((uint8_t)rpdo_mapped[idx][subidx]); + std::memcpy(&data, &sub->getVal(), 1); + } + if (co_def == CO_DEFTYPE_INTEGER8) + { + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal((int8_t)rpdo_mapped[idx][subidx]); + std::memcpy(&data, &sub->getVal(), 1); + } + if (co_def == CO_DEFTYPE_UNSIGNED16) + { + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal((uint16_t)rpdo_mapped[idx][subidx]); + std::memcpy(&data, &sub->getVal(), 2); + } + if (co_def == CO_DEFTYPE_INTEGER16) + { + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal((int16_t)rpdo_mapped[idx][subidx]); + std::memcpy(&data, &sub->getVal(), 2); + } + if (co_def == CO_DEFTYPE_UNSIGNED32) + { + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal((uint32_t)rpdo_mapped[idx][subidx]); + std::memcpy(&data, &sub->getVal(), 4); + } + if (co_def == CO_DEFTYPE_INTEGER32) + { + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal((int32_t)rpdo_mapped[idx][subidx]); + std::memcpy(&data, &sub->getVal(), 4); + } + COData codata = {idx, subidx, data}; + + // We do not care so much about missing a message, rather push them through. + rpdo_queue->push(codata); +} + +void LelyDriverBridge::OnEmcy(uint16_t eec, uint8_t er, uint8_t msef[5]) noexcept +{ + FiberDriver::OnEmcy(eec, er, msef); + + COEmcy emcy; + emcy.eec = eec; + emcy.er = er; + for (int i = 0; i < 5; i++) emcy.msef[i] = msef[i]; + + emcy_queue->push(emcy); +} + +std::future LelyDriverBridge::async_sdo_write(COData data) +{ + std::unique_lock lck(sdo_mutex); + if (running) + { + sdo_cond.wait(lck); + } + running = true; + + sdo_write_data_promise = std::make_shared>(); + lely::COSub * sub = this->dictionary_->find(data.index_, data.subindex_); + if (sub == nullptr) + { + std::cout << "async_sdo_write: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex + << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ + << " object does not exist" << std::endl; + this->sdo_write_data_promise->set_value(false); + this->running = false; + return sdo_write_data_promise->get_future(); + } + uint8_t co_def = (uint8_t)sub->getType(); + try + { + if (co_def == CO_DEFTYPE_UNSIGNED8) + { + this->submit_write(data); + } + if (co_def == CO_DEFTYPE_INTEGER8) + { + this->submit_write(data); + } + if (co_def == CO_DEFTYPE_UNSIGNED16) + { + this->submit_write(data); + } + if (co_def == CO_DEFTYPE_INTEGER16) + { + this->submit_write(data); + } + if (co_def == CO_DEFTYPE_UNSIGNED32) + { + this->submit_write(data); + } + if (co_def == CO_DEFTYPE_INTEGER32) + { + this->submit_write(data); + } + } + catch (lely::canopen::SdoError & e) + { + this->sdo_read_data_promise->set_exception(lely::canopen::make_sdo_exception_ptr( + this->get_id(), data.index_, data.subindex_, e.code(), "AsyncUpload")); + this->running = false; + this->sdo_cond.notify_one(); + } + + return sdo_write_data_promise->get_future(); +} + +std::future LelyDriverBridge::async_sdo_read(COData data) +{ + std::unique_lock lck(sdo_mutex); + if (running) + { + sdo_cond.wait(lck); + } + running = true; + + sdo_read_data_promise = std::make_shared>(); + lely::COSub * sub = this->dictionary_->find(data.index_, data.subindex_); + if (sub == nullptr) + { + std::cout << "async_sdo_read: id=" << (unsigned int)this->get_id() << " index=0x" << std::hex + << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ + << " object does not exist" << std::endl; + try + { + throw lely::canopen::SdoError( + this->get_id(), data.index_, data.subindex_, lely::canopen::SdoErrc::NO_OBJ); + } + catch (...) + { + this->sdo_read_data_promise->set_exception(std::current_exception()); + } + this->running = false; + return sdo_read_data_promise->get_future(); + } + uint8_t co_def = (uint8_t)sub->getType(); + try + { + if (co_def == CO_DEFTYPE_UNSIGNED8) + { + this->submit_read(data); + } + if (co_def == CO_DEFTYPE_INTEGER8) + { + this->submit_read(data); + } + if (co_def == CO_DEFTYPE_UNSIGNED16) + { + this->submit_read(data); + } + if (co_def == CO_DEFTYPE_INTEGER16) + { + this->submit_read(data); + } + if (co_def == CO_DEFTYPE_UNSIGNED32) + { + this->submit_read(data); + } + if (co_def == CO_DEFTYPE_INTEGER32) + { + this->submit_read(data); + } + } + catch (lely::canopen::SdoError & e) + { + this->sdo_read_data_promise->set_exception(lely::canopen::make_sdo_exception_ptr( + this->get_id(), data.index_, data.subindex_, e.code(), "AsyncUpload")); + this->running = false; + this->sdo_cond.notify_one(); + } + return sdo_read_data_promise->get_future(); +} + +std::future LelyDriverBridge::async_request_nmt() +{ + std::scoped_lock lk(nmt_mtex); + nmt_state_is_set.store(false); + nmt_state_promise = std::promise(); + return nmt_state_promise.get_future(); +} + +std::shared_ptr> LelyDriverBridge::get_rpdo_queue() { return rpdo_queue; } + +std::shared_ptr> LelyDriverBridge::get_emcy_queue() { return emcy_queue; } + +void LelyDriverBridge::tpdo_transmit(COData data) +{ + lely::COSub * sub = this->dictionary_->find(data.index_, data.subindex_); + if (sub == nullptr) + { + std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex + << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ + << " object does not exist" << std::endl; + return; + } + uint8_t co_def = (uint8_t)sub->getType(); + try + { + if (co_def == CO_DEFTYPE_UNSIGNED8) + { + uint8_t val; + std::memcpy(&val, &data.data_, sizeof(uint8_t)); + tpdo_mapped[data.index_][data.subindex_] = val; + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal(val); + } + if (co_def == CO_DEFTYPE_INTEGER8) + { + int8_t val; + std::memcpy(&val, &data.data_, sizeof(int8_t)); + tpdo_mapped[data.index_][data.subindex_] = val; + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal(val); + } + if (co_def == CO_DEFTYPE_UNSIGNED16) + { + uint16_t val; + std::memcpy(&val, &data.data_, sizeof(uint16_t)); + tpdo_mapped[data.index_][data.subindex_] = val; + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal(val); + } + if (co_def == CO_DEFTYPE_INTEGER16) + { + int16_t val; + std::memcpy(&val, &data.data_, sizeof(int16_t)); + tpdo_mapped[data.index_][data.subindex_] = val; + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal(val); + } + if (co_def == CO_DEFTYPE_UNSIGNED32) + { + uint32_t val; + std::memcpy(&val, &data.data_, sizeof(uint32_t)); + tpdo_mapped[data.index_][data.subindex_] = val; + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal(val); + } + if (co_def == CO_DEFTYPE_INTEGER32) + { + int32_t val; + std::memcpy(&val, &data.data_, sizeof(int32_t)); + tpdo_mapped[data.index_][data.subindex_] = val; + std::scoped_lock lck(this->dictionary_mutex_); + sub->setVal(val); + } + tpdo_mapped[data.index_][data.subindex_].WriteEvent(); + std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex + << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ + << (uint32_t)data.data_ << std::endl; + } + catch (lely::canopen::SdoError & e) + { + std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex + << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ + << e.what() << std::endl; + return; + } +} + +void LelyDriverBridge::nmt_command(canopen::NmtCommand command) +{ + this->master.Command(command, nodeid); +} + +uint8_t LelyDriverBridge::get_id() { return nodeid; } +} // namespace ros2_canopen diff --git a/canopen_base_driver/src/lifecycle_base_driver.cpp b/canopen_base_driver/src/lifecycle_base_driver.cpp new file mode 100644 index 00000000..ad449ab3 --- /dev/null +++ b/canopen_base_driver/src/lifecycle_base_driver.cpp @@ -0,0 +1,28 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "canopen_base_driver/lifecycle_base_driver.hpp" + +using namespace ros2_canopen; + +LifecycleBaseDriver::LifecycleBaseDriver(rclcpp::NodeOptions node_options) +: LifecycleCanopenDriver(node_options) +{ + node_canopen_base_driver_ = + std::make_shared>(this); + node_canopen_driver_ = std::static_pointer_cast( + node_canopen_base_driver_); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleBaseDriver) diff --git a/canopen_base_driver/src/lifecycle_canopen_base_driver.cpp b/canopen_base_driver/src/lifecycle_canopen_base_driver.cpp deleted file mode 100644 index aa6eb8d0..00000000 --- a/canopen_base_driver/src/lifecycle_canopen_base_driver.cpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include -#include "canopen_base_driver/lifecycle_canopen_base_driver.hpp" - -using namespace lely; -using namespace ros2_canopen; -using namespace std::chrono_literals; - -void LifecycleBaseDriver::nmt_listener() -{ - while (rclcpp::ok()) - { - std::future f; - { - std::scoped_lock lock (this->driver_mutex_); - f = driver_->async_request_nmt(); - } - while (f.wait_for(non_transmit_timeout_) != std::future_status::ready) - { - if (!this->activated_.load()) - return; - } - on_nmt(f.get()); - } -} - -void LifecycleBaseDriver::rdpo_listener() -{ - while (rclcpp::ok()) - { - std::future f; - { - std::scoped_lock lock (this->driver_mutex_); - f = driver_->async_request_rpdo(); - } - - while (f.wait_for(non_transmit_timeout_) != std::future_status::ready) - { - if (!this->activated_.load()) - return; - } - - on_rpdo(f.get()); - } -} - - -bool LifecycleBaseDriver::add() -{ - std::shared_ptr>> prom; - prom = std::make_shared>>(); - std::future> f = prom->get_future(); - master_->GetExecutor().post([this, prom]() - { - std::scoped_lock lock (this->driver_mutex_); - driver_ = - std::make_shared(*exec_, *master_, node_id_); - driver_->Boot(); - prom->set_value(driver_); }); - auto future_status = f.wait_for(this->non_transmit_timeout_); - if (future_status != std::future_status::ready) - { - return false; - } - driver_ = f.get(); - return true; -} - -bool LifecycleBaseDriver::remove() -{ - std::shared_ptr> prom = std::make_shared>(); - auto f = prom->get_future(); - exec_->post([this, prom]() - { - driver_.reset(); - prom->set_value(); }); - auto future_status = f.wait_for(this->non_transmit_timeout_); - - if (future_status != std::future_status::ready) - { - return false; - } - return true; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleBaseDriver::on_configure(const rclcpp_lifecycle::State &state) -{ - return LifecycleDriverInterface::on_configure(state); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleBaseDriver::on_activate(const rclcpp_lifecycle::State &state) -{ - return LifecycleDriverInterface::on_activate(state); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleBaseDriver::on_deactivate(const rclcpp_lifecycle::State &state) -{ - return LifecycleDriverInterface::on_deactivate(state); -} -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleBaseDriver::on_cleanup(const rclcpp_lifecycle::State &state) -{ - return LifecycleDriverInterface::on_cleanup(state); -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleBaseDriver::on_shutdown(const rclcpp_lifecycle::State &state) -{ - return LifecycleDriverInterface::on_shutdown(state); -} \ No newline at end of file diff --git a/canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp b/canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp new file mode 100644 index 00000000..cdfcbc07 --- /dev/null +++ b/canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp @@ -0,0 +1,9 @@ +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp" +#include "canopen_core/driver_error.hpp" + +using namespace ros2_canopen::node_interfaces; + +template class ros2_canopen::node_interfaces::NodeCanopenBaseDriver; +template class ros2_canopen::node_interfaces::NodeCanopenBaseDriver< + rclcpp_lifecycle::LifecycleNode>; diff --git a/canopen_base_driver/test/CMakeLists.txt b/canopen_base_driver/test/CMakeLists.txt new file mode 100644 index 00000000..eda8ccb8 --- /dev/null +++ b/canopen_base_driver/test/CMakeLists.txt @@ -0,0 +1,28 @@ +ament_add_gtest(test_node_canopen_base_driver_ros +test_node_canopen_base_driver_ros.cpp +) +ament_target_dependencies(test_node_canopen_base_driver_ros + ${dependencies} +) +target_include_directories(test_node_canopen_base_driver_ros PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_node_canopen_base_driver_ros + node_canopen_base_driver +) + + + + +ament_add_gtest(test_base_driver_component +test_base_driver_component.cpp +) +ament_target_dependencies(test_base_driver_component + ${dependencies} +) +target_include_directories(test_base_driver_component PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) + + +FILE(COPY ${CMAKE_CURRENT_SOURCE_DIR}/master.dcf DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/canopen_base_driver/test/master.dcf b/canopen_base_driver/test/master.dcf new file mode 100644 index 00000000..4f6f975e --- /dev/null +++ b/canopen_base_driver/test/master.dcf @@ -0,0 +1,694 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=2 +NrOfTxPDO=2 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=32 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1401 +15=0x1600 +16=0x1601 +17=0x1800 +18=0x1801 +19=0x1A00 +20=0x1A01 +21=0x1F25 +22=0x1F55 +23=0x1F80 +24=0x1F81 +25=0x1F82 +26=0x1F84 +27=0x1F85 +28=0x1F86 +29=0x1F87 +30=0x1F88 +31=0x1F89 +32=0x1F8A + +[ManufacturerObjects] +SupportedObjects=12 +1=0x2000 +2=0x2001 +3=0x2200 +4=0x2201 +5=0x5800 +6=0x5801 +7=0x5A00 +8=0x5A01 +9=0x5C00 +10=0x5C01 +11=0x5E00 +12=0x5E01 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=2 +2=0x00000082 +3=0x00000083 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1401] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1401sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1401sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000183 + +[1401sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1401sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1401sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1401sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1601] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1601Value] +NrOfEntries=1 +1=0x20010120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1801] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1801sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1801sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000203 + +[1801sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1801sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1801sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1A01] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A01Value] +NrOfEntries=1 +1=0x22010120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=2 +2=0x00000005 +3=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=2 +2=0x00000360 +3=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=proxy_device_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2001] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 2 +ObjectType=0x09 + +[2001sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2001sub1] +ParameterName=proxy_device_2: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=proxy_device_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2201] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 2 +ObjectType=0x09 + +[2201sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2201sub1] +ParameterName=proxy_device_2: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5801] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5A01] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A01Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5C01] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 + +[5E01] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E01Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_base_driver/test/test_base_driver_component.cpp b/canopen_base_driver/test/test_base_driver_component.cpp new file mode 100644 index 00000000..577e12c2 --- /dev/null +++ b/canopen_base_driver/test/test_base_driver_component.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "gtest/gtest.h" +using namespace rclcpp_components; + +TEST(ComponentLoad, test_load_component_1) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_base_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[0]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} + +TEST(ComponentLoad, test_load_component_2) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_base_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[1]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} diff --git a/canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp b/canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp new file mode 100644 index 00000000..56daa1c8 --- /dev/null +++ b/canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp @@ -0,0 +1,75 @@ +#include +#include +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "gtest/gtest.h" + +TEST(NodeCanopenBaseDriver, test_good_sequence_advanced) +{ + rclcpp::init(0, nullptr); + rclcpp::Node * node = new rclcpp::Node("Node"); + auto interface = new ros2_canopen::node_interfaces::NodeCanopenBaseDriver(node); + auto exec = std::make_shared(); + exec->add_node(node->get_node_base_interface()); + std::thread spinner = std::thread([exec] { exec->spin(); }); + + auto iface = static_cast(interface); + + EXPECT_NO_THROW(iface->init()); + + rclcpp::Parameter container_name("container_name", "none"); + rclcpp::Parameter node_id("node_id", 1); + rclcpp::Parameter timeout("non_transmit_timeout", 100); + rclcpp::Parameter config( + "config", + "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " + "\"simple.eds\"\ndcf_path: \"\"\n"); + node->set_parameter(container_name); + node->set_parameter(node_id); + node->set_parameter(timeout); + node->set_parameter(config); + EXPECT_NO_THROW(iface->configure()); + // Can't activate as master cannot be set. + EXPECT_ANY_THROW(iface->activate()); + iface->shutdown(); + rclcpp::shutdown(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (spinner.joinable()) + { + spinner.join(); + } +} + +TEST(NodeCanopenBasicLifecycleMaster, test_good_sequence_advanced) +{ + rclcpp::init(0, nullptr); + rclcpp_lifecycle::LifecycleNode * node = new rclcpp_lifecycle::LifecycleNode("Node"); + auto interface = new ros2_canopen::node_interfaces::NodeCanopenBaseDriver(node); + auto exec = std::make_shared(); + exec->add_node(node->get_node_base_interface()); + std::thread spinner = std::thread([exec] { exec->spin(); }); + + auto iface = static_cast(interface); + + EXPECT_NO_THROW(iface->init()); + + rclcpp::Parameter container_name("container_name", "none"); + rclcpp::Parameter node_id("node_id", 1); + rclcpp::Parameter timeout("non_transmit_timeout", 100); + rclcpp::Parameter config( + "config", + "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " + "\"simple.eds\"\ndcf_path: \"\"\n"); + node->set_parameter(container_name); + node->set_parameter(node_id); + node->set_parameter(timeout); + node->set_parameter(config); + EXPECT_NO_THROW(iface->configure()); + // Can't activate as master cannot be set. + EXPECT_ANY_THROW(iface->activate()); + rclcpp::shutdown(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (spinner.joinable()) + { + spinner.join(); + } +} diff --git a/canopen_core/CMakeLists.txt b/canopen_core/CMakeLists.txt index 8b941ccd..6a62b140 100644 --- a/canopen_core/CMakeLists.txt +++ b/canopen_core/CMakeLists.txt @@ -12,156 +12,114 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - #add_compile_options(-Wall -Wextra)# -Wpedantic) + add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") # find dependencies find_package(ament_cmake REQUIRED) +find_package(canopen_interfaces REQUIRED) +find_package(lely_core_libraries REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(yaml_cpp_vendor REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(lifecycle_msgs REQUIRED) - -find_package(lely_core_libraries REQUIRED) -find_package(canopen_interfaces REQUIRED) - - +find_package(yaml_cpp_vendor REQUIRED) +include(ConfigExtras.cmake) -set (dependencies +set (dependencies + canopen_interfaces lely_core_libraries + lifecycle_msgs rclcpp rclcpp_components - yaml_cpp_vendor rclcpp_lifecycle - lifecycle_msgs - canopen_interfaces + yaml_cpp_vendor + Boost ) -add_library(configuration_manager + +add_library(node_canopen_driver SHARED - src/configuration_manager.cpp + src/node_interfaces/node_canopen_driver.cpp + src/driver_error.cpp + src/driver_node.cpp ) -target_compile_features(configuration_manager PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(configuration_manager PUBLIC +target_compile_features(node_canopen_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(node_canopen_driver PUBLIC -fPIC -Wl,--no-undefined) +target_include_directories(node_canopen_driver PUBLIC $ $ ) -ament_target_dependencies(configuration_manager +ament_target_dependencies(node_canopen_driver rclcpp + rclcpp_lifecycle + lely_core_libraries yaml_cpp_vendor + canopen_interfaces + Boost ) -add_library(master_node +add_library(node_canopen_master SHARED + src/node_interfaces/node_canopen_master.cpp + src/master_error.cpp src/master_node.cpp - src/lely_master_bridge.cpp - ) - -target_compile_features(master_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(master_node PUBLIC - $ - $) -ament_target_dependencies( - master_node - "rclcpp" - "rclcpp_components" - "canopen_interfaces" - "lely_core_libraries" - "yaml_cpp_vendor" - "rclcpp_lifecycle" - "lifecycle_msgs" ) -rclcpp_components_register_nodes(master_node "ros2_canopen::MasterNode") -set(node_plugins "${node_plugins}ros2_canopen::MasterNode;$\n") - - -add_library(lifecycle_master_node - SHARED - src/lifecycle_master_node.cpp - src/lely_master_bridge.cpp - ) - -target_compile_features(lifecycle_master_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(lifecycle_master_node PUBLIC +target_compile_features(node_canopen_master PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(node_canopen_master PUBLIC -fPIC -Wl,--no-undefined) +target_include_directories(node_canopen_master PUBLIC $ - $) -ament_target_dependencies( - lifecycle_master_node - "rclcpp" - "rclcpp_components" - "canopen_interfaces" - "lely_core_libraries" - "yaml_cpp_vendor" - "rclcpp_lifecycle" - "lifecycle_msgs" + $ + ) +ament_target_dependencies(node_canopen_master + rclcpp + rclcpp_lifecycle + lely_core_libraries + yaml_cpp_vendor + canopen_interfaces + Boost ) -rclcpp_components_register_nodes(lifecycle_master_node "ros2_canopen::LifecycleMasterNode") -set(node_plugins "${node_plugins}ros2_canopen::LifecycleMasterNode;$\n") -# device manager node -add_library(lifecycle_device_manager_node +add_library(device_container SHARED - src/lifecycle_device_manager_node.cpp - ) - -target_compile_features(lifecycle_device_manager_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(lifecycle_device_manager_node PUBLIC - $ - $) - -target_link_libraries(lifecycle_device_manager_node - configuration_manager) - -ament_target_dependencies( - lifecycle_device_manager_node - "rclcpp" - "rclcpp_components" - "canopen_interfaces" - "rclcpp_lifecycle" - "lifecycle_msgs" - "std_srvs" + src/device_container.cpp + src/configuration_manager.cpp + src/device_container_error.cpp + src/lifecycle_manager.cpp ) -rclcpp_components_register_nodes(lifecycle_device_manager_node "ros2_canopen::LifecycleDeviceManagerNode") -set(node_plugins "${node_plugins}ros2_canopen::LifecycleDeviceManagerNode;$\n") - - -# device container node -add_executable(lifecycle_device_container_node - src/lifecycle_device_container_node.cpp +target_compile_features(device_container PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(device_container PUBLIC -fPIC -Wl,--no-undefined) +target_include_directories(device_container PUBLIC + $ + $ ) -target_link_libraries(lifecycle_device_container_node - configuration_manager - lifecycle_device_manager_node - lifecycle_master_node -) -ament_target_dependencies(lifecycle_device_container_node - ${dependencies} +ament_target_dependencies(device_container + rclcpp + rclcpp_lifecycle + lely_core_libraries + yaml_cpp_vendor + canopen_interfaces + rclcpp_components ) - -target_include_directories(lifecycle_device_container_node PUBLIC - "$" - "$" - ${rclcpp_INCLUDE_DIRS} +target_link_libraries(device_container + node_canopen_master + node_canopen_driver ) -# device container node -add_executable(device_container_node +add_executable(device_container_node src/device_container_node.cpp ) target_link_libraries(device_container_node - configuration_manager - lifecycle_master_node + device_container + node_canopen_master + node_canopen_driver ) -ament_target_dependencies(device_container_node +ament_target_dependencies(device_container_node ${dependencies} ) - target_include_directories(device_container_node PUBLIC "$" "$" @@ -174,62 +132,28 @@ install( ) install( - TARGETS configuration_manager - EXPORT export_configuration_manager - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - TARGETS master_node - EXPORT export_master_node - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch/ ) install( - TARGETS lifecycle_master_node - EXPORT export_lifecycle_master_node + TARGETS node_canopen_driver node_canopen_master device_container + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install( - TARGETS lifecycle_device_manager_node - EXPORT export_lifecycle_device_manager_node - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -install(TARGETS lifecycle_device_container_node - DESTINATION lib/${PROJECT_NAME}) install(TARGETS device_container_node - DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY - launch/ - DESTINATION share/${PROJECT_NAME}/launch/ + DESTINATION lib/${PROJECT_NAME} ) - - - if(BUILD_TESTING) - #find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - #ament_lint_auto_find_test_dependencies() - #find_package(launch_testing_ament_cmake) - #add_launch_test(tests/test_device_up.py) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + add_subdirectory(test) endif() ament_export_include_directories( @@ -237,22 +161,17 @@ ament_export_include_directories( ) ament_export_libraries( - configuration_manager - master_node - lifecycle_master_node - lifecycle_device_manager_node + node_canopen_driver + node_canopen_master ) + ament_export_targets( - export_configuration_manager - export_master_node - export_lifecycle_master_node - export_lifecycle_device_manager_node + export_${PROJECT_NAME} ) ament_export_dependencies( - canopen_interfaces - lely_core_libraries - yaml_cpp_vendor + ${dependencies} ) -ament_package() +# https://github.com/ament/ament_cmake/blob/e78ed7e084489c2a48cb91dec9da92c13e9653c9/ament_cmake_core/cmake/core/ament_package.cmake#L24-L32 +ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/canopen_core/ConfigExtras.cmake b/canopen_core/ConfigExtras.cmake new file mode 100644 index 00000000..01e6b9a1 --- /dev/null +++ b/canopen_core/ConfigExtras.cmake @@ -0,0 +1 @@ +find_package(Boost REQUIRED system thread) diff --git a/canopen_core/include/canopen_core/configuration_manager.hpp b/canopen_core/include/canopen_core/configuration_manager.hpp index 1ed8e3b9..95928e92 100644 --- a/canopen_core/include/canopen_core/configuration_manager.hpp +++ b/canopen_core/include/canopen_core/configuration_manager.hpp @@ -16,75 +16,96 @@ #ifndef CONFIGURATION_MANAGER_HPP #define CONFIGURATION_MANAGER_HPP -#include #include #include -#include #include +#include +#include +#include #include "yaml-cpp/yaml.h" namespace ros2_canopen { - /** - * @brief Manager for Bus Configuration. - * - * The Bus configuration Manager stores the YAML bus configuration and - * enables reading configuration entries. The configuration manager is passed - * to all ros2_canopen master and slave drivers to enable reading driver specific - * configuration parameters from the YAML configuration file. - * - */ - class ConfigurationManager - { - private: - std::string file_; ///< Stores the configuration file name - YAML::Node root_; ///< Stores YAML root node - std::map devices_; ///< Stores all configuration per device +/** + * @brief Manager for Bus Configuration. + * + * The Bus configuration Manager stores the YAML bus configuration and + * enables reading configuration entries. The configuration manager is passed + * to all ros2_canopen master and slave drivers to enable reading driver specific + * configuration parameters from the YAML configuration file. + * + */ +class ConfigurationManager +{ +private: + std::string file_; ///< Stores the configuration file name + YAML::Node root_; ///< Stores YAML root node + std::map devices_; ///< Stores all configuration per device - public: - ConfigurationManager(std::string &file) : file_(file) - { - root_ = YAML::LoadFile(file_.c_str()); - } +public: + ConfigurationManager(std::string & file) : file_(file) { root_ = YAML::LoadFile(file_.c_str()); } - /** - * @brief Gets a configuration entry for a specific device - * - * @tparam T Datatype of the retrieved object - * @param device_name Device name - * @param entry_name Entry name - * @return std::optional Return value, can be empty. - */ - template - std::optional get_entry(std::string device_name, std::string entry_name) - { - try - { - auto config = devices_.at(device_name); - return std::optional(config[entry_name.c_str()].as()); - } - catch (const std::exception &e) - { - std::cerr << e.what() << '\n'; - } + /** + * @brief Gets a configuration entry for a specific device + * + * @tparam T Datatype of the retrieved object + * @param device_name Device name + * @param entry_name Entry name + * @return std::optional Return value, can be empty. + */ + template + std::optional get_entry(std::string device_name, std::string entry_name) + { + try + { + auto config = devices_.at(device_name); + return std::optional(config[entry_name.c_str()].as()); + } + catch (const std::exception & e) + { + RCLCPP_INFO( + rclcpp::get_logger("yaml-cpp"), "Failed to load entry \"%s\" for device \"%s\" ", + entry_name.c_str(), device_name.c_str()); + } + + return std::nullopt; + } - return std::nullopt; - } + /** + * @brief Dump device string + * + * @param device_name + * @return std::string + */ + std::string dump_device(std::string device_name) + { + std::string result; + try + { + auto config = devices_.at(device_name); + result = YAML::Dump(config); + } + catch (const std::exception & e) + { + std::cerr << e.what() << '\n'; + } + return result; + } - /** - * @brief Initialises the configuration. - * - */ - void init_config(); + /** + * @brief Initialises the configuration. + * + */ + void init_config(); - /** - * @brief Returns all device names - * - * @param devices List with names of all devices - * @return uint32_t Number of devices discovered - */ - uint32_t get_all_devices(std::vector &devices); - }; -} + /** + * @brief Returns all device names + * + * @param devices List with names of all devices + * @return uint32_t Number of devices discovered + */ + uint32_t get_all_devices(std::vector & devices); +}; +} // namespace ros2_canopen -#endif \ No newline at end of file +#endif diff --git a/canopen_core/include/canopen_core/device.hpp b/canopen_core/include/canopen_core/device.hpp deleted file mode 100644 index 123649f9..00000000 --- a/canopen_core/include/canopen_core/device.hpp +++ /dev/null @@ -1,367 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef DEVICE_HPP_ -#define DEVICE_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "canopen_interfaces/srv/co_node.hpp" -#include "canopen_core/configuration_manager.hpp" - -using namespace lely; - -namespace ros2_canopen -{ - - class DriverInterface : public rclcpp::Node - { - public: - DriverInterface(const std::string &node_name, - const rclcpp::NodeOptions &node_options = rclcpp::NodeOptions()) : rclcpp::Node(node_name, node_options) {} - - virtual void init(ev::Executor &exec, - canopen::AsyncMaster &master, - uint8_t node_id, - std::shared_ptr config) noexcept = 0; - - virtual void remove(ev::Executor &exec, - canopen::AsyncMaster &master, - uint8_t node_id) noexcept = 0; - }; - - // Base class for driver plugin - // Pluginlib API does allows only default constructors - class LifecycleDriverInterface : public rclcpp_lifecycle::LifecycleNode - { - public: - /** - * @brief Construct a new LifecycleDriverInterface object - * - * @param [in] node_name - * @param [in] node_options - */ - LifecycleDriverInterface(const std::string &node_name, - const rclcpp::NodeOptions &node_options = rclcpp::NodeOptions()) - : rclcpp_lifecycle::LifecycleNode(node_name, node_options) {} - - /** - * @brief Initializer for the driver - * - * Initializes the driver, adds it to the CANopen Master. - * This function needs to be executed inside the masters - * event loop or the masters thread! - * - * @param [in] exec The executor to be used for the driver - * @param [in] master The master the driver should be added to - * @param [in] node_id The nodeid of the device the driver commands - */ - virtual void init_from_master(std::shared_ptr exec, - std::shared_ptr master, - std::shared_ptr config) - { - RCLCPP_DEBUG(this->get_logger(), "init_from_master_start"); - this->exec_ = exec; - this->master_ = master; - this->config_ = config; - this->initialised_ = true; - RCLCPP_DEBUG(this->get_logger(), "init_from_master_end"); - } - - virtual void init() - { - RCLCPP_DEBUG(this->get_logger(), "init_start"); - this->activated_.store(false); - register_ros_interface(); - RCLCPP_DEBUG(this->get_logger(), "init_end"); - } - - virtual bool add() = 0; - virtual bool remove() = 0; - - /** - * @brief Start Threads - * - * Used during activate transition to start threads that run tasks in - * active state. - * - */ - virtual void start_threads() - { - } - /** - * @brief Join Threads - * - * Used during deactivate transition to join threads that run tasks in - * active state. - * - */ - virtual void join_threads() - { - } - - virtual void update_parameters() - { - RCLCPP_DEBUG(this->get_logger(), "update_parameters_start"); - int millis; - this->get_parameter("container_name", container_name_); - this->get_parameter("non_transmit_timeout", millis); - this->get_parameter("node_id", this->node_id_); - this->non_transmit_timeout_ = std::chrono::milliseconds(millis); - RCLCPP_DEBUG(this->get_logger(), "update_parameters_start"); - } - - virtual void read_config() - { - } - - /** - * @brief Registers ROS Interface - * - * Registers the ros interface (services, topics) - * - */ - virtual void register_ros_interface() - { - RCLCPP_DEBUG(this->get_logger(), "register_ros_interface_start"); - client_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - timer_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - this->declare_parameter("container_name", ""); - this->declare_parameter("node_id", 0); - this->declare_parameter("non_transmit_timeout", 100); - RCLCPP_DEBUG(this->get_logger(), "register_ros_interface_end"); - } - - /** - * @brief Registers ROS Timers - * - * Registers ROS timers that do things during active state. - * - */ - virtual void - start_timers() - { - } - - /** - * @brief Registers ROS Timers - * - * Registers ROS timers that do things during active state. - * - */ - virtual void stop_timers() - { - } - - bool demand_init_from_master() - { - RCLCPP_DEBUG(this->get_logger(), "demand_init_from_master_start"); - std::string init_service_name = container_name_ + "/init_driver"; - auto demand_init_from_master_client = - this->create_client( - init_service_name, - rmw_qos_profile_services_default, - client_cbg_); - - while (!demand_init_from_master_client->wait_for_service(non_transmit_timeout_)) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for init_driver service. Exiting."); - return false; - } - RCLCPP_INFO(this->get_logger(), "init_driver service not available, waiting again..."); - } - auto request = std::make_shared(); - request->nodeid = node_id_; - - auto future_result = demand_init_from_master_client->async_send_request(request); - - auto future_status = future_result.wait_for(non_transmit_timeout_); - RCLCPP_DEBUG(this->get_logger(), "demand_init_from_master_end"); - if (future_status == std::future_status::ready) - { - try - { - return future_result.get()->success; - } - catch (...) - { - return false; - } - } - return false; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &state) - { - this->activated_.store(false); - this->update_parameters(); - if (!this->demand_init_from_master()) - { - RCLCPP_ERROR(this->get_logger(), "Demand init from master service call failed."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - if (!this->initialised_) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - read_config(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &state) - { - this->activated_.store(true); - if (!this->add()) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - this->start_threads(); - this->start_timers(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &state) - { - this->activated_.store(false); - this->stop_timers(); - this->join_threads(); - if (!this->remove()) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &state) - { - this->activated_.store(false); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &state) - { - this->activated_.store(false); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - protected: - // Storing ros2 canopen objects - std::shared_ptr exec_; - std::shared_ptr master_; - std::shared_ptr config_; - rclcpp::Client::SharedPtr demand_init_from_master_client_; - - // Storing parameter values - uint8_t node_id_; - std::chrono::milliseconds non_transmit_timeout_; - std::string container_name_; - - // Callback groups - rclcpp::CallbackGroup::SharedPtr client_cbg_; - rclcpp::CallbackGroup::SharedPtr timer_cbg_; - - // Synchronisation - std::atomic activated_; - bool initialised_; - }; - - class MasterInterface : public rclcpp::Node - { - protected: - std::string dcf_txt_; - std::string dcf_bin_; - std::string can_interface_name_; - uint8_t node_id_; - std::shared_ptr config_; - - public: - MasterInterface( - const std::string &node_name, - const rclcpp::NodeOptions &node_options) : rclcpp::Node(node_name, node_options) - { - } - virtual void init( - std::string dcf_txt, - std::string dcf_bin, - std::string can_interface_name, - uint8_t nodeid, - std::shared_ptr config) - { - dcf_txt_ = dcf_txt; - dcf_bin_ = dcf_bin; - can_interface_name_ = can_interface_name; - node_id_ = nodeid; - config_ = config; - } - virtual void add_driver(std::shared_ptr, uint8_t node_id) = 0; - virtual void remove_driver(std::shared_ptr, uint8_t node_id) = 0; - }; - - class LifecycleMasterInterface : public rclcpp_lifecycle::LifecycleNode - { - protected: - std::string dcf_txt_; - std::string dcf_bin_; - std::string can_interface_name_; - uint8_t node_id_; - std::shared_ptr config_; - - public: - /** - * @brief Construct a new Master Interface object - * - * @param node_name - * @param node_options - * @param dcf_txt - * @param dcf_bin - * @param can_interface_name - * @param nodeid - */ - LifecycleMasterInterface( - const std::string &node_name, - const rclcpp::NodeOptions &node_options) : rclcpp_lifecycle::LifecycleNode(node_name, node_options) - { - } - virtual void init() - { - } - - /** - * @brief Initialises a driver - * - * @param node_id - */ - virtual void init_driver(std::shared_ptr, uint8_t node_id) = 0; - }; - -} // end ros2_canopen namespace - -#endif // DEVICE_HPP_ \ No newline at end of file diff --git a/canopen_core/include/canopen_core/device_container.hpp b/canopen_core/include/canopen_core/device_container.hpp new file mode 100644 index 00000000..da4042a8 --- /dev/null +++ b/canopen_core/include/canopen_core/device_container.hpp @@ -0,0 +1,347 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIFECYCLE_DEVICE_CONTAINER_NODE_HPP +#define LIFECYCLE_DEVICE_CONTAINER_NODE_HPP + +#include +#include +#include +#include +#include +#include "canopen_core/configuration_manager.hpp" +#include "canopen_core/driver_node.hpp" +#include "canopen_core/lifecycle_manager.hpp" +#include "canopen_core/master_node.hpp" +#include "canopen_interfaces/srv/co_node.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" + +using namespace std::chrono_literals; + +namespace ros2_canopen +{ +/** + * @brief Lifecycle Device Container for CANopen + * + * This class provides the functionality for loading a + * the CANopen Master and CANopen Device Drivers based on the + * Configuration Files. Configuration files need to be passed in + * as parameters. + * + */ +class DeviceContainer : public rclcpp_components::ComponentManager +{ +public: + /** + * @brief Construct a new Lifecycle Device Container Node object + * + * @param [in] executor The executor to add loaded master and devices to. + * @param [in] node_name The name of the node + * @param [in] node_options Passed to the device_container node + */ + DeviceContainer( + std::weak_ptr executor = + std::weak_ptr(), + std::string node_name = "device_container", + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : rclcpp_components::ComponentManager(executor, node_name, node_options) + { + executor_ = executor; + this->declare_parameter("can_interface_name", ""); + this->declare_parameter("master_config", ""); + this->declare_parameter("bus_config", ""); + this->declare_parameter("master_bin", ""); + client_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + init_driver_service_ = this->create_service( + "~/init_driver", + std::bind( + &DeviceContainer::on_init_driver, this, std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default, client_cbg_); + + this->loadNode_srv_.reset(); + this->unloadNode_srv_.reset(); + lifecycle_operation_ = false; + } + + /** + * @brief Executes the initialisation + * + * This will read nodes parameters and initialize + * the configuration defined in the parameters. It + * will also start loading the components. + * + * @return true + * @return false + */ + void init(); + + /** + * @brief Executes the initialisation + * + * Same as init() only that ros parameters are not used. + * + * @param can_interface_name + * @param master_config + * @param bus_config + * @param master_bin + */ + void init( + const std::string & can_interface_name, const std::string & master_config, + const std::string & bus_config, const std::string & master_bin = ""); + + virtual void configure(); + /** + * @brief Loads drivers from configuration + * + * @return true + * @return false + */ + virtual bool load_drivers(); + /** + * @brief Loads master from configuration + * + * @return true + * @return false + */ + virtual bool load_master(); + /** + * @brief Loads the device manager + * + * @return true + * @return false + */ + virtual bool load_manager(); + + /** + * @brief Load a component + * + * @param [in] package_name Name of the package + * @param [in] driver_name Name of the driver class to load + * @param [in] node_id CANopen node id of the target device + * @param [in] node_name Node name of the ROS node + * @return true + * @return false + */ + virtual bool load_component( + std::string & package_name, std::string & driver_name, uint16_t node_id, + std::string & node_name, std::vector & params); + + /** + * @brief Shutdown all devices. + * + * This function will shutdown all devices that were created. + * The function calls the shutdown function of each created + * device. + * + */ + virtual void shutdown() + { + for (auto it = registered_drivers_.begin(); it != registered_drivers_.end(); ++it) + { + try + { + it->second->shutdown(); + } + catch (const std::exception & e) + { + std::cerr << e.what() << '\n'; + } + } + try + { + can_master_->shutdown(); + } + catch (const std::exception & e) + { + std::cerr << e.what() << '\n'; + } + } + /** + * @brief Callback for the listing service + * + * @param request_header + * @param request + * @param response + */ + virtual void on_list_nodes( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) override; + + /** + * @brief Get the registered drivers object + * + * This function will return a map of all driver objects that were created. + * The returned map can be queried by node id. + * + * @return std::map> + */ + virtual std::map> get_registered_drivers() + { + return registered_drivers_; + } + + /** + * @brief Get the number of registered drivers + * + * @return size_t + */ + virtual size_t count_drivers() { return registered_drivers_.size(); } + + /** + * @brief Get node ids of all drivers with type + * + * This function gets the ids of all drivers that have the passed + * in type. The type must be the fully qualified class name of the + * driver as string. + * + * @param [in] type + * @return std::vector + */ + std::vector get_ids_of_drivers_with_type(std::string type) + { + std::vector devices; + std::vector ids; + uint32_t count = this->config_->get_all_devices(devices); + + for (auto it = devices.begin(); it != devices.end(); it++) + { + auto driver_name = config_->get_entry(*it, "driver"); + if (driver_name.has_value()) + { + std::string name = driver_name.value(); + if (name.compare(type) == 0) + { + auto node_id = config_->get_entry(*it, "node_id"); + ids.push_back(node_id.value()); + } + } + } + return ids; + } + /** + * @brief Get the type of driver by node id + * + * This function will return the type of the driver that is registered + * for the passed node ids. + * + * @param [in] id + * @return std::string + */ + std::string get_driver_type(uint16_t id) + { + std::vector devices; + uint32_t count = this->config_->get_all_devices(devices); + for (auto it = devices.begin(); it != devices.end(); it++) + { + auto node_id = config_->get_entry(*it, "node_id"); + if (node_id.has_value() && node_id.value() == id) + { + auto driver_name = config_->get_entry(*it, "driver"); + return driver_name.value(); + } + } + } + +protected: + // Components + std::map> + registered_drivers_; ///< Map of drivers registered in busconfiguration. Name is key. + std::shared_ptr + can_master_; ///< Pointer to can master instance + uint16_t can_master_id_; + std::unique_ptr lifecycle_manager_; + + // Configuration + std::shared_ptr + config_; ///< Pointer to configuration manager instance + std::string dcf_txt_; ///< Cached value of .dcf file parameter + std::string bus_config_; ///< Cached value of bus.yml file parameter + std::string dcf_bin_; ///< Cached value of .bin file parameter + std::string can_interface_name_; ///< Cached value of can interface name + bool lifecycle_operation_; + + // ROS Objects + std::weak_ptr executor_; ///< Pointer to ros executor instance + rclcpp::Service::SharedPtr + init_driver_service_; ///< Service object for init_driver service + rclcpp::CallbackGroup::SharedPtr client_cbg_; ///< Callback group for services + + /** + * @brief Set the ROS executor object + * + * @param [in] executor Pointer to the Executor + */ + void set_executor(const std::weak_ptr executor); + + /** + * @brief Initialises a driver + * + * This function needs to be called before a driver can be added + * to the CANopen Master Event Loop. It sets the master and executor, + * the driver will be added to. + * + * @param node_id CANopen node id of the target device + * + * @return true + * @return false + */ + bool init_driver(uint16_t node_id); + + /** + * @brief Callback for init driver service + * + * @param request + * @param response + */ + void on_init_driver( + const canopen_interfaces::srv::CONode::Request::SharedPtr request, + canopen_interfaces::srv::CONode::Response::SharedPtr response) + { + response->success = init_driver(request->nodeid); + } + + /** + * @brief Returns a list of components + * + * @return std::map + */ + std::map list_components(); + + /** + * @brief Add node to executor + * + * @param [in] node_interface NodeBaseInterface of the node that should be added to the executor. + */ + virtual void add_node_to_executor( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface) + { + if (auto exec = executor_.lock()) + { + RCLCPP_INFO( + this->get_logger(), "Added %s to executor", node_interface->get_fully_qualified_name()); + exec->add_node(node_interface, true); + } + else + { + RCLCPP_ERROR( + this->get_logger(), "Failed to add component %s", + node_interface->get_fully_qualified_name()); + } + } +}; + +} // namespace ros2_canopen +#endif // LIFECYCLE_DEVICE_CONTAINER_NODE_HPP diff --git a/canopen_core/include/canopen_core/device_container_error.hpp b/canopen_core/include/canopen_core/device_container_error.hpp new file mode 100644 index 00000000..d43a66ba --- /dev/null +++ b/canopen_core/include/canopen_core/device_container_error.hpp @@ -0,0 +1,30 @@ +#ifndef DEVICE_CONTAINER_ERROR_HPP_ +#define DEVICE_CONTAINER_ERROR_HPP_ + +#include +#include + +namespace ros2_canopen +{ + +/** + * @brief Device Container Exception + * + * This exception is used, when the device container + * fails. + * + */ +class DeviceContainerException : public std::exception +{ +private: + std::string what_; + +public: + DeviceContainerException(std::string what) { what_ = what; } + + char * what(); +}; + +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/device_container_node.hpp b/canopen_core/include/canopen_core/device_container_node.hpp deleted file mode 100644 index 9d67b80e..00000000 --- a/canopen_core/include/canopen_core/device_container_node.hpp +++ /dev/null @@ -1,144 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef DEVICE_CONTAINER_NODE_HPP -#define DEVICE_CONTAINER_NODE_HPP - -#include -#include -#include -#include -#include "canopen_core/master_node.hpp" -#include "canopen_core/configuration_manager.hpp" - -#include "device.hpp" - -class DeviceContainerNode : public rclcpp_components::ComponentManager { -public: - DeviceContainerNode( - std::weak_ptr executor = - std::weak_ptr(), - std::string node_name = "device_manager", - const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() - .start_parameter_services(false) - .start_parameter_event_publisher(false)) : - rclcpp_components::ComponentManager(executor, node_name, node_options) { - - executor_ = executor; - this->declare_parameter("can_interface_name", ""); - this->declare_parameter("master_config", ""); - this->declare_parameter("bus_config", ""); - this->declare_parameter("master_bin", ""); - } - - bool init(); - - virtual void - on_load_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - virtual void - OnLoadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) override - { - on_load_node(request_header, request, response); - } - - virtual void - on_unload_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - virtual void - OnUnloadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) override - { - on_unload_node(request_header, request, response); - } - - virtual void - on_list_nodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - virtual void - OnListNodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) override - { - on_list_nodes(request_header, request, response); - } - - -private: - std::map> registered_drivers_; - std::map> active_drivers_; - std::shared_ptr can_master_; - std::shared_ptr exec_; - std::weak_ptr executor_; - std::map node_wrappers_; - std::shared_ptr config_; - std::string dcf_txt_; - std::string bus_config_; - std::string dcf_bin_; - std::string can_interface_name_; - - void set_executor(const std::weak_ptr executor); - void add_node_to_executor(const std::string &driver_name, const uint8_t node_id, const std::string &node_name); - void remove_node_from_executor(const std::string &driver_name, const uint8_t node_id, const std::string &node_name); - - /** - * @brief Adds driver to master - * - * This function needs to be called to add the driver to the - * CANopen master loop, so that it has access to CANopen events - * and can send messages. - * - * @param driver_name Name of the driver - * @param node_id CANopen Id of the device the driver targets - * - * - */ - void add_driver_to_master(std::string driver_name, uint8_t node_id); - - /** - * @brief Removes driver from master - * - * This function removes the driver with the specified id from - * the CANopen master loop. This needs to be done when unloading - * a driver. This function should be called before removing the driver - * from the ros2 executor. - * - * @param node_id CANopen Id of the device the driver targets - */ - void remove_driver_from_master(uint8_t node_id); - - bool load_component(std::string& package_name, std::string& driver_name, uint8_t node_id, std::string& node_name); - std::map list_components(); - - bool init_devices_from_config(); - bool add_master(uint8_t node_id); -}; - -#endif // DEVICE_CONTAINER_NODE_HPP \ No newline at end of file diff --git a/canopen_core/include/canopen_core/driver_error.hpp b/canopen_core/include/canopen_core/driver_error.hpp new file mode 100644 index 00000000..657b6c0a --- /dev/null +++ b/canopen_core/include/canopen_core/driver_error.hpp @@ -0,0 +1,29 @@ +#ifndef DRIVER_ERROR_HPP_ +#define DRIVER_ERROR_HPP_ + +#include +#include + +namespace ros2_canopen +{ +/** + * @brief Driver Exception + * + * This exception is used, when a driver + * fails. + * + */ +class DriverException : public std::exception +{ +private: + std::string what_; + +public: + DriverException(std::string what) { what_ = what; } + + char * what(); +}; + +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/driver_node.hpp b/canopen_core/include/canopen_core/driver_node.hpp new file mode 100644 index 00000000..c7f6b4ab --- /dev/null +++ b/canopen_core/include/canopen_core/driver_node.hpp @@ -0,0 +1,314 @@ +#ifndef DRIVER_NODE_HPP_ +#define DRIVER_NODE_HPP_ + +#include +#include "canopen_core/node_interfaces/node_canopen_driver.hpp" + +namespace ros2_canopen +{ +/** + * @brief Canopen Driver Interface + * + * This provides an interface that all driver nodes that are loaded + * by ros2_canopen::DeviceContainer need to implement. + * + */ +class CanopenDriverInterface +{ +public: + /** + * @brief Initialise the driver + * + * This function will initialise the drivers functionalities. + * It will be called by the device_container when the node has + * been added to the executor. + */ + virtual void init() = 0; + + /** + * @brief Set the master object + * + * This function will set the Canopen Master Objects that are + * necessary for the driver to be instantiated. It will be called + * by the device container when the init_driver service is invoked. + * + * @param exec + * @param master + */ + virtual void set_master( + std::shared_ptr exec, + std::shared_ptr master) = 0; + + /** + * @brief Get the node base interface object + * + * This function shall return an rclcpp::node_interfaces::NodeBaseInterface::SharedPtr. + * The pointer will be used to add the driver node to the executor. + * + * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + */ + virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() = 0; + + /** + * @brief Shutdown the driver + * + * This function will shutdown the driver and will especially + * join all threads to enable a clean shutdown. + * + */ + virtual void shutdown() = 0; + + /** + * @brief Check whether this is a LifecycleNode + * + * This function provides runtime information on whether the driver + * is a lifecycle driver or not. + * + * @return true + * @return false + */ + virtual bool is_lifecycle() = 0; + + /** + * @brief Get the node canopen driver interface object + * + * This function gives access to the underlying NodeCanopenDriverInterface. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr + get_node_canopen_driver_interface() = 0; +}; + +/** + * @brief Canopen Driver + * + * This provides a class, that driver nodes that are based on rclcpp::Node + * should be derived of. This class implements the ros2_canopen::CanopenDriverInterface. + * + */ +class CanopenDriver : public CanopenDriverInterface, public rclcpp::Node +{ +public: + std::shared_ptr node_canopen_driver_; + explicit CanopenDriver(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : rclcpp::Node("canopen_driver", node_options) + { + node_canopen_driver_ = std::make_shared>(this); + } + + /** + * @brief Initialise the driver + * + * This function will activate the driver using the instantiated + * node_canopen_driver_. It will call the init, configure, demand_set_master and activate + * of the NodeCanopenDriverInterface. If the function finishes without exception, + * the driver is activated and ready for use. + * + */ + virtual void init() override; + + /** + * @brief Set the master object + * + * This function will set the Canopen Master Objects that are + * necessary for the driver to be instantiated. It will be called + * by the device container when the init_driver service is invoked. + * + * @param exec + * @param master + */ + virtual void set_master( + std::shared_ptr exec, + std::shared_ptr master) override; + + /** + * @brief Get the node base interface object + * + * This function shall return an rclcpp::node_interfaces::NodeBaseInterface::SharedPtr. + * The pointer will be used to add the driver node to the executor. + * + * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + */ + virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override + { + return rclcpp::Node::get_node_base_interface(); + } + + /** + * @brief Shutdown the driver + * + * This function will shutdown the driver by calling the + * shutdown() function of node_canopen_driver_. + * + */ + virtual void shutdown() override; + + /** + * @brief Check whether this is a LifecycleNode + * + * @return false + */ + virtual bool is_lifecycle() override { return false; } + + /** + * @brief Get the node canopen driver interface object + * + * This function gives access to the underlying NodeCanopenDriverInterface. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr + get_node_canopen_driver_interface() + { + return node_canopen_driver_; + } +}; + +/** + * @brief Lifecycle Canopen Driver + * + * This provides a class, that driver nodes that are based on rclcpp_lifecycle::LifecycleNode + * should be derived of. This class implements the ros2_canopen::CanopenDriverInterface. + * + */ +class LifecycleCanopenDriver : public CanopenDriverInterface, public rclcpp_lifecycle::LifecycleNode +{ +protected: + std::shared_ptr node_canopen_driver_; + +public: + explicit LifecycleCanopenDriver(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : rclcpp_lifecycle::LifecycleNode("lifecycle_canopen_driver", node_options) + { + node_canopen_driver_ = + std::make_shared>(this); + } + /** + * @brief Initialise the driver + * + * This function will activate the driver using the instantiated + * node_canopen_driver_. It will call init() + * of the NodeCanopenDriverInterface. If the function finishes without exception, + * the driver can be configured. + * + */ + virtual void init() override; + + /** + * @brief Set the master object + * + * This function will set the Canopen Master Objects that are + * necessary for the driver to be instantiated. It will be called + * by the device container when the init_driver service is invoked. + * + * @param exec + * @param master + */ + virtual void set_master( + std::shared_ptr exec, + std::shared_ptr master) override; + + /** + * @brief Configure Callback + * + * This function will call configure() and demand_set_master() of the + * node_canopen_driver_. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & state); + /** + * @brief Activate Callback + * + * This function will call activate() of the + * node_canopen_driver_. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & state); + + /** + * @brief Deactivate Callback + * + * This function will call deactivate() of the + * node_canopen_driver_. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & state); + + /** + * @brief Deactivate Callback + * + * This function will call cleanup() of the + * node_canopen_driver_. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & state); + + /** + * @brief Deactivate Callback + * + * This function will call shutdown() of the + * node_canopen_driver_. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state); + + /** + * @brief Get the node base interface object + * + * This function shall return an rclcpp::node_interfaces::NodeBaseInterface::SharedPtr. + * The pointer will be used to add the driver node to the executor. + * + * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + */ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override + { + return rclcpp_lifecycle::LifecycleNode::get_node_base_interface(); + } + + /** + * @brief Shutdown the driver + * + * This function will shutdown the driver by calling the + * shutdown() function of node_canopen_driver_. + * + */ + virtual void shutdown() override; + /** + * @brief Check whether this is a LifecycleNode + * + * @return true + */ + virtual bool is_lifecycle() override { return true; } + /** + * @brief Get the node canopen driver interface object + * + * This function gives access to the underlying NodeCanopenDriverInterface. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr + get_node_canopen_driver_interface() + { + return node_canopen_driver_; + } +}; + +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/exchange.hpp b/canopen_core/include/canopen_core/exchange.hpp index 550ab115..2632e224 100644 --- a/canopen_core/include/canopen_core/exchange.hpp +++ b/canopen_core/include/canopen_core/exchange.hpp @@ -1,6 +1,6 @@ // Copyright 2022 Harshavadan Deshpande // Christoph Hellmann Santos -// +// // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -15,14 +15,12 @@ #ifndef EXCHANGE_HPP #define EXCHANGE_HPP -namespace ros2_canopen { -enum CODataTypes +#include +#include +#include + +namespace ros2_canopen { - CODataUnkown = 0, - COData8 = 8, - COData16 = 16, - COData32 = 32 -}; struct COData { @@ -30,8 +28,127 @@ struct COData uint16_t index_; uint8_t subindex_; uint32_t data_; - CODataTypes type_; }; -} -#endif // EXCHANGE_HPP \ No newline at end of file +struct COEmcy +{ +public: + uint16_t eec; + uint8_t er; + uint8_t msef[5]; +}; + +/** + * @brief Thread Safe Queue for CANOpen Data Exchange + * @tparam T Type of the data to be exchanged + * @details This class is a wrapper around boost::lockfree::queue to provide thread safe data + * exchange between threads using the queue. + */ +template +class SafeQueue +{ +private: + std::size_t capacity_; + std::unique_ptr> queue_; + +public: + /** + * @brief Constructor for the SafeQueue + * @param capacity Capacity of the queue + */ + explicit SafeQueue(std::size_t capacity = 10) + : capacity_(capacity), queue_(new boost::lockfree::queue(capacity_)) + { + } + + /** + * @brief Push a value to the queue + * @param value Value to be pushed + */ + void push(T value) { queue_->push(std::move(value)); } + + /** + * @brief Try to pop a value from the queue + * @return Value if available, boost::none otherwise + */ + std::optional try_pop() + { + T value; + if (queue_->pop(value)) return std::optional(std::move(value)); + return std::optional(); + } + + /** + * @brief Try to pop a value from the queue + * @param value Value to be returned + */ + bool try_pop(T & value) + { + if (queue_->pop(value)) return true; + return false; + } + + /** + * @brief Wait for a value to be available in the queue + * @return Value if available, boost::none otherwise + */ + boost::optional wait_and_pop() + { + T value; + while (!queue_->pop(value)) boost::this_thread::yield(); + return value; + } + + /** + * @brief Wait for a value to be available in the queue for a given timeout + * @param value Value to be returned + */ + void wait_and_pop(T & value) + { + while (!queue_->pop(value)) boost::this_thread::yield(); + } + + /** + * @brief Wait for a value to be available in the queue for a given timeout + * @param timeout Timeout in milliseconds + * @return Value if available, boost::none otherwise + */ + boost::optional wait_and_pop_for(const std::chrono::milliseconds & timeout) + { + T value; + auto start_time = std::chrono::steady_clock::now(); + while (!queue_->pop(value)) + { + if ( + timeout != std::chrono::milliseconds::zero() && + std::chrono::steady_clock::now() - start_time >= timeout) + return boost::none; + boost::this_thread::yield(); + } + return value; + } + + /** + * @brief Wait for a value to be available in the queue for a given timeout + * @param timeout Timeout in milliseconds + * @param value Value to be returned + */ + bool wait_and_pop_for(const std::chrono::milliseconds & timeout, T & value) + { + auto start_time = std::chrono::steady_clock::now(); + while (!queue_->pop(value)) + { + if ( + timeout != std::chrono::milliseconds::zero() && + std::chrono::steady_clock::now() - start_time >= timeout) + return false; + boost::this_thread::yield(); + } + return true; + } + + bool empty() const { return queue_->empty(); } +}; +} // namespace ros2_canopen + +#endif // EXCHANGE_HPP diff --git a/canopen_core/include/canopen_core/lely_master_bridge.hpp b/canopen_core/include/canopen_core/lely_master_bridge.hpp deleted file mode 100644 index 59d781e4..00000000 --- a/canopen_core/include/canopen_core/lely_master_bridge.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LELY_MASTER_BRIDGE_HPP -#define LELY_MASTER_BRIDGE_HPP - -#include "lely/coapp/master.hpp" -#include -#include - -#include -#include -#include -#include -#include "canopen_core/exchange.hpp" - -namespace ros2_canopen -{ - /** - * @brief Creates services to pass values between Lely and ROS executor. - * - */ - class LelyMasterBridge : public lely::canopen::AsyncMaster - { - - std::shared_ptr> sdo_read_data_promise; - std::shared_ptr> sdo_write_data_promise; - std::promise nmt_promise; - std::mutex sdo_mutex; - bool running; - std::condition_variable sdo_cond; - - public: - LelyMasterBridge( - ev_exec_t *exec, - lely::io::TimerBase &timer, - lely::io::CanChannelBase &chan, - const std::string &dcf_txt, - const std::string &dcf_bin = "", - uint8_t id = (uint8_t)255U) : lely::canopen::AsyncMaster(exec, timer, chan, dcf_txt, dcf_bin, id) - { - } - - std::future async_write_sdo(uint8_t id, COData data); - std::future async_read_sdo(uint8_t id, COData data); - - /** - * @brief async_write_nmt - * - * @param id - * @param command - * @return std::future - * - * @todo Catch errors and transmit via future - */ - std::future async_write_nmt(uint8_t id, uint8_t command); - }; - -} // ros2_canopen - -#endif // LELY_MASTER_BRIDGE_HPP \ No newline at end of file diff --git a/canopen_core/include/canopen_core/lifecycle_device_container_node.hpp b/canopen_core/include/canopen_core/lifecycle_device_container_node.hpp deleted file mode 100644 index d9e3d8f3..00000000 --- a/canopen_core/include/canopen_core/lifecycle_device_container_node.hpp +++ /dev/null @@ -1,162 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LIFECYCLE_DEVICE_CONTAINER_NODE_HPP -#define LIFECYCLE_DEVICE_CONTAINER_NODE_HPP - -#include -#include -#include -#include -#include "canopen_core/lifecycle_master_node.hpp" -#include "canopen_core/configuration_manager.hpp" -#include "canopen_core/lifecycle_device_manager_node.hpp" -#include "canopen_interfaces/srv/co_node.hpp" - -#include "device.hpp" - -class LifecycleDeviceContainerNode : public rclcpp_components::ComponentManager -{ -public: - LifecycleDeviceContainerNode( - std::weak_ptr executor = - std::weak_ptr(), - std::string node_name = "lifecycle_device_container_node", - const rclcpp::NodeOptions &node_options = rclcpp::NodeOptions()) : rclcpp_components::ComponentManager(executor, node_name, node_options) - { - - executor_ = executor; - this->declare_parameter("can_interface_name", ""); - this->declare_parameter("master_config", ""); - this->declare_parameter("bus_config", ""); - this->declare_parameter("master_bin", ""); - - init_driver_service_ = this->create_service( - "~/init_driver", - std::bind( - &LifecycleDeviceContainerNode::on_init_driver, - this, - std::placeholders::_1, - std::placeholders::_2)); - } - - bool init(); - - virtual void - on_load_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - virtual void - OnLoadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) override - { - on_load_node(request_header, request, response); - } - - virtual void - on_unload_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - virtual void - OnUnloadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) override - { - on_unload_node(request_header, request, response); - } - - virtual void - on_list_nodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - virtual void - OnListNodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) override - { - on_list_nodes(request_header, request, response); - } - -private: - // Stores registered drivers as device_name, pair(node_id, driver_name) - std::map> registered_drivers_; - // Stores drivers that were succesfully added to executor as device_name, pair(node_id, driver_name) - std::map> active_drivers_; - // Stores componentes as node_id and wrapper_object - std::map node_wrappers_; - std::shared_ptr can_master_; - std::shared_ptr exec_; - std::weak_ptr executor_; - std::shared_ptr config_; - std::string dcf_txt_; - std::string bus_config_; - std::string dcf_bin_; - std::string can_interface_name_; - - rclcpp::Service::SharedPtr init_driver_service_; - rclcpp::Service::SharedPtr remove_node_master_service_; - bool init_device_manager(uint16_t node_id); - void set_executor(const std::weak_ptr executor); - void add_node_to_executor(const std::string &driver_name, const uint16_t node_id, const std::string &node_name); - void remove_node_from_executor(const std::string &driver_name, const uint16_t node_id, const std::string &node_name); - - /** - * @brief Adds driver to master - * - * This function needs to be called to add the driver to the - * CANopen master loop, so that it has access to CANopen events - * and can send messages. - * - * @param driver_name Name of the driver - * @param node_id CANopen Id of the device the driver targets - * - * - */ - bool init_driver(uint16_t node_id); - - void on_init_driver( - const canopen_interfaces::srv::CONode::Request::SharedPtr request, - canopen_interfaces::srv::CONode::Response::SharedPtr response) - { - response->success = init_driver(request->nodeid); - } - - bool load_component(std::string &package_name, std::string &driver_name, uint16_t node_id, std::string &node_name); - - /** - * @brief Returns a list of components - * - * @return std::map - */ - std::map list_components(); - - bool load_drivers_from_config(); - bool load_master_from_config(); - bool load_manager(); - - bool init_master(uint16_t node_id); -}; - -#endif // LIFECYCLE_DEVICE_CONTAINER_NODE_HPP \ No newline at end of file diff --git a/canopen_core/include/canopen_core/lifecycle_device_manager_node.hpp b/canopen_core/include/canopen_core/lifecycle_device_manager_node.hpp deleted file mode 100644 index 3e5930a4..00000000 --- a/canopen_core/include/canopen_core/lifecycle_device_manager_node.hpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef LIFECYCLE_DEVICE_MANAGER_NODE_HPP -#define LIFECYCLE_DEVICE_MANAGER_NODE_HPP - - -#include -#include -#include -#include - -#include "lifecycle_msgs/msg/state.hpp" -#include "lifecycle_msgs/msg/transition.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/srv/get_state.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "canopen_core/configuration_manager.hpp" -#include "canopen_interfaces/srv/co_node.hpp" - -using namespace std::chrono_literals; -/* -Start-up sequence --------------------------------------------------------------------- -master | unconfigured | x | - | inactive | | x | - | active | | x | x | x | x | x |... --------------------------------------------------------------------- -drv1 | unconfigured | x | x | x | - | inactive | | x | - | active | | x | x | x |... --------------------------------------------------------------------- -driv2 | unconfigured | x | x | x | x | x | - | inactive | | x | - | active | | x |... --------------------------------------------------------------------- -*/ -namespace ros2_canopen -{ - class LifecycleDeviceManagerNode : public rclcpp_lifecycle::LifecycleNode - { - public: - LifecycleDeviceManagerNode(const rclcpp::NodeOptions &node_options) - : rclcpp_lifecycle::LifecycleNode("LifecycleDeviceManagerNode", node_options) - { - this->declare_parameter("container_name", ""); - cbg_clients = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - } - - void init(std::shared_ptr config); - - protected: - rclcpp::CallbackGroup::SharedPtr cbg_clients; - std::shared_ptr config_; - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &state); - - // stores node_id and get_state client - std::map::SharedPtr> drivers_get_state_clients; - // stores node_id and change_state client - std::map::SharedPtr> drivers_change_state_clients; - // stores device_name and node_id - std::map device_names_to_ids; - - rclcpp::Client::SharedPtr add_driver_client_; - rclcpp::Client::SharedPtr remove_driver_client_; - - uint8_t master_id_; - std::string container_name_; - - private: - template - std::future_status - wait_for_result( - FutureT &future, - WaitTimeT time_to_wait) - { - auto end = std::chrono::steady_clock::now() + time_to_wait; - std::chrono::milliseconds wait_period(100); - std::future_status status = std::future_status::timeout; - do - { - auto now = std::chrono::steady_clock::now(); - auto time_left = end - now; - if (time_left <= std::chrono::seconds(0)) - { - break; - } - status = future.wait_for((time_left < wait_period) ? time_left : wait_period); - } while (rclcpp::ok() && status != std::future_status::ready); - return status; - } - - unsigned int - get_state(uint8_t node_id, std::chrono::seconds time_out = 3s); - - bool - change_state(uint8_t node_id, uint8_t transition, std::chrono::seconds time_out = 3s); - - /** - * @brief Brings up master and all drivers - * - * This funnnction brings up the CANopen master and all drivers that are defined - * in the configuration. The order of bringing up drivers is arbitrary. - * - * @return true - * @return false - */ - bool - bring_up_all(); - - /** - * @brief Brings down master and all drivers - * - * This funnnction brings up the CANopen master and all drivers that are defined - * in the configuration. First all drivers are brought down, then the master. - * - * @return true - * @return false - */ - bool - bring_down_all(); - - /** - * @brief Brings up master - * - * This function brings up the CANopen master. The master transitioned twice, - * first the configure transition is triggered. Once the transition is successfully - * finished, the activate transition is triggered. - * - * @return true - * @return false - */ - bool - bring_up_master(); - - /** - * @brief Bring down master - * - * This function brrignsdown the CANopen master. The master transitioned twice, - * first the deactivate transition is triggered. Once the transition is successfully - * finished, the cleanup transition is triggered. - * - * @return true - * @return false - */ - bool - bring_down_master(); - - /** - * @brief Brings up the drivers with specified node_id - * - * This function bringsup the CANopen driver for the device with the specified - * node_id. The driver is transitioned twice, - * first the configure transition is triggered. Once the transition is successfully - * finished, the activate transition is triggered. - * - * @param device_name - * @return true - * @return false - */ - bool - bring_up_driver(std::string device_name); - - /** - * @brief Brings down the driver with specified node_id - * - * This function brings down the CANopen driver for the device with the specified - * node_id. The driver is transitioned twice, - * first the deactivate transition is triggered. Once the transition is successfully - * finished, the cleanup transition is triggered. - * - * @param device_name - * @return true - * @return false - */ - bool - bring_down_driver(std::string device_name); - - /** - * @brief Load information from configuration - * - * This function loads the information about the CANopen Bus specified in - * the configuration file and creates the necessary ROS2 services and clients. - * - * @return true - * @return false - */ - bool - load_from_config(); - }; -} - -#endif \ No newline at end of file diff --git a/canopen_core/include/canopen_core/lifecycle_manager.hpp b/canopen_core/include/canopen_core/lifecycle_manager.hpp new file mode 100644 index 00000000..f82404d3 --- /dev/null +++ b/canopen_core/include/canopen_core/lifecycle_manager.hpp @@ -0,0 +1,289 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef LIFECYCLE_DEVICE_MANAGER_NODE_HPP +#define LIFECYCLE_DEVICE_MANAGER_NODE_HPP + +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +//#include "std_srvs/srv/trigger.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "canopen_core/configuration_manager.hpp" +#include "canopen_interfaces/srv/co_node.hpp" + +using namespace std::chrono_literals; +/* + +*/ +namespace ros2_canopen +{ +/** + * @brief Lifecycle Device Manager Node + * + * This class provides functionalities to coordinate the lifecycle + * of master and device drivers that are loaded into the executor. + * + * Start-up sequence + * -------------------------------------------------------------------- + * master | unconfigured | x | + * | inactive | | x | + * | active | | x | x | x | x | x |... + * -------------------------------------------------------------------- + * drv1 | unconfigured | x | x | x | + * | inactive | | x | + * | active | | x | x | x |... + * -------------------------------------------------------------------- + * driv2 | unconfigured | x | x | x | x | x | + * | inactive | | x | + * | active | | x |... + * -------------------------------------------------------------------- + * + * + * + */ +class LifecycleManager : public rclcpp_lifecycle::LifecycleNode +{ +public: + /** + * @brief Construct a new Lifecycle Device Manager Node object + * + * @param node_options + */ + LifecycleManager(const rclcpp::NodeOptions & node_options) + : rclcpp_lifecycle::LifecycleNode("lifecycle_manager", node_options) + { + this->declare_parameter("container_name", ""); + cbg_clients = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + } + + void init(std::shared_ptr config); + +protected: + rclcpp::CallbackGroup::SharedPtr cbg_clients; + std::shared_ptr config_; + + /** + * @brief Callback for the Configure Transition + * + * This will cause the device manager to load the configuration + * from the configuration file. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & state); + + /** + * @brief Callback for the Activate Transition + * + * This will bringup master and device drivers using the + * bring_up_all function. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & state); + + /** + * @brief Callback for the Deactivate Transition + * + * This will bring down master and device drivers using the + * bring_down_all function. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & state); + + /** + * @brief Callback for the Cleanup Transition + * + * Does nothing. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & state); + + /** + * @brief Callback for the Shutdown Transition + * + * Does nothing. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state); + + std::map::SharedPtr> + drivers_get_state_clients; ///< stores node_id and get_state client + std::map::SharedPtr> + drivers_change_state_clients; ///< stores node_id and change_state client + std::map device_names_to_ids; // stores device_name and node_id + + /** + * @todo Remove relicts? + * + */ + rclcpp::Client::SharedPtr + add_driver_client_; ///< Service client object for adding a driver + rclcpp::Client::SharedPtr + remove_driver_client_; ///< Service client object for removing a driver + + uint8_t master_id_; ///< Stores master id + std::string container_name_; ///< Stores name of the associated device_container + +protected: + template + std::future_status wait_for_result(FutureT & future, WaitTimeT time_to_wait) + { + auto end = std::chrono::steady_clock::now() + time_to_wait; + std::chrono::milliseconds wait_period(100); + std::future_status status = std::future_status::timeout; + do + { + auto now = std::chrono::steady_clock::now(); + auto time_left = end - now; + if (time_left <= std::chrono::seconds(0)) + { + break; + } + status = future.wait_for((time_left < wait_period) ? time_left : wait_period); + } while (rclcpp::ok() && status != std::future_status::ready); + return status; + } + + /** + * @brief Get the lifecycle state of a driver node + * + * @param [in] node_id + * @param [in] time_out + * @return unsigned int + */ + virtual unsigned int get_state(uint8_t node_id, std::chrono::seconds time_out); + + /** + * @brief Change the lifecycle state of a driver node + * + * @param [in] node_id CANopen node id of the driver + * @param [in] transition Lifecycle transition to trigger + * @param [in] time_out Timeout + * @return true + * @return false + */ + virtual bool change_state(uint8_t node_id, uint8_t transition, std::chrono::seconds time_out); + + /** + * @brief Brings up master and all drivers + * + * This funnnction brings up the CANopen master and all drivers that are defined + * in the configuration. The order of bringing up drivers is arbitrary. + * + * @return true + * @return false + */ + virtual bool bring_up_all(); + + /** + * @brief Brings down master and all drivers + * + * This funnnction brings up the CANopen master and all drivers that are defined + * in the configuration. First all drivers are brought down, then the master. + * + * @return true + * @return false + */ + virtual bool bring_down_all(); + + /** + * @brief Brings up master + * + * This function brings up the CANopen master. The master transitioned twice, + * first the configure transition is triggered. Once the transition is successfully + * finished, the activate transition is triggered. + * + * @return true + * @return false + */ + virtual bool bring_up_master(); + + /** + * @brief Bring down master + * + * This function brrignsdown the CANopen master. The master transitioned twice, + * first the deactivate transition is triggered. Once the transition is successfully + * finished, the cleanup transition is triggered. + * + * @return true + * @return false + */ + virtual bool bring_down_master(); + + /** + * @brief Brings up the drivers with specified node_id + * + * This function bringsup the CANopen driver for the device with the specified + * node_id. The driver is transitioned twice, + * first the configure transition is triggered. Once the transition is successfully + * finished, the activate transition is triggered. + * + * @param device_name + * @return true + * @return false + */ + virtual bool bring_up_driver(std::string device_name); + + /** + * @brief Brings down the driver with specified node_id + * + * This function brings down the CANopen driver for the device with the specified + * node_id. The driver is transitioned twice, + * first the deactivate transition is triggered. Once the transition is successfully + * finished, the cleanup transition is triggered. + * + * @param device_name + * @return true + * @return false + */ + virtual bool bring_down_driver(std::string device_name); + + /** + * @brief Load information from configuration + * + * This function loads the information about the CANopen Bus specified in + * the configuration file and creates the necessary ROS2 services and clients. + * + * @return true + * @return false + */ + virtual bool load_from_config(); +}; +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/lifecycle_master_node.hpp b/canopen_core/include/canopen_core/lifecycle_master_node.hpp deleted file mode 100644 index a5844795..00000000 --- a/canopen_core/include/canopen_core/lifecycle_master_node.hpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef LIFECYCLE_MASTER_NODE_HPP -#define LIFECYCLE_MASTER_NODE_HPP - -#include -#include -#include - -#include - -#include "canopen_core/exchange.hpp" -#include "canopen_core/device.hpp" -#include "canopen_core/lely_master_bridge.hpp" -#include "canopen_interfaces/srv/co_write_id.hpp" -#include "canopen_interfaces/srv/co_read_id.hpp" - -namespace ros2_canopen -{ - class LifecycleMasterNode : public LifecycleMasterInterface - { - protected: - std::atomic activated; - std::shared_ptr master_; - std::unique_ptr io_guard_; - std::unique_ptr ctx_; - std::unique_ptr poll_; - std::unique_ptr loop_; - std::shared_ptr exec_; - std::unique_ptr timer_; - std::unique_ptr ctrl_; - std::unique_ptr chan_; - std::unique_ptr sigset_; - std::thread spinner_; - - rclcpp::Service::SharedPtr sdo_read_service; - rclcpp::Service::SharedPtr sdo_write_service; - - public: - LifecycleMasterNode( - const rclcpp::NodeOptions &node_options - ) : LifecycleMasterInterface("master", node_options) - { - } - - /** - * @brief Initialises the LifecycleMasterNode - * - * As LifecycleMasterNode is a component this function enables passing data to the - * node that would usually be passed via the constructor. - * - */ - void init() override; - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State & state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State & state); - - /** - * @brief Add a device driver - * - * This function only has an effect if the LifecycleMasterNode is in active state. - * The function will add a driver for a specific node id to the CANopen - * event loop. - * - * @param node_instance - * @param node_id - */ - void init_driver(std::shared_ptr node_instance, uint8_t node_id) override; - - /** - * @brief Read Service Data Object - * - * This Service is only available when the node is in active lifecycle state. - * It will return with success false in any other lifecycle state and log an - * RCLCPP_ERROR. - * - * @param request - * @param response - */ - void on_sdo_read( - const std::shared_ptr request, - std::shared_ptr response); - - /** - * @brief Write Service Data Object - * - * This service is only available when the node is in active lifecycle state. - * It will return with success false in any other lifecycle state and log an - * RCLCPP_ERROR. - * - * @param request - * @param response - */ - void on_sdo_write( - const std::shared_ptr request, - std::shared_ptr response); - }; -} - - -#endif \ No newline at end of file diff --git a/canopen_core/include/canopen_core/master_error.hpp b/canopen_core/include/canopen_core/master_error.hpp new file mode 100644 index 00000000..947302c5 --- /dev/null +++ b/canopen_core/include/canopen_core/master_error.hpp @@ -0,0 +1,27 @@ +#ifndef MASTER_ERROR_HPP_ +#define MASTER_ERROR_HPP_ + +#include + +namespace ros2_canopen +{ +/** + * @brief Master Exception + * + * This exception is used, when a master + * fails. + * + */ +class MasterException : public std::exception +{ +private: + std::string what_; + +public: + MasterException(std::string what) { what_ = what; } + + char * what(); +}; +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/master_node.hpp b/canopen_core/include/canopen_core/master_node.hpp index 61ef9a33..b779b497 100644 --- a/canopen_core/include/canopen_core/master_node.hpp +++ b/canopen_core/include/canopen_core/master_node.hpp @@ -1,84 +1,290 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef MASTER_NODE_HPP -#define MASTER_NODE_HPP +#ifndef MASTER_NODE_HPP_ +#define MASTER_NODE_HPP_ -#include -#include +#include +#include "canopen_core/node_interfaces/node_canopen_master.hpp" - -#include "canopen_core/exchange.hpp" -#include "canopen_core/device.hpp" -#include "canopen_core/lely_master_bridge.hpp" -#include "canopen_interfaces/srv/co_write_id.hpp" -#include "canopen_interfaces/srv/co_read_id.hpp" namespace ros2_canopen { - class MasterNode : public MasterInterface - { - protected: - std::shared_ptr master_; - std::unique_ptr io_guard_; - std::unique_ptr ctx_; - std::unique_ptr poll_; - std::unique_ptr loop_; - std::shared_ptr exec_; - std::unique_ptr timer_; - std::unique_ptr ctrl_; - std::unique_ptr chan_; - std::unique_ptr sigset_; - std::thread spinner_; +/** + * @brief Canopen Master Interface + * + * This class provides the interface that all master's that are loaded + * by the device container need to implement. + */ +class CanopenMasterInterface +{ +public: + /** + * @brief Initialise the master + * + * This function should initialise the master's functionality. + * It will be called by the device container after the node was + * added to the ros executor. + */ + virtual void init() = 0; + /** + * @brief Shutdown the driver + * + * This function should shutdown all functionality and make sure that + * the master will exit cleanly. It will be called by the device container + * before exiting. + * + */ + virtual void shutdown() = 0; + /** + * @brief Get the master object + * + * This function should return the lely master object. It should + * throw a ros2_canopen::MasterException if the master object + * was not yet set. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_master() = 0; + /** + * @brief Get the executor object + * + * This function gets the lely executor object. It should throw + * a ros2_canopen::MasterException if the executor object is not + * yet instantiated. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_executor() = 0; - rclcpp::Service::SharedPtr sdo_read_service; - rclcpp::Service::SharedPtr sdo_write_service; + /** + * @brief Get the node base interface object + * + * This function should return the NodeBaseInterface of the associated + * ROS node. This is used by device container to add the master to the + * ros executor. + * + * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + */ + virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() = 0; - public: - MasterNode( - const rclcpp::NodeOptions &node_options - ) : MasterInterface("master", node_options) - { - } + /** + * @brief Check whether the node is a lifecycle node + * + * @return true + * @return false + */ + virtual bool is_lifecycle() = 0; +}; - void init(std::string dcf_txt, std::string dcf_bin, std::string can_interface_name, uint8_t nodeid, - std::shared_ptr config) override; +/** + * @brief Canopen Master + * + * This class implements the Canopen Master Interface and is derived + * from rclcpp::Node. All unmanaged master nodes should inherit from + * this class. + * + */ +class CanopenMaster : public CanopenMasterInterface, public rclcpp::Node +{ +public: + std::shared_ptr node_canopen_master_; + explicit CanopenMaster(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : rclcpp::Node("canopen_master", node_options) + { + // node_canopen_master_ = + // std::make_shared>(this); + } + /** + * @brief Initialises the driver + * + * This function initialises the driver. It is called by the + * device container after adding the node to the ros executor. + * This function uses the NodeCanopenMasterInterface. It will + * call init(), configure() and activate(). If this function + * does not throw, the driver is successfully initialised and + * ready to be used. + * + */ + virtual void init() override; - void add_driver(std::shared_ptr node_instance, uint8_t node_id) override; + /** + * @brief Shuts the master down + * + * This function is called by the device container before exiting, + * it should enable a clean exit of the master and especially join + * all threads. It will call the shutdown() function of the + * NodeCanopenMasterInterface object associated with this class. + */ + virtual void shutdown() override; - void remove_driver(std::shared_ptr node_instance, uint8_t node_id) override; + /** + * @brief Get the master object + * + * This function should return the lely master object. It should + * throw a ros2_canopen::MasterException if the master object + * was not yet set. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_master() override; + /** + * @brief Get the executor object + * + * This function gets the lely executor object. It should throw + * a ros2_canopen::MasterException if the executor object is not + * yet instantiated. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_executor() override; + /** + * @brief Get the node base interface object + * + * This function should return the NodeBaseInterface of the associated + * ROS node. This is used by device container to add the master to the + * ros executor. + * + * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + */ + virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override + { + return rclcpp::Node::get_node_base_interface(); + } + /** + * @brief Check whether the node is a lifecycle node + * + * @return false + */ + virtual bool is_lifecycle() { return false; } +}; + +class LifecycleCanopenMaster : public CanopenMasterInterface, public rclcpp_lifecycle::LifecycleNode +{ +protected: + std::shared_ptr node_canopen_master_; - /** - * @brief on_sdo_read - * - * @param request - * @param response - */ - void on_sdo_read( - const std::shared_ptr request, - std::shared_ptr response); +public: + LifecycleCanopenMaster(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : rclcpp_lifecycle::LifecycleNode("lifecycle_canopen_master", node_options) + { + node_canopen_master_ = + std::make_shared>(this); + } + /** + * @brief Initialises the driver + * + * This function initialises the driver. It is called by the + * device container after adding the node to the ros executor. + * This function uses the NodeCanopenMasterInterface. It will + * call init(). If this function + * does not throw, the driver is successfully initialised and + * ready to switch through the lifecycle. + * + */ + virtual void init() override; - /** - * @brief on_sdo_write - * - * @param request - * @param response - */ - void on_sdo_write( - const std::shared_ptr request, - std::shared_ptr response); - }; -} + /** + * @brief Shuts the master down + * + * This function is called by the device container before exiting, + * it should enable a clean exit of the master and especially join + * all threads. It will call the shutdown() function of the + * NodeCanopenMasterInterface object associated with this class. + */ + virtual void shutdown() override; + /** + * @brief Get the master object + * + * This function should return the lely master object. It should + * throw a ros2_canopen::MasterException if the master object + * was not yet set. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_master() override; + /** + * @brief Get the executor object + * + * This function gets the lely executor object. It should throw + * a ros2_canopen::MasterException if the executor object is not + * yet instantiated. + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_executor() override; + /** + * @brief Configure Transition Callback + * + * This function will call the configure() function of the + * NodeCanopenMasterInterface object associated with this class. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & state); + /** + * @brief Activate Transition Callback + * + * This function will call the active() function of the + * NodeCanopenMasterInterface object associated with this class. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & state); + /** + * @brief Deactivet Transition Callback + * + * This function will call the deactivate() function of the + * NodeCanopenMasterInterface object associated with this class. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & state); + /** + * @brief Cleanup Transition Callback + * + * This function will call the cleanup() function of the + * NodeCanopenMasterInterface object associated with this class. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & state); + /** + * @brief Shutdown Transition Callback + * + * This function will call the shutdown() function of the + * NodeCanopenMasterInterface object associated with this class. + * + * @param state + * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state); + /** + * @brief Get the node base interface object + * + * This function should return the NodeBaseInterface of the associated + * ROS node. This is used by device container to add the master to the + * ros executor. + * + * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + */ + virtual rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override + { + return rclcpp_lifecycle::LifecycleNode::get_node_base_interface(); + } + /** + * @brief Check whether the node is a lifecycle node + * + * @return true + */ + virtual bool is_lifecycle() { return true; } +}; +} // namespace ros2_canopen -#endif \ No newline at end of file +#endif diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp new file mode 100644 index 00000000..fcae632a --- /dev/null +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp @@ -0,0 +1,370 @@ +#ifndef NODE_CANOPEN_DRIVER_HPP_ +#define NODE_CANOPEN_DRIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "canopen_core/driver_error.hpp" +#include "canopen_core/node_interfaces/node_canopen_driver_interface.hpp" +#include "canopen_core/visibility_control.h" +#include "canopen_interfaces/srv/co_node.hpp" + +using namespace rclcpp; +namespace ros2_canopen +{ +namespace node_interfaces +{ +/** + * @brief Node Canopen Driver + * + * This class implements the NodeCanopenDriverInterface. It provides + * core functionality and logic for CanopenDriver, indepentently of the + * ROS node type. Currently rclcpp::Node and rclcpp_lifecycle::LifecycleNode + * and derived classes are supported. Other node types will lead to compile + * time error. + * + * @tparam NODETYPE + */ +template +class NodeCanopenDriver : public NodeCanopenDriverInterface +{ + static_assert( + std::is_base_of::value || + std::is_base_of::value, + "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); + +protected: + NODETYPE * node_; + + std::shared_ptr exec_; + std::shared_ptr master_; + std::shared_ptr driver_; + + std::chrono::milliseconds non_transmit_timeout_; + YAML::Node config_; + uint8_t node_id_; + std::string container_name_; + std::string eds_; + std::string bin_; + + rclcpp::CallbackGroup::SharedPtr client_cbg_; + rclcpp::CallbackGroup::SharedPtr timer_cbg_; + + std::atomic master_set_; + std::atomic initialised_; + std::atomic configured_; + std::atomic activated_; + +public: + NodeCanopenDriver(NODETYPE * node) + : master_set_(false), initialised_(false), configured_(false), activated_(false) + { + node_ = node; + } + + /** + * @brief Set Master + * + * Sets the Lely Canopen Master Objects needed by the driver + * to add itself to the master. + * + */ + virtual void set_master( + std::shared_ptr exec, std::shared_ptr master) + { + RCLCPP_DEBUG(node_->get_logger(), "set_master_start"); + if (!configured_.load()) + { + throw DriverException("Set Master: driver is not configured"); + } + if (activated_.load()) + { + throw DriverException("Set Master: driver is not activated"); + } + this->exec_ = exec; + this->master_ = master; + this->master_set_.store(true); + RCLCPP_DEBUG(node_->get_logger(), "set_master_end"); + } + + /** + * @brief Initialise the driver + * + * In this function the ROS interface should be created and + * potentially necessary callback groups. + * + */ + void init() + { + RCLCPP_DEBUG(node_->get_logger(), "init_start"); + if (configured_.load()) + { + throw DriverException("Init: Driver is already configured"); + } + if (activated_.load()) + { + throw DriverException("Init: Driver is already activated"); + } + client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + node_->declare_parameter("container_name", ""); + node_->declare_parameter("node_id", 0); + node_->declare_parameter("non_transmit_timeout", 100); + node_->declare_parameter("config", ""); + this->init(true); + this->initialised_.store(true); + RCLCPP_DEBUG(node_->get_logger(), "init_end"); + } + + /** + * @brief Initialises the driver + * + * This does not do anything, it is an empty function. it + * should be overridden by derived classes. + * + * @todo + * Potentially make this an abstract function. This is mainly + * for debugging purposes. + * + * @param called_from_base + */ + virtual void init(bool called_from_base) {} + + /** + * @brief Configure the driver + * + * This function should contain the configuration related things, + * such as reading parameter data or configuration data from files. + * + * This function reads the parameters container_name, non_transmit_timeout, + * node_id and config. Once done it will call the configure(bool) function + * that should be over + * + */ + void configure() + { + RCLCPP_DEBUG(node_->get_logger(), "configure_start"); + if (!initialised_.load()) + { + throw DriverException("Configure: driver is not initialised"); + } + if (configured_.load()) + { + throw DriverException("Configure: driver is already configured"); + } + if (activated_.load()) + { + throw DriverException("Configure: driver is already activated"); + } + int non_transmit_timeout; + std::string config; + node_->get_parameter("container_name", container_name_); + node_->get_parameter("non_transmit_timeout", non_transmit_timeout); + node_->get_parameter("node_id", this->node_id_); + node_->get_parameter("config", config); + this->config_ = YAML::Load(config); + this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout); + auto path = this->config_["dcf_path"].as(); + auto dcf = this->config_["dcf"].as(); + auto name = this->node_->get_name(); + eds_ = path + "/" + dcf; + bin_ = path + "/" + name + ".bin"; + this->configure(true); + this->configured_.store(true); + RCLCPP_DEBUG(node_->get_logger(), "configure_end"); + } + /** + * @brief Configure the driver + * + * This function should be overridden by derived classes. + * + * @param called_from_base + */ + virtual void configure(bool called_from_base) {} + + /** + * @brief Activate the driver + * + * This function should activate the driver, consequently it needs to start all timers and threads + * necessary for the operation of the driver. + * + */ + void activate() + { + RCLCPP_DEBUG(node_->get_logger(), "activate_start"); + if (!master_set_.load()) + { + throw DriverException("Activate: master is not set"); + } + if (!initialised_.load()) + { + throw DriverException("Activate: driver is not initialised"); + } + if (!configured_.load()) + { + throw DriverException("Activate: driver is not configured"); + } + if (activated_.load()) + { + throw DriverException("Activate: driver is already activated"); + } + this->add_to_master(); + this->activate(true); + this->activated_.store(true); + RCLCPP_DEBUG(node_->get_logger(), "activate_end"); + } + + /** + * @brief Activates the driver. + * + * This function should be overridden by derived + * classes. + * + * @param called_from_base + */ + virtual void activate(bool called_from_base) {} + + /** + * @brief Deactivate the driver + * + * This function should deactivate the driver, consequently it needs to stop all timers and + * threads that are related to the operation of the diver. + * + */ + void deactivate() + { + RCLCPP_DEBUG(node_->get_logger(), "deactivate_start"); + if (!master_set_.load()) + { + throw DriverException("Activate: master is not set"); + } + if (!initialised_.load()) + { + throw DriverException("Deactivate: driver is not initialised"); + } + if (!configured_.load()) + { + throw DriverException("Deactivate: driver is not configured"); + } + if (!activated_.load()) + { + throw DriverException("Deactivate: driver is not activated"); + } + this->activated_.store(false); + this->remove_from_master(); + this->deactivate(true); + RCLCPP_DEBUG(node_->get_logger(), "deactivate_end"); + } + /** + * @brief Deactivates the driver + * + * This function should be overridden by derived classes. + * + * @param called_from_base + */ + virtual void deactivate(bool called_from_base) {} + + /** + * @brief Cleanup the driver + * + * This function needs to clean the internal state of the driver. This means + * all data should be deleted. + * + */ + void cleanup() + { + if (!initialised_.load()) + { + throw DriverException("Cleanup: driver is not initialised"); + } + if (!configured_.load()) + { + throw DriverException("Cleanup: driver is not configured"); + } + if (activated_.load()) + { + throw DriverException("Cleanup: driver is still activated"); + } + this->configured_.store(false); + } + + /** + * @brief Cleanup the driver + * + * This function should be overridden by derived classes. + * + * @param called_from_base + */ + virtual void cleanup(bool called_from_base) {} + + /** + * @brief Shutdown the driver + * + * This function should shutdown the driver. + * + */ + void shutdown() + { + RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down."); + if (this->activated_) + { + this->deactivate(); + } + if (this->configured_) + { + this->cleanup(); + } + shutdown(true); + this->master_set_.store(false); + this->initialised_.store(false); + this->configured_.store(false); + this->activated_.store(false); + } + /** + * @brief Shuts down the driver + * + * This function should be overridden by derived classes. + * + * @param called_from_base + */ + virtual void shutdown(bool called_from_base) {} + + virtual void demand_set_master(); + +protected: + /** + * @brief Add the driver to master + * + */ + virtual void add_to_master() { throw DriverException("Add to master not implemented."); } + + /** + * @brief Remove the driver from master + * + */ + virtual void remove_from_master() + { + throw DriverException("Remove from master not implemented."); + } +}; +} // namespace node_interfaces +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp new file mode 100644 index 00000000..d4b918b8 --- /dev/null +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp @@ -0,0 +1,111 @@ +#ifndef NODE_CANOPEN_DRIVER_INTERFACE_HPP_ +#define NODE_CANOPEN_DRIVER_INTERFACE_HPP_ + +#include +#include + +namespace ros2_canopen +{ +namespace node_interfaces +{ +/** + * @brief Node Canopen Driver Interface + * + * This node provides the interface for NodeCanopenDriver classes + * that provide ROS node independent CANopen functionality. + * + */ +class NodeCanopenDriverInterface +{ +public: + NodeCanopenDriverInterface() {} + /** + * @brief Set Master + * + * Sets the Lely Canopen Master Objects needed by the driver + * to add itself to the master. + * + */ + virtual void set_master( + std::shared_ptr exec, + std::shared_ptr master) = 0; + + /** + * @brief Demand set Master + * + * This function should ask the drivers parent container to + * set the master objects. This should result in a call to + * set_master function. + * + */ + virtual void demand_set_master() = 0; + + /** + * @brief Initialise the driver + * + * In this function the ROS interface should be created and + * potentially necessary callback groups. + * + */ + virtual void init() = 0; + + /** + * @brief Configure the driver + * + * This function should contain the configuration related things, + * such as reading parameter data or configuration data from files. + * + */ + virtual void configure() = 0; + + /** + * @brief Activate the driver + * + * This function should activate the driver, consequently it needs to start all timers and threads + * necessary for the operation of the driver. + * + */ + virtual void activate() = 0; + + /** + * @brief Deactivate the driver + * + * This function should deactivate the driver, consequently it needs to stop all timers and + * threads that are related to the operation of the diver. + * + */ + virtual void deactivate() = 0; + + /** + * @brief Cleanup the driver + * + * This function needs to clean the internal state of the driver. This means + * all data should be deleted. + * + */ + virtual void cleanup() = 0; + + /** + * @brief Shutdown the driver + * + * This function should shutdown the driver. + * + */ + virtual void shutdown() = 0; + + /** + * @brief Add the driver to master + * + */ + virtual void add_to_master() = 0; + + /** + * @brief Remove the driver from master + * + */ + virtual void remove_from_master() = 0; +}; +} // namespace node_interfaces +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp new file mode 100644 index 00000000..73a96c2e --- /dev/null +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp @@ -0,0 +1,351 @@ +#ifndef NODE_CANOPEN_MASTER_HPP_ +#define NODE_CANOPEN_MASTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "canopen_core/master_error.hpp" +#include "canopen_core/node_interfaces/node_canopen_master_interface.hpp" + +namespace ros2_canopen +{ +namespace node_interfaces +{ +/** + * @brief Node Canopen Master + * + * This class implements the NodeCanopenMasterInterface. It provides + * core functionality and logic for CanopenMaster, indepentently of the + * ROS node type. Currently rclcpp::Node and rclcpp_lifecycle::LifecycleNode + * and derived classes are supported. Other node types will lead to compile + * time error. + * + * @tparam NODETYPE + */ +template +class NodeCanopenMaster : public NodeCanopenMasterInterface +{ + static_assert( + std::is_base_of::value || + std::is_base_of::value, + "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); + +protected: + NODETYPE * node_; + + std::atomic initialised_; + std::atomic configured_; + std::atomic activated_; + std::atomic master_set_; + + std::shared_ptr master_; + std::shared_ptr exec_; + + std::unique_ptr io_guard_; + std::unique_ptr ctx_; + std::unique_ptr poll_; + std::unique_ptr loop_; + std::unique_ptr timer_; + std::unique_ptr ctrl_; + std::unique_ptr chan_; + std::unique_ptr sigset_; + + rclcpp::CallbackGroup::SharedPtr client_cbg_; + rclcpp::CallbackGroup::SharedPtr timer_cbg_; + + YAML::Node config_; + uint8_t node_id_; + std::chrono::milliseconds non_transmit_timeout_; + std::string container_name_; + std::string master_dcf_; + std::string master_bin_; + std::string can_interface_name_; + + std::thread spinner_; + +public: + NodeCanopenMaster(NODETYPE * node) + : initialised_(false), configured_(false), activated_(false), master_set_(false) + { + node_ = node; + } + + /** + * @brief Initialise Master + * + */ + void init() override + { + RCLCPP_DEBUG(node_->get_logger(), "init_start"); + if (configured_.load()) + { + throw MasterException("Init: Master is already configured."); + } + if (activated_.load()) + { + throw MasterException("Init: Master is already activated."); + } + client_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_cbg_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + node_->declare_parameter("container_name", ""); + node_->declare_parameter("master_dcf", ""); + node_->declare_parameter("master_bin", ""); + node_->declare_parameter("can_interface_name", "vcan0"); + node_->declare_parameter("node_id", 0); + node_->declare_parameter("non_transmit_timeout", 100); + node_->declare_parameter("config", ""); + this->init(true); + this->initialised_.store(true); + RCLCPP_DEBUG(node_->get_logger(), "init_end"); + } + virtual void init(bool called_from_base) {} + + /** + * @brief Configure the driver + * + * This function should contain the configuration related things, + * such as reading parameter data or configuration data from files. + * + */ + void configure() override + { + if (!initialised_.load()) + { + throw MasterException("Configure: Master is not initialised."); + } + if (configured_.load()) + { + throw MasterException("Configure: Master is already configured."); + } + if (activated_.load()) + { + throw MasterException("Configure: Master is already activated."); + } + + int non_transmit_timeout; + std::string config; + + node_->get_parameter("container_name", container_name_); + node_->get_parameter("master_dcf", master_dcf_); + node_->get_parameter("master_bin", master_bin_); + node_->get_parameter("can_interface_name", can_interface_name_); + node_->get_parameter("node_id", node_id_); + node_->get_parameter("non_transmit_timeout", non_transmit_timeout); + node_->get_parameter("config", config); + + this->config_ = YAML::Load(config); + this->non_transmit_timeout_ = std::chrono::milliseconds(non_transmit_timeout); + + this->configure(true); + this->configured_.store(true); + } + + virtual void configure(bool called_from_base) {} + + /** + * @brief Activate the driver + * + * This function should activate the driver, consequently it needs to start all timers and threads + * necessary for the operation of the driver. + * + */ + void activate() override + { + RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate start"); + if (!initialised_.load()) + { + throw MasterException("Activate: master is not initialised"); + } + if (!configured_.load()) + { + throw MasterException("Activate: master is not configured"); + } + if (activated_.load()) + { + throw MasterException("Activate: master is already activated"); + } + + io_guard_ = std::make_unique(); + ctx_ = std::make_unique(); + poll_ = std::make_unique(*ctx_); + loop_ = std::make_unique(poll_->get_poll()); + + exec_ = std::make_shared(loop_->get_executor()); + timer_ = std::make_unique(*poll_, *exec_, CLOCK_MONOTONIC); + ctrl_ = std::make_unique(can_interface_name_.c_str()); + chan_ = std::make_unique(*poll_, *exec_); + chan_->open(*ctrl_); + + this->activate(true); + if (!master_) + { + throw MasterException("Activate: master not set"); + } + this->master_set_.store(true); + this->master_->Reset(); + this->spinner_ = std::thread( + [this]() + { + try + { + loop_->run(); + } + catch (const std::exception & e) + { + RCLCPP_INFO(this->node_->get_logger(), e.what()); + } + RCLCPP_INFO(this->node_->get_logger(), "Canopen master loop stopped"); + }); + this->activated_.store(true); + RCLCPP_DEBUG(this->node_->get_logger(), "NodeCanopenMaster activate end"); + } + /** + * @brief Activate hook for derived classes + * + * This function should create a Master using exec_, timer_, master_dcf_, master_bin_ + * and node_id_ members and store it in master_. It should also create a thread and run + * the master's event loop. + * + * @param called_from_base + */ + virtual void activate(bool called_from_base) {} + + /** + * @brief Deactivate the driver + * + * This function should deactivate the driver, consequently it needs to stop all timers and + * threads that are related to the operation of the diver. + * + */ + void deactivate() override + { + if (!initialised_.load()) + { + throw MasterException("Deactivate: master is not initialised"); + } + if (!configured_.load()) + { + throw MasterException("Deactivate: master is not configured"); + } + if (!activated_.load()) + { + throw MasterException("Deactivate: master is not activated"); + } + exec_->post( + [this]() + { + RCLCPP_INFO(node_->get_logger(), "Lely Core Context Shutdown"); + ctx_->shutdown(); + }); + this->spinner_.join(); + + this->deactivate(true); + this->activated_.store(false); + } + + /** + * @brief Deactivate hook for derived classes + * + * This function should wait to join the thread created in the activate + * function. + * + * @param called_from_base + */ + virtual void deactivate(bool called_from_base) {} + + /** + * @brief Cleanup the driver + * + * This function needs to clean the internal state of the driver. This means + * all data should be deleted. + * + */ + void cleanup() override + { + if (!initialised_.load()) + { + throw MasterException("Cleanup: master is not initialised."); + } + if (!configured_.load()) + { + throw MasterException("Cleanup: master is not configured"); + } + if (activated_.load()) + { + throw MasterException("Cleanup: master is still active"); + } + this->cleanup(true); + this->configured_.store(false); + } + virtual void cleanup(bool called_from_base) {} + + /** + * @brief Shutdown the driver + * + * This function should shutdown the driver. + * + */ + void shutdown() override + { + RCLCPP_DEBUG(this->node_->get_logger(), "Shutting down."); + if (this->activated_) + { + this->deactivate(); + } + + if (this->configured_) + { + this->cleanup(); + } + shutdown(true); + + this->master_set_.store(false); + this->initialised_.store(false); + this->configured_.store(false); + this->activated_.store(false); + } + virtual void shutdown(bool called_from_base) {} + + /** + * @brief Get the master object + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_master() + { + if (!master_set_.load()) + { + throw MasterException("Get Master: Master is not set."); + } + return master_; + } + /** + * @brief Get the executor object + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_executor() + { + if (!master_set_.load()) + { + throw MasterException("Get Executor: master is not set"); + } + return exec_; + } +}; +} // namespace node_interfaces +} // namespace ros2_canopen + +#endif diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp new file mode 100644 index 00000000..6357e0c0 --- /dev/null +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp @@ -0,0 +1,86 @@ +#ifndef NODE_CANOPEN_MASTER_INTERFACE_HPP_ +#define NODE_CANOPEN_MASTER_INTERFACE_HPP_ + +#include +#include +#include "canopen_core/node_interfaces/node_canopen_driver_interface.hpp" + +namespace ros2_canopen +{ +namespace node_interfaces +{ +/** + * @brief Node Canopen Master Interface + * + * This node provides the interface for NodeCanopenMaster classes + * that provide ROS node independent CANopen functionality. + * + */ +class NodeCanopenMasterInterface +{ +public: + /** + * @brief Initialise Master + * + */ + virtual void init() = 0; + + /** + * @brief Configure the driver + * + * This function should contain the configuration related things, + * such as reading parameter data or configuration data from files. + * + */ + virtual void configure() = 0; + + /** + * @brief Activate the driver + * + * This function should activate the driver, consequently it needs to start all timers and threads + * necessary for the operation of the driver. + * + */ + virtual void activate() = 0; + + /** + * @brief Deactivate the driver + * + * This function should deactivate the driver, consequently it needs to stop all timers and + * threads that are related to the operation of the diver. + * + */ + virtual void deactivate() = 0; + + /** + * @brief Cleanup the driver + * + * This function needs to clean the internal state of the driver. This means + * all data should be deleted. + * + */ + virtual void cleanup() = 0; + + /** + * @brief Shutdown the driver + * + * This function should shutdown the driver. + * + */ + virtual void shutdown() = 0; + /** + * @brief Get the master object + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_master() = 0; + /** + * @brief Get the executor object + * + * @return std::shared_ptr + */ + virtual std::shared_ptr get_executor() = 0; +}; +} // namespace node_interfaces +} // namespace ros2_canopen +#endif diff --git a/canopen_core/include/canopen_core/visibility_control.h b/canopen_core/include/canopen_core/visibility_control.h index a78302e3..9ce5eb7c 100644 --- a/canopen_core/include/canopen_core/visibility_control.h +++ b/canopen_core/include/canopen_core/visibility_control.h @@ -1,5 +1,5 @@ // Copyright 2022 Christoph Hellmann Santos -// +// // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -18,31 +18,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define CANOPEN_CORE_EXPORT __attribute__ ((dllexport)) - #define CANOPEN_CORE_IMPORT __attribute__ ((dllimport)) - #else - #define CANOPEN_CORE_EXPORT __declspec(dllexport) - #define CANOPEN_CORE_IMPORT __declspec(dllimport) - #endif - #ifdef CANOPEN_CORE_BUILDING_LIBRARY - #define CANOPEN_CORE_PUBLIC CANOPEN_CORE_EXPORT - #else - #define CANOPEN_CORE_PUBLIC CANOPEN_CORE_IMPORT - #endif - #define CANOPEN_CORE_PUBLIC_TYPE CANOPEN_CORE_PUBLIC - #define CANOPEN_CORE_LOCAL +#ifdef __GNUC__ +#define CANOPEN_CORE_EXPORT __attribute__((dllexport)) +#define CANOPEN_CORE_IMPORT __attribute__((dllimport)) +#else +#define CANOPEN_CORE_EXPORT __declspec(dllexport) +#define CANOPEN_CORE_IMPORT __declspec(dllimport) +#endif +#ifdef CANOPEN_CORE_BUILDING_LIBRARY +#define CANOPEN_CORE_PUBLIC CANOPEN_CORE_EXPORT #else - #define CANOPEN_CORE_EXPORT __attribute__ ((visibility("default"))) - #define CANOPEN_CORE_IMPORT - #if __GNUC__ >= 4 - #define CANOPEN_CORE_PUBLIC __attribute__ ((visibility("default"))) - #define CANOPEN_CORE_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define CANOPEN_CORE_PUBLIC - #define CANOPEN_CORE_LOCAL - #endif - #define CANOPEN_CORE_PUBLIC_TYPE +#define CANOPEN_CORE_PUBLIC CANOPEN_CORE_IMPORT +#endif +#define CANOPEN_CORE_PUBLIC_TYPE CANOPEN_CORE_PUBLIC +#define CANOPEN_CORE_LOCAL +#else +#define CANOPEN_CORE_EXPORT __attribute__((visibility("default"))) +#define CANOPEN_CORE_IMPORT +#if __GNUC__ >= 4 +#define CANOPEN_CORE_PUBLIC __attribute__((visibility("default"))) +#define CANOPEN_CORE_LOCAL __attribute__((visibility("hidden"))) +#else +#define CANOPEN_CORE_PUBLIC +#define CANOPEN_CORE_LOCAL +#endif +#define CANOPEN_CORE_PUBLIC_TYPE #endif #endif // CANOPEN_CORE__VISIBILITY_CONTROL_H_ diff --git a/canopen_core/launch/canopen.launch.py b/canopen_core/launch/canopen.launch.py index 4e36bcb2..38728024 100644 --- a/canopen_core/launch/canopen.launch.py +++ b/canopen_core/launch/canopen.launch.py @@ -1,15 +1,15 @@ - import os import sys -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) # noqa -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'launch')) # noqa + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) # noqa +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "launch")) # noqa import launch import launch.actions import launch.events import launch_ros -import launch_ros.events +import launch_ros.events import launch_ros.events.lifecycle from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution from launch.actions import DeclareLaunchArgument @@ -19,64 +19,56 @@ def generate_launch_description(): bus_conf_arg = DeclareLaunchArgument( - 'bus_config', - default_value=TextSubstitution(text=''), - description="Path to the bus configuration to use." + "bus_config", + default_value=TextSubstitution(text=""), + description="Path to the bus configuration to use.", ) master_conf_arg = DeclareLaunchArgument( - 'master_config', - default_value=TextSubstitution(text=''), - description="Path to the master configuration to use (dcf)." + "master_config", + default_value=TextSubstitution(text=""), + description="Path to the master configuration to use (dcf).", ) master_bin_arg = DeclareLaunchArgument( - 'master_bin', - default_value=TextSubstitution(text='2'), - description="Path to the master configuration to use (bin)." + "master_bin", + default_value=TextSubstitution(text=""), + description="Path to the master configuration to use (bin).", ) - can_interface_arg = DeclareLaunchArgument( - 'can_interface_name', - default_value=TextSubstitution(text="vcan0"), - description="CAN interface used by master and drivers." + can_interface_name_arg = DeclareLaunchArgument( + "can_interface_name", + default_value=TextSubstitution(text="vcan0"), + description="CAN interface used by master and drivers.", ) ld = launch.LaunchDescription() logging = launch.actions.GroupAction( - actions=[ - launch.actions.LogInfo(msg=LaunchConfiguration("bus_config")), - launch.actions.LogInfo(msg=LaunchConfiguration("master_config")), - launch.actions.LogInfo(msg=LaunchConfiguration("master_bin")), - launch.actions.LogInfo(msg=LaunchConfiguration("can_interface_name")), - ] + actions=[ + launch.actions.LogInfo(msg=LaunchConfiguration("bus_config")), + launch.actions.LogInfo(msg=LaunchConfiguration("master_config")), + launch.actions.LogInfo(msg=LaunchConfiguration("master_bin")), + launch.actions.LogInfo(msg=LaunchConfiguration("can_interface_name")), + ] ) lifecycle_device_container_node = launch_ros.actions.LifecycleNode( name="device_container_node", - namespace="", - package="canopen_core", - output="screen", + namespace="", + package="canopen_core", + output="screen", executable="device_container_node", - parameters=[ - { - "bus_config": LaunchConfiguration("bus_config") - }, - { - "master_config": LaunchConfiguration("master_config") - }, - { - "master_bin": LaunchConfiguration("master_bin") - }, - { - "can_interface_name": LaunchConfiguration("can_interface_name") - }, + parameters=[ + {"bus_config": LaunchConfiguration("bus_config")}, + {"master_config": LaunchConfiguration("master_config")}, + {"master_bin": LaunchConfiguration("master_bin")}, + {"can_interface_name": LaunchConfiguration("can_interface_name")}, ], ) ld.add_action(bus_conf_arg) ld.add_action(master_conf_arg) ld.add_action(master_bin_arg) - ld.add_action(can_interface_arg) + ld.add_action(can_interface_name_arg) ld.add_action(logging) ld.add_action(lifecycle_device_container_node) diff --git a/canopen_core/launch/canopen_lifecycle.launch.py b/canopen_core/launch/canopen_lifecycle.launch.py deleted file mode 100644 index c0b00f31..00000000 --- a/canopen_core/launch/canopen_lifecycle.launch.py +++ /dev/null @@ -1,83 +0,0 @@ - -import os -import sys -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) # noqa -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'launch')) # noqa - -import launch -import launch.actions -import launch.events - -import launch_ros -import launch_ros.events -import launch_ros.events.lifecycle -from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution -from launch.actions import DeclareLaunchArgument -import lifecycle_msgs.msg - - -def generate_launch_description(): - - bus_conf_arg = DeclareLaunchArgument( - 'bus_config', - default_value=TextSubstitution(text=''), - description="Path to the bus configuration to use." - ) - - master_conf_arg = DeclareLaunchArgument( - 'master_config', - default_value=TextSubstitution(text=''), - description="Path to the master configuration to use (dcf)." - ) - - master_bin_arg = DeclareLaunchArgument( - 'master_bin', - default_value=TextSubstitution(text='2'), - description="Path to the master configuration to use (bin)." - ) - - can_interface_arg = DeclareLaunchArgument( - 'can_interface_name', - default_value=TextSubstitution(text="vcan0"), - description="CAN interface used by master and drivers." - ) - - ld = launch.LaunchDescription() - logging = launch.actions.GroupAction( - actions=[ - launch.actions.LogInfo(msg=LaunchConfiguration("bus_config")), - launch.actions.LogInfo(msg=LaunchConfiguration("master_config")), - launch.actions.LogInfo(msg=LaunchConfiguration("master_bin")), - launch.actions.LogInfo(msg=LaunchConfiguration("can_interface_name")), - ] - ) - lifecycle_device_container_node = launch_ros.actions.LifecycleNode( - name="lifecycle_device_container_node", - namespace="", - package="canopen_core", - output="screen", - executable="lifecycle_device_container_node", - parameters=[ - { - "bus_config": LaunchConfiguration("bus_config") - }, - { - "master_config": LaunchConfiguration("master_config") - }, - { - "master_bin": LaunchConfiguration("master_bin") - }, - { - "can_interface_name": LaunchConfiguration("can_interface_name") - }, - ], - ) - - ld.add_action(bus_conf_arg) - ld.add_action(master_conf_arg) - ld.add_action(master_bin_arg) - ld.add_action(can_interface_arg) - ld.add_action(logging) - ld.add_action(lifecycle_device_container_node) - - return ld diff --git a/canopen_core/package.xml b/canopen_core/package.xml index 0c4f2170..906695c4 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -9,13 +9,15 @@ ament_cmake + canopen_interfaces lely_core_libraries + lifecycle_msgs rclcpp rclcpp_components - yaml_cpp_vendor - canopen_interfaces rclcpp_lifecycle - lifecycle_msgs + yaml_cpp_vendor + boost + ament_lint_auto diff --git a/canopen_core/readme.md b/canopen_core/readme.md index 0ab3bc6c..cf3d87c7 100644 --- a/canopen_core/readme.md +++ b/canopen_core/readme.md @@ -3,7 +3,7 @@ Welcome to the ROS2 canopen documentation. ## About -ROS2 CANopen is being developed and maintainted by the [ROS-Industrial Consortium](rosindustrial.org). The package heavily builds ontop of [Lely's canopen stack](https://opensource.lely.com/canopen/). +ROS2 CANopen is being developed and maintainted by the [ROS-Industrial Consortium](rosindustrial.org). The package heavily builds on top of [Lely's canopen stack](https://opensource.lely.com/canopen/). ## Getting started @@ -33,7 +33,7 @@ In order to use the package for your CANopen network, you need to create a netwo motioncontroller_1: node_id: 2 dcf: "simple.eds" - driver: "BasicDevice" + driver: "BasicDevice" 3. Generate a master.dcf file using Lely CANopen's dcfgen tool. This outputs a master.dcf. @@ -51,7 +51,7 @@ In order to use the package for your CANopen network, you need to create a netwo ## ROS2 canopen (preliminary) -1. +1. ros2 launch bring_up_master.launch.py parameter_file_path:=[Path to your config yaml] @@ -74,9 +74,3 @@ sh 5. Activate canopen_master node and then all driver nodes. 6. The system should now be running and you should be able to use the driver nodes to communicate with your devices. - - - - - - diff --git a/canopen_core/scripts/setup_vcan.sh b/canopen_core/scripts/setup_vcan.sh index 1fdcd71a..636a6c98 100644 --- a/canopen_core/scripts/setup_vcan.sh +++ b/canopen_core/scripts/setup_vcan.sh @@ -1,3 +1,3 @@ modprobe vcan ip link add dev vcan0 type vcan -ip link set up vcan0 \ No newline at end of file +ip link set up vcan0 diff --git a/canopen_core/src/configuration_manager.cpp b/canopen_core/src/configuration_manager.cpp index bb24721b..490d7f13 100644 --- a/canopen_core/src/configuration_manager.cpp +++ b/canopen_core/src/configuration_manager.cpp @@ -4,29 +4,42 @@ namespace ros2_canopen { - void ConfigurationManager::init_config() +void ConfigurationManager::init_config() +{ + std::string dcf_path = ""; + for (YAML::const_iterator it = root_.begin(); it != root_.end(); it++) + { + std::string driver_name = it->first.as(); + if (driver_name != "options") continue; + YAML::Node config_node = it->second; + if (config_node["dcf_path"]) { - for ( - YAML::const_iterator it = root_.begin(); - it != root_.end(); - it++) - { - std::string driver_name = it->first.as(); - YAML::Node config_node = it->second; - devices_.insert({driver_name, config_node}); - } - + dcf_path = config_node["dcf_path"].as(); } + } - uint32_t ConfigurationManager::get_all_devices(std::vector &devices) + for (YAML::const_iterator it = root_.begin(); it != root_.end(); it++) + { + std::string driver_name = it->first.as(); + if (driver_name == "options") continue; + YAML::Node config_node = it->second; + if (!config_node["dcf_path"]) { - uint32_t count = 0; - for(auto it = devices_.begin(); it != devices_.end(); it++) - { - devices.emplace_back(it->first); - count++; - } - return count; + config_node["dcf_path"] = dcf_path; } + devices_.insert({driver_name, config_node}); + } +} + +uint32_t ConfigurationManager::get_all_devices(std::vector & devices) +{ + uint32_t count = 0; + for (auto it = devices_.begin(); it != devices_.end(); it++) + { + devices.emplace_back(it->first); + count++; + } + return count; +} -} // namespace ros2_canopen +} // namespace ros2_canopen diff --git a/canopen_core/src/device_container.cpp b/canopen_core/src/device_container.cpp new file mode 100644 index 00000000..43590041 --- /dev/null +++ b/canopen_core/src/device_container.cpp @@ -0,0 +1,469 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "canopen_core/device_container.hpp" +#include "canopen_core/device_container_error.hpp" + +using namespace ros2_canopen; + +void DeviceContainer::set_executor(const std::weak_ptr executor) +{ + executor_ = executor; +} + +bool DeviceContainer::init_driver(uint16_t node_id) +{ + RCLCPP_DEBUG(this->get_logger(), "init_driver"); + registered_drivers_[node_id]->set_master( + this->can_master_->get_executor(), this->can_master_->get_master()); + return true; +} + +bool DeviceContainer::load_component( + std::string & package_name, std::string & driver_name, uint16_t node_id, std::string & node_name, + std::vector & params) +{ + ComponentResource component; + std::string resource_index("rclcpp_components"); + std::vector components = + this->get_component_resources(package_name, resource_index); + for (auto it = components.begin(); it != components.end(); ++it) + { + if (it->first.compare(driver_name) == 0) + { + std::shared_ptr factory_node = + this->create_component_factory(*it); + rclcpp::NodeOptions opts; + opts.use_global_arguments(false); + opts.use_intra_process_comms(true); + std::vector remap_rules; + remap_rules.push_back("--ros-args"); + // remap_rules.push_back("--log-level"); + // remap_rules.push_back("debug"); + remap_rules.push_back("-r"); + remap_rules.push_back("__node:=" + node_name); + + opts.arguments(remap_rules); + opts.parameter_overrides(params); + + try + { + auto wrapper = factory_node->create_node_instance(opts); + if (node_name.compare("master") == 0) + { + RCLCPP_INFO(this->get_logger(), "Load master component."); + can_master_ = + std::static_pointer_cast(wrapper.get_node_instance()); + } + else + { + RCLCPP_INFO(this->get_logger(), "Load driver component."); + if (this->lifecycle_operation_) + { + auto node = + std::static_pointer_cast(wrapper.get_node_instance()); + if (!node->is_lifecycle()) + { + std::string execption_string; + execption_string.append("Driver "); + execption_string.append(driver_name); + execption_string.append(" for device "); + execption_string.append(node_name); + execption_string.append(" is not a lifecycle driver while the master is."); + RCLCPP_ERROR(this->get_logger(), execption_string.c_str()); + throw DeviceContainerException(execption_string); + } + else + { + registered_drivers_[node_id] = node; + } + } + else + { + auto node = + std::static_pointer_cast(wrapper.get_node_instance()); + if (node->is_lifecycle()) + { + std::string execption_string; + execption_string.append("Driver "); + execption_string.append(driver_name); + execption_string.append(" for device "); + execption_string.append(node_name); + execption_string.append(" is a lifecycle driver while the master is not."); + RCLCPP_ERROR(this->get_logger(), execption_string.c_str()); + throw DeviceContainerException(execption_string); + } + else + { + registered_drivers_[node_id] = node; + } + } + } + } + catch (const std::exception & ex) + { + // In the case that the component constructor throws an exception, + // rethrow into the following catch block. + throw DeviceContainerException( + "Component constructor threw an exception: " + std::string(ex.what())); + } + catch (...) + { + // In the case that the component constructor throws an exception, + // rethrow into the following catch block. + throw DeviceContainerException("Component constructor threw an exception"); + } + + return true; + } + } + return false; +} + +void DeviceContainer::configure() +{ + if (!this->get_parameter("can_interface_name", can_interface_name_)) + { + throw DeviceContainerException("Fatal: Getting Parameter failed."); + RCLCPP_ERROR(this->get_logger(), "Parameter can_interface_name could not be read."); + } + if (!this->get_parameter("master_config", dcf_txt_)) + { + throw DeviceContainerException("Fatal: Getting Parameter failed."); + RCLCPP_ERROR(this->get_logger(), "Parameter master_config could not be read."); + } + if (!this->get_parameter("master_bin", dcf_bin_)) + { + throw DeviceContainerException("Fatal: Getting Parameter failed."); + RCLCPP_ERROR(this->get_logger(), "Parameter master_bin could not be read."); + } + + if (!this->get_parameter("bus_config", bus_config_)) + { + throw DeviceContainerException("Fatal: Getting Parameter failed."); + RCLCPP_ERROR(this->get_logger(), "Parameter bus_config could not be read."); + } + + if (can_interface_name_.length() == 0) + { + RCLCPP_ERROR(this->get_logger(), "Parameter can_interface_name was not set."); + throw DeviceContainerException("Fatal: Getting Parameter failed."); + } + + if (dcf_txt_.length() == 0) + { + RCLCPP_ERROR(this->get_logger(), "Parameter master_config was not set."); + throw DeviceContainerException("Fatal: Getting Parameter failed."); + } + + if (bus_config_.length() == 0) + { + RCLCPP_ERROR(this->get_logger(), "Parameter bus_config was not set."); + throw DeviceContainerException("Fatal: Getting Parameter failed."); + } + + RCLCPP_INFO(this->get_logger(), "Starting Device Container with:"); + RCLCPP_INFO(this->get_logger(), "\t master_config %s", dcf_txt_.c_str()); + RCLCPP_INFO(this->get_logger(), "\t bus_config %s", bus_config_.c_str()); + RCLCPP_INFO(this->get_logger(), "\t can_interface_name %s", can_interface_name_.c_str()); + + try + { + this->config_ = std::make_unique(bus_config_); + this->config_->init_config(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(this->get_logger(), "Loading bus configuration %s failed", bus_config_.c_str()); + throw DeviceContainerException("Fatal: Loading bus configuration " + bus_config_ + " failed."); + } +} + +bool DeviceContainer::load_master() +{ + RCLCPP_INFO(this->get_logger(), "Loading Master Configuration."); + std::vector devices; + uint32_t count = this->config_->get_all_devices(devices); + bool master_found = false; + + // Find master in configuration + for (auto it = devices.begin(); it != devices.end(); it++) + { + if (it->find("master") != std::string::npos && !master_found) + { + auto node_id = config_->get_entry(*it, "node_id"); + auto driver_name = config_->get_entry(*it, "driver"); + auto package_name = config_->get_entry(*it, "package"); + + if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), "Error: Bus Configuration has incomplete configuration for master"); + return false; + } + std::vector params; + params.push_back(rclcpp::Parameter("container_name", this->get_fully_qualified_name())); + params.push_back(rclcpp::Parameter("master_dcf", this->dcf_txt_)); + params.push_back(rclcpp::Parameter("master_bin", this->dcf_bin_)); + params.push_back(rclcpp::Parameter("can_interface_name", this->can_interface_name_)); + params.push_back(rclcpp::Parameter("node_id", (int)node_id.value())); + params.push_back(rclcpp::Parameter("non_transmit_timeout", 100)); + params.push_back(rclcpp::Parameter("config", config_->dump_device(*it))); + + if (!this->load_component( + package_name.value(), driver_name.value(), node_id.value(), *it, params)) + { + RCLCPP_ERROR(this->get_logger(), "Error: Loading master failed."); + return false; + } + + add_node_to_executor(can_master_->get_node_base_interface()); + can_master_->init(); + master_found = true; + can_master_id_ = node_id.value(); + this->lifecycle_operation_ = can_master_->is_lifecycle(); + } + } + + if (!master_found) + { + RCLCPP_ERROR(this->get_logger(), "Error: Master not in configuration"); + return false; + } + return true; +} + +bool DeviceContainer::load_drivers() +{ + RCLCPP_INFO(this->get_logger(), "Loading Driver Configuration."); + std::vector devices; + uint32_t count = this->config_->get_all_devices(devices); + + for (auto it = devices.begin(); it != devices.end(); it++) + { + if (it->find("master") == std::string::npos && it->find("options") == std::string::npos) + { + auto node_id = config_->get_entry(*it, "node_id"); + auto driver_name = config_->get_entry(*it, "driver"); + auto package_name = config_->get_entry(*it, "package"); + if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), "Error: Bus Configuration has incomplete configuration for %s", + it->c_str()); + return false; + } + + if (node_id.value() == can_master_id_) + { + RCLCPP_ERROR( + this->get_logger(), "Error: Bus Configuration has duplicate entry for node id %i", + node_id.value()); + return false; + } + + if (registered_drivers_.count(node_id.value()) != 0) + { + RCLCPP_ERROR( + this->get_logger(), "Error: Bus Configuration has duplicate entry for node id %i", + node_id.value()); + return false; + } + + RCLCPP_INFO( + this->get_logger(), "Found device %s with driver %s", it->c_str(), + driver_name.value().c_str()); + + std::vector params; + params.push_back(rclcpp::Parameter("container_name", this->get_fully_qualified_name())); + params.push_back(rclcpp::Parameter("node_id", (int)node_id.value())); + params.push_back(rclcpp::Parameter("config", config_->dump_device(*it))); + params.push_back(rclcpp::Parameter("non_transmit_timeout", 100)); + + if (!this->load_component( + package_name.value(), driver_name.value(), node_id.value(), *it, params)) + { + RCLCPP_ERROR( + this->get_logger(), + "Loading driver failed: node_id(%hu), node_name(%s), driver(%s), driver_package(%s)", + node_id.value(), it->c_str(), driver_name.value().c_str(), package_name.value().c_str()); + return false; + } + add_node_to_executor(registered_drivers_[node_id.value()]->get_node_base_interface()); + registered_drivers_[node_id.value()]->init(); + } + } + return true; +} + +bool DeviceContainer::load_manager() +{ + if (this->lifecycle_operation_) + { + RCLCPP_INFO(this->get_logger(), "Loading Manager Configuration."); + auto node_options = rclcpp::NodeOptions(); + node_options.use_global_arguments(false); + + // Set parameters + rclcpp::Parameter container_name("container_name", this->get_fully_qualified_name()); + std::vector params; + params.push_back(container_name); + node_options.parameter_overrides(params); + + // Instantiate Manager + this->lifecycle_manager_ = std::make_unique(node_options); + + // Add to executor + add_node_to_executor(this->lifecycle_manager_->get_node_base_interface()); + + // Initialise based on configuration file. + this->lifecycle_manager_->init(this->config_); + } + return true; +} + +void DeviceContainer::init() +{ + configure(); + if (!this->load_master()) + { + throw DeviceContainerException("Fatal: Loading Master Failed."); + } + if (!this->load_drivers()) + { + throw DeviceContainerException("Fatal: Loading Drivers Failed."); + } + + if (!this->load_manager()) + { + throw DeviceContainerException("Fatal: Loading Manager Failed."); + } +} + +void DeviceContainer::init( + const std::string & can_interface_name, const std::string & master_config, + const std::string & bus_config, const std::string & master_bin) +{ + can_interface_name_ = can_interface_name; + dcf_txt_ = master_config; + dcf_bin_ = master_bin; + bus_config_ = bus_config; + + RCLCPP_INFO(this->get_logger(), "Starting Device Container with:"); + RCLCPP_INFO(this->get_logger(), "\t master_config %s", dcf_txt_.c_str()); + RCLCPP_INFO(this->get_logger(), "\t bus_config %s", bus_config_.c_str()); + + this->config_ = std::make_unique(bus_config_); + this->config_->init_config(); + + if (!this->load_master()) + { + throw DeviceContainerException("Fatal: Loading Master Failed."); + } + if (!this->load_drivers()) + { + throw DeviceContainerException("Fatal: Loading Drivers Failed."); + } + + if (!this->load_manager()) + { + throw DeviceContainerException("Fatal: Loading Manager Failed."); + } +} + +void DeviceContainer::on_list_nodes( + const std::shared_ptr request_header, + const std::shared_ptr request, std::shared_ptr response) +{ + auto components = list_components(); + for (auto it = components.begin(); it != components.end(); ++it) + { + RCLCPP_INFO(this->get_logger(), "%hu - %s", it->first, it->second.c_str()); + response->unique_ids.push_back((uint64_t)it->first); + response->full_node_names.push_back(it->second); + } +} + +std::map DeviceContainer::list_components() +{ + std::map components; + + components[can_master_id_] = can_master_->get_node_base_interface()->get_fully_qualified_name(); + + if (this->lifecycle_operation_) + { + components[256] = lifecycle_manager_->get_node_base_interface()->get_fully_qualified_name(); + } + + for (auto & driver : registered_drivers_) + { + components[driver.first] = driver.second->get_node_base_interface()->get_fully_qualified_name(); + } + + return components; +} + +// int main(int argc, char const *argv[]) +// { +// rclcpp::init(argc, argv); +// auto exec = std::make_shared(); +// auto lifecycle_device_container_node = std::make_shared(exec); +// exec->add_node(lifecycle_device_container_node); +// std::thread spinThread([&exec]() +// { exec->spin(); }); +// std::this_thread::sleep_for(100ms); +// if (lifecycle_device_container_node->init()) +// { +// RCLCPP_INFO(lifecycle_device_container_node->get_logger(), "Initialisation successful."); +// } +// else +// { +// RCLCPP_INFO(lifecycle_device_container_node->get_logger(), "Initialisation failed."); +// } + +// spinThread.join(); +// return 0; +// } + +// bool DeviceContainer::set_remote_paramter(std::string node_name, rclcpp::Parameter ¶m) +// { +// std::string service_name = node_name + "/set_parameters"; +// rclcpp::Client::SharedPtr set_parameter_client; + +// set_parameter_client = node_->create_client( +// service_name); + +// while (!set_parameter_client->wait_for_service(non_transmit_timeout_)) +// { +// if (!rclcpp::ok()) +// { +// RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for init_driver service. +// Exiting."); +// } +// RCLCPP_INFO(node_->get_logger(), "init_driver service not available, waiting again..."); +// } +// auto request = std::make_shared(); +// request->parameters.push_back(param.to_parameter_msg()); + +// auto future_result = demand_set_master_client->async_send_request(request); + +// auto future_status = future_result.wait_for(std::chrono::milliseconds(1000)); +// RCLCPP_DEBUG(node_->get_logger(), "demand_set_master end"); + +// if (future_status == std::future_status::ready) +// { +// future_result.get(); +// } +// } diff --git a/canopen_core/src/device_container_error.cpp b/canopen_core/src/device_container_error.cpp new file mode 100644 index 00000000..33e13de7 --- /dev/null +++ b/canopen_core/src/device_container_error.cpp @@ -0,0 +1,12 @@ +#include "canopen_core/device_container_error.hpp" +#include +namespace ros2_canopen +{ + +char * DeviceContainerException::what() +{ + char * res = new char[1000]; + strcpy(res, what_.c_str()); + return res; +} +} // namespace ros2_canopen diff --git a/canopen_core/src/device_container_node.cpp b/canopen_core/src/device_container_node.cpp index 3ae83f6b..ffa4b740 100644 --- a/canopen_core/src/device_container_node.cpp +++ b/canopen_core/src/device_container_node.cpp @@ -13,308 +13,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "canopen_core/device_container_node.hpp" +#include "canopen_core/device_container.hpp" using namespace ros2_canopen; -void DeviceContainerNode::set_executor(const std::weak_ptr executor) +int main(int argc, char const * argv[]) { - executor_ = executor; + rclcpp::init(argc, argv); + auto exec = std::make_shared(); + auto device_container = std::make_shared(exec); + std::thread spinThread([&device_container]() { device_container->init(); }); + exec->add_node(device_container); + exec->spin(); + spinThread.join(); + device_container->shutdown(); + rclcpp::shutdown(); + return 0; } - -void DeviceContainerNode::add_node_to_executor(const std::string &driver_name, const uint8_t node_id, const std::string &node_name) -{ - if (auto exec = executor_.lock()) - { - exec->add_node(node_wrappers_[node_id].get_node_base_interface(), true); - - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - - active_drivers_.insert({node_name, std::make_pair(node_id, driver_name)}); - - RCLCPP_INFO(this->get_logger(), "Added node of type %s with name \"%s\" for node_id %hhu to executor.", driver_name.c_str(), node_name.c_str(), node_id); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to add %s of type %s to executor", node_name.c_str(), driver_name.c_str()); - } -} - -void DeviceContainerNode::add_driver_to_master(std::string driver_name, uint8_t node_id) -{ - RCLCPP_INFO(this->get_logger(), "Adding %s for node id %u to master loop.", driver_name.c_str(), node_id); - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - can_master_->add_driver(node_instance, node_id); -} - -void DeviceContainerNode::remove_node_from_executor(const std::string &driver_name, const uint8_t node_id, const std::string &node_name) -{ - RCLCPP_INFO(this->get_logger(), "Removing %s", driver_name.c_str()); - if (auto exec = executor_.lock()) - { - exec->remove_node(node_wrappers_[node_id].get_node_base_interface()); - - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - - RCLCPP_INFO(this->get_logger(), "Removed %s of type %s from executor", node_name.c_str(), driver_name.c_str()); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to remove %s of type %s from executor", node_name.c_str(), driver_name.c_str()); - } -} - -void DeviceContainerNode::remove_driver_from_master(uint8_t node_id) -{ - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - can_master_->remove_driver(node_instance, node_id); -} - -bool DeviceContainerNode::load_component(std::string &package_name, std::string &driver_name, uint8_t node_id, std::string &node_name) -{ - ComponentResource component; - std::vector components = this->get_component_resources(package_name); - for (auto it = components.begin(); it != components.end(); ++it) - { - if (it->first.compare(driver_name) == 0) - { - auto factory_node = this->create_component_factory(*it); - rclcpp::NodeOptions opts; - opts.use_global_arguments(false); - std::vector remap_rules; - remap_rules.push_back("--ros-args"); - remap_rules.push_back("-r"); - remap_rules.push_back("__node:=" + node_name); - opts.arguments(remap_rules); - - try - { - node_wrappers_[node_id] = factory_node->create_node_instance(opts); - } - catch (const std::exception &ex) - { - // In the case that the component constructor throws an exception, - // rethrow into the following catch block. - throw rclcpp_components::ComponentManagerException( - "Component constructor threw an exception: " + std::string(ex.what())); - } - catch (...) - { - // In the case that the component constructor throws an exception, - // rethrow into the following catch block. - throw rclcpp_components::ComponentManagerException("Component constructor threw an exception"); - } - - return true; - } - } - return false; -} - -std::map DeviceContainerNode::list_components() -{ - std::map components; - for (auto &wrapper : node_wrappers_) - { - components[wrapper.first] = - wrapper.second.get_node_base_interface()->get_fully_qualified_name(); - } - return components; -} - -bool DeviceContainerNode::add_master(uint8_t node_id) -{ - RCLCPP_INFO(this->get_logger(), "Adding master with node id %u", node_id); - - can_master_ = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - can_master_->init(dcf_txt_, dcf_bin_, can_interface_name_, node_id, config_); - return true; -} - -bool DeviceContainerNode::init_devices_from_config() -{ - std::vector devices; - uint32_t count = this->config_->get_all_devices(devices); - RCLCPP_INFO(this->get_logger(), "Found %u devices", count); - bool master_found = false; - - for (auto it = devices.begin(); it != devices.end(); it++) - { - if (it->find("master") != std::string::npos && !master_found) - { - RCLCPP_INFO(this->get_logger(), "Found Master."); - auto node_id = config_->get_entry(*it, "node_id"); - auto driver_name = config_->get_entry(*it, "driver"); - auto package_name = config_->get_entry(*it, "package"); - - if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value()) - { - RCLCPP_ERROR(this->get_logger(), "Error: Bus Configuration has uncomplete configuration for master"); - return false; - } - - if (!this->load_component(package_name.value(), driver_name.value(), node_id.value(), *it)) - { - RCLCPP_ERROR(this->get_logger(), "Error: Loading master failed."); - return false; - } - add_node_to_executor(driver_name.value(), node_id.value(), *it); - add_master(node_id.value()); - master_found = true; - } - } - - if (!master_found) - { - RCLCPP_ERROR(this->get_logger(), "Error: Master not in configuration"); - return false; - } - - for (auto it = devices.begin(); it != devices.end(); it++) - { - if (it->find("master") == std::string::npos) - { - auto node_id = config_->get_entry(*it, "node_id"); - auto driver_name = config_->get_entry(*it, "driver"); - auto package_name = config_->get_entry(*it, "package"); - auto enable_lazy_load = config_->get_entry(*it, "enable_lazy_load"); - if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value() || !enable_lazy_load.has_value()) - { - RCLCPP_ERROR(this->get_logger(), "Error: Bus Configuration has uncomplete configuration for %s", it->c_str()); - return false; - } - auto res = this->registered_drivers_.emplace(*it, std::make_pair(node_id.value(), driver_name.value())); - if (!res.second) - { - RCLCPP_ERROR(this->get_logger(), "Error: Bus Configuration has duplicate configuration for %s", it->c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "Found Driver %s with lazy_load %s", driver_name.value().c_str(), enable_lazy_load.value() ? "true" : "false"); - if (!enable_lazy_load.value()) - { - this->load_component(package_name.value(), driver_name.value(), node_id.value(), *it); - add_node_to_executor(driver_name.value(), node_id.value(), *it); - this->add_driver_to_master(driver_name.value(), node_id.value()); - } - } - } - - - auto components = list_components(); - RCLCPP_INFO(this->get_logger(), "List of active components:"); - for (auto it = components.begin(); it != components.end(); ++it) - { - RCLCPP_INFO(this->get_logger(), "%i : %s", it->first, it->second.c_str()); - } - return true; -} - -bool DeviceContainerNode::init() -{ - this->get_parameter("can_interface_name", can_interface_name_); - this->get_parameter("master_config", dcf_txt_); - this->get_parameter("master_bin", dcf_bin_); - this->get_parameter("bus_config", bus_config_); - - this->config_ = std::make_shared(bus_config_); - this->config_->init_config(); - this->init_devices_from_config(); - return true; -} - -void DeviceContainerNode::on_load_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -{ - (void)request_header; - std::string package_name = request->package_name; - std::string driver_name = request->plugin_name; - std::string node_name = request->node_name; - - if (node_name.empty()) - { - response->error_message = "Node name cannot be empty. To pass node name use '-n' option."; - response->success = false; - return; - } - - auto registered_it = registered_drivers_.find(node_name); - auto active_it = active_drivers_.find(node_name); - if (registered_it == registered_drivers_.end()) - { - response->error_message = "No node registered with the name " + node_name + "."; - response->success = false; - return; - } - if (active_it != active_drivers_.end()) - { - response->error_message = node_name + " is already loaded."; - response->success = false; - return; - } - - // pair of {node_id, driver_name} - auto node_id = registered_drivers_[node_name].first; - - bool is_loaded = this->load_component(package_name, driver_name, node_id, node_name); - - if (is_loaded) - { - add_node_to_executor(driver_name, node_id, node_name); - this->add_driver_to_master(driver_name, node_id); - response->full_node_name = node_name; - response->unique_id = node_id; - response->success = true; - } - else - { - response->error_message = "Failed to find class with the requested plugin name."; - response->success = false; - } - -} - -void DeviceContainerNode::on_unload_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -{ -} - -void DeviceContainerNode::on_list_nodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -{ - auto components = list_components(); - // RCLCPP_INFO(this->get_logger(), "List of active components:"); - for (auto it = components.begin(); it != components.end(); ++it) - { - // RCLCPP_INFO(this->get_logger(), "%i : %s", it->first, it->second.c_str()); - response->unique_ids.push_back(it->first); - response->full_node_names.push_back(it->second); - } -} - -int main(int argc, char const *argv[]) -{ - rclcpp::init(argc, argv); - auto exec = std::make_shared(); - auto device_manager = std::make_shared(exec); - std::thread spinThread([&device_manager]() - { - if(device_manager->init()) - { - RCLCPP_INFO(device_manager->get_logger(), "Initialisation successful."); - } - else - { - RCLCPP_INFO(device_manager->get_logger(), "Initialisation failed."); - } - }); - exec->add_node(device_manager); - exec->spin(); - spinThread.join(); - return 0; -} \ No newline at end of file diff --git a/canopen_core/src/driver_error.cpp b/canopen_core/src/driver_error.cpp new file mode 100644 index 00000000..8fae1cb8 --- /dev/null +++ b/canopen_core/src/driver_error.cpp @@ -0,0 +1,11 @@ +#include "canopen_core/driver_error.hpp" +#include +namespace ros2_canopen +{ +char * DriverException::what() +{ + char * res = new char[1000]; + strcpy(res, what_.c_str()); + return res; +} +} // namespace ros2_canopen diff --git a/canopen_core/src/driver_node.cpp b/canopen_core/src/driver_node.cpp new file mode 100644 index 00000000..3fdb9071 --- /dev/null +++ b/canopen_core/src/driver_node.cpp @@ -0,0 +1,104 @@ +#include "canopen_core/driver_node.hpp" + +using namespace ros2_canopen; +void CanopenDriver::init() +{ + node_canopen_driver_->init(); + node_canopen_driver_->configure(); + bool success = false; + for (int i = 0; i < 5; i++) + { + try + { + node_canopen_driver_->demand_set_master(); + success = true; + break; + } + catch (std::exception & e) + { + RCLCPP_WARN( + this->get_logger(), "Failed to get demand set master result becauss %s. Retrying.", + e.what()); + } + } + if (!success) + { + RCLCPP_WARN(this->get_logger(), "Failed to get demand set master result. Exiting..."); + throw DriverException("Failed to get demand set master result. Exiting..."); + } + node_canopen_driver_->activate(); +} + +void CanopenDriver::shutdown() { node_canopen_driver_->shutdown(); } + +void CanopenDriver::set_master( + std::shared_ptr exec, std::shared_ptr master) +{ + node_canopen_driver_->set_master(exec, master); +} + +void LifecycleCanopenDriver::init() { node_canopen_driver_->init(); } + +void LifecycleCanopenDriver::shutdown() { node_canopen_driver_->shutdown(); } + +void LifecycleCanopenDriver::set_master( + std::shared_ptr exec, std::shared_ptr master) +{ + node_canopen_driver_->set_master(exec, master); +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenDriver::on_configure(const rclcpp_lifecycle::State & state) +{ + node_canopen_driver_->configure(); + bool success = false; + for (int i = 0; i < 5; i++) + { + try + { + node_canopen_driver_->demand_set_master(); + success = true; + break; + } + catch (std::exception & e) + { + RCLCPP_WARN( + this->get_logger(), "Failed to get demand set master result becauss %s. Retrying.", + e.what()); + } + } + if (!success) + { + RCLCPP_WARN(this->get_logger(), "Failed to get demand set master result. Exiting..."); + throw DriverException("Failed to get demand set master result. Exiting..."); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenDriver::on_activate(const rclcpp_lifecycle::State & state) +{ + node_canopen_driver_->activate(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenDriver::on_deactivate(const rclcpp_lifecycle::State & state) +{ + node_canopen_driver_->deactivate(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenDriver::on_cleanup(const rclcpp_lifecycle::State & state) +{ + node_canopen_driver_->cleanup(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenDriver::on_shutdown(const rclcpp_lifecycle::State & state) +{ + node_canopen_driver_->shutdown(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} diff --git a/canopen_core/src/lely_master_bridge.cpp b/canopen_core/src/lely_master_bridge.cpp deleted file mode 100644 index 841f15a9..00000000 --- a/canopen_core/src/lely_master_bridge.cpp +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include "canopen_core/lely_master_bridge.hpp" - -#include - -using namespace std::literals::chrono_literals; - -namespace ros2_canopen -{ - std::future LelyMasterBridge::async_write_sdo(uint8_t id, COData data) - { - std::unique_lock lck(sdo_mutex); - if (running) - { - sdo_cond.wait(lck); - } - running = true; - - sdo_write_data_promise = std::make_shared>(); - if (data.type_ == CODataTypes::COData8) - { - this->SubmitWrite( - this->GetExecutor(), - id, - data.index_, data.subindex_, (uint8_t)data.data_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec) mutable - { - if (ec) - { - this->sdo_write_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, - idx, - subidx, - ec, - "AsyncDownload")); - } - else - { - this->sdo_write_data_promise->set_value(true); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - else if (data.type_ == CODataTypes::COData16) - { - this->SubmitWrite( - this->GetExecutor(), - id, - data.index_, data.subindex_, (uint16_t)data.data_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec) mutable - { - if (ec) - { - this->sdo_write_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncDownload")); - } - else - { - this->sdo_write_data_promise->set_value(true); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - else if (data.type_ == CODataTypes::COData32) - { - this->SubmitWrite( - this->GetExecutor(), - id, - data.index_, data.subindex_, (uint32_t)data.data_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec) mutable - { - if (ec) - { - this->sdo_write_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncDownload")); - } - else - { - this->sdo_write_data_promise->set_value(true); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - - return sdo_write_data_promise->get_future(); - } - - std::future LelyMasterBridge::async_read_sdo(uint8_t id, COData data) - { - std::unique_lock lck(sdo_mutex); - if (running) - { - sdo_cond.wait(lck); - } - running = true; - - sdo_read_data_promise = std::make_shared>(); - if (data.type_ == CODataTypes::COData8) - { - this->SubmitRead( - this->GetExecutor(), - id, - data.index_, data.subindex_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec, uint8_t value) mutable - { - if (ec) - { - this->sdo_read_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncUpload")); - } - else - { - COData d = {idx, subidx, value, CODataTypes::COData16}; - this->sdo_read_data_promise->set_value(d); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - else if (data.type_ == CODataTypes::COData16) - { - this->SubmitRead( - this->GetExecutor(), - id, - data.index_, data.subindex_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec, uint16_t value) mutable - { - if (ec) - { - this->sdo_read_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncUpload")); - } - else - { - COData d = {idx, subidx, value, CODataTypes::COData16}; - this->sdo_read_data_promise->set_value(d); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - else if (data.type_ == CODataTypes::COData32) - { - this->SubmitRead( - this->GetExecutor(), - id, - data.index_, data.subindex_, - [this](uint8_t id, uint16_t idx, uint8_t subidx, - ::std::error_code ec, uint32_t value) mutable - { - if (ec) - { - this->sdo_read_data_promise->set_exception( - lely::canopen::make_sdo_exception_ptr( - id, idx, subidx, ec, "AsyncUpload")); - } - else - { - COData d = {idx, subidx, value, CODataTypes::COData16}; - this->sdo_read_data_promise->set_value(d); - } - std::unique_lock lck(this->sdo_mutex); - this->running = false; - this->sdo_cond.notify_one(); - }, - 20ms); - } - return sdo_read_data_promise->get_future(); - } - - std::future LelyMasterBridge::async_write_nmt(uint8_t id, uint8_t command) - { - lely::canopen::NmtCommand command_ = static_cast(command); - switch (command_) - { - case lely::canopen::NmtCommand::ENTER_PREOP: - case lely::canopen::NmtCommand::RESET_COMM: - case lely::canopen::NmtCommand::RESET_NODE: - case lely::canopen::NmtCommand::START: - case lely::canopen::NmtCommand::STOP: - this->Command(command_, id); - break; - default: - break; - } - - nmt_promise.set_value(true); - return nmt_promise.get_future(); - } -} \ No newline at end of file diff --git a/canopen_core/src/lifecycle_device_container_node.cpp b/canopen_core/src/lifecycle_device_container_node.cpp deleted file mode 100644 index 6390b59a..00000000 --- a/canopen_core/src/lifecycle_device_container_node.cpp +++ /dev/null @@ -1,348 +0,0 @@ -// Copyright 2022 Harshavadan Deshpande -// Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "canopen_core/lifecycle_device_container_node.hpp" - -using namespace ros2_canopen; - -void LifecycleDeviceContainerNode::set_executor(const std::weak_ptr executor) -{ - executor_ = executor; -} - -void LifecycleDeviceContainerNode::add_node_to_executor(const std::string &driver_name, const uint16_t node_id, const std::string &node_name) -{ - if (auto exec = executor_.lock()) - { - exec->add_node(node_wrappers_[node_id].get_node_base_interface(), true); - - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - - active_drivers_.insert({node_name, std::make_pair(node_id, driver_name)}); - - RCLCPP_INFO(this->get_logger(), "Added node of type %s with name \"%s\" for node_id %hu to executor.", driver_name.c_str(), node_name.c_str(), node_id); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to add %s of type %s to executor", node_name.c_str(), driver_name.c_str()); - } -} - -void LifecycleDeviceContainerNode::remove_node_from_executor(const std::string &driver_name, const uint16_t node_id, const std::string &node_name) -{ - RCLCPP_INFO(this->get_logger(), "Removing %s", driver_name.c_str()); - if (auto exec = executor_.lock()) - { - exec->remove_node(node_wrappers_[node_id].get_node_base_interface()); - - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - - RCLCPP_INFO(this->get_logger(), "Removed %s of type %s from executor", node_name.c_str(), driver_name.c_str()); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to remove %s of type %s from executor", node_name.c_str(), driver_name.c_str()); - } -} - -bool LifecycleDeviceContainerNode::init_driver(uint16_t node_id) -{ - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - can_master_->init_driver(node_instance, node_id); - return true; -} - -bool LifecycleDeviceContainerNode::load_component(std::string &package_name, std::string &driver_name, uint16_t node_id, std::string &node_name) -{ - ComponentResource component; - std::vector components = this->get_component_resources(package_name); - for (auto it = components.begin(); it != components.end(); ++it) - { - if (it->first.compare(driver_name) == 0) - { - auto factory_node = this->create_component_factory(*it); - rclcpp::NodeOptions opts; - opts.use_global_arguments(false); - std::vector remap_rules; - remap_rules.push_back("--ros-args"); - remap_rules.push_back("-r"); - remap_rules.push_back("__node:=" + node_name); - opts.arguments(remap_rules); - - try - { - node_wrappers_[node_id] = factory_node->create_node_instance(opts); - } - catch (const std::exception &ex) - { - // In the case that the component constructor throws an exception, - // rethrow into the following catch block. - throw rclcpp_components::ComponentManagerException( - "Component constructor threw an exception: " + std::string(ex.what())); - } - catch (...) - { - // In the case that the component constructor throws an exception, - // rethrow into the following catch block. - throw rclcpp_components::ComponentManagerException("Component constructor threw an exception"); - } - - return true; - } - } - return false; -} - -std::map LifecycleDeviceContainerNode::list_components() -{ - std::map components; - for (auto &wrapper : node_wrappers_) - { - components[wrapper.first] = - wrapper.second.get_node_base_interface()->get_fully_qualified_name(); - } - return components; -} - -bool LifecycleDeviceContainerNode::init_master(uint16_t node_id) -{ - RCLCPP_INFO(this->get_logger(), "Adding master with node id %u", node_id); - can_master_ = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - can_master_->init(); - can_master_->set_parameter(rclcpp::Parameter("master_config", this->dcf_txt_)); - can_master_->set_parameter(rclcpp::Parameter("master_bin", this->dcf_bin_)); - can_master_->set_parameter(rclcpp::Parameter("can_interface_name", this->can_interface_name_)); - can_master_->set_parameter(rclcpp::Parameter("node_id", node_id)); - RCLCPP_INFO(this->get_logger(), "Added master with node id %u", node_id); - return true; -} - -bool LifecycleDeviceContainerNode::init_device_manager(uint16_t node_id) -{ - RCLCPP_INFO(this->get_logger(), "Initialising device_manager with node id %u", node_id); - auto device_manager = std::static_pointer_cast(node_wrappers_[node_id].get_node_instance()); - device_manager->init(this->config_); - device_manager->set_parameter(rclcpp::Parameter("container_name", this->get_fully_qualified_name())); - return true; -} - -bool LifecycleDeviceContainerNode::load_master_from_config() -{ - RCLCPP_INFO(this->get_logger(), "Loading Master Configuration."); - std::vector devices; - uint32_t count = this->config_->get_all_devices(devices); - bool master_found = false; - - // Find master in configuration - for (auto it = devices.begin(); it != devices.end(); it++) - { - if (it->find("master") != std::string::npos && !master_found) - { - auto node_id = config_->get_entry(*it, "node_id"); - auto driver_name = config_->get_entry(*it, "driver"); - auto package_name = config_->get_entry(*it, "package"); - - if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value()) - { - RCLCPP_ERROR(this->get_logger(), "Error: Bus Configuration has uncomplete configuration for master"); - return false; - } - auto res = this->registered_drivers_.emplace(*it, std::make_pair(node_id.value(), driver_name.value())); - if (!res.second) - { - RCLCPP_ERROR(this->get_logger(), "Error: Bus Configuration has duplicate configuration for %s", it->c_str()); - return false; - } - - if (!this->load_component(package_name.value(), driver_name.value(), node_id.value(), *it)) - { - RCLCPP_ERROR(this->get_logger(), "Error: Loading master failed."); - return false; - } - add_node_to_executor(driver_name.value(), node_id.value(), *it); - init_master(node_id.value()); - master_found = true; - } - } - - if (!master_found) - { - RCLCPP_ERROR(this->get_logger(), "Error: Master not in configuration"); - return false; - } - return true; -} - -bool LifecycleDeviceContainerNode::load_drivers_from_config() -{ - RCLCPP_INFO(this->get_logger(), "Loading Driver Configuration."); - std::vector devices; - uint32_t count = this->config_->get_all_devices(devices); - for (auto it = devices.begin(); it != devices.end(); it++) - { - if (it->find("master") == std::string::npos) - { - auto node_id = config_->get_entry(*it, "node_id"); - auto driver_name = config_->get_entry(*it, "driver"); - auto package_name = config_->get_entry(*it, "package"); - if (!node_id.has_value() || !driver_name.has_value() || !package_name.has_value()) - { - RCLCPP_ERROR(this->get_logger(), "Error: Bus Configuration has uncomplete configuration for %s", it->c_str()); - return false; - } - auto res = this->registered_drivers_.emplace(*it, std::make_pair(node_id.value(), driver_name.value())); - if (!res.second) - { - RCLCPP_ERROR(this->get_logger(), "Error: Bus Configuration has duplicate configuration for %s", it->c_str()); - return false; - } - - RCLCPP_INFO(this->get_logger(), "Found device %s with driver %s", it->c_str(), driver_name.value().c_str()); - - if (!this->load_component(package_name.value(), driver_name.value(), node_id.value(), *it)) - { - RCLCPP_ERROR(this->get_logger(), "Error: Loading master failed."); - return false; - } - - add_node_to_executor(driver_name.value(), node_id.value(), *it); - auto node_instance = std::static_pointer_cast(node_wrappers_[node_id.value()].get_node_instance()); - node_instance->init(); - node_instance->set_parameter(rclcpp::Parameter("node_id", node_id.value())); - node_instance->set_parameter(rclcpp::Parameter("container_name", this->get_fully_qualified_name())); - //init_driver(node_id.value()); - } - } - return true; -} - -bool LifecycleDeviceContainerNode::load_manager() -{ - RCLCPP_INFO(this->get_logger(), "Loading Manager Configuration."); - std::string package = "canopen_core"; - std::string driver = "ros2_canopen::LifecycleDeviceManagerNode"; - uint16_t id = 256; - std::string name = "lifecycle_device_manager_node"; - if (!this->load_component(package, driver, 256, name)) - { - RCLCPP_ERROR(this->get_logger(), "Error: Loading device_manager failed."); - return false; - } - add_node_to_executor(driver, id, name); - init_device_manager(id); - return true; -} - -bool LifecycleDeviceContainerNode::init() -{ - if (!this->get_parameter("can_interface_name", can_interface_name_)) - { - RCLCPP_ERROR(this->get_logger(), "Parameter can_interface_name could not be read."); - } - if (!this->get_parameter("master_config", dcf_txt_)) - { - RCLCPP_ERROR(this->get_logger(), "Parameter master_config could not be read."); - } - if (!this->get_parameter("master_bin", dcf_bin_)) - { - RCLCPP_ERROR(this->get_logger(), "Parameter master_bin could not be read."); - } - - if (!this->get_parameter("bus_config", bus_config_)) - { - RCLCPP_ERROR(this->get_logger(), "Parameter bus_config could not be read."); - } - - RCLCPP_INFO(this->get_logger(), "Starting Device Container with:"); - RCLCPP_INFO(this->get_logger(), "\t master_config %s", dcf_txt_.c_str()); - RCLCPP_INFO(this->get_logger(), "\t bus_config %s", bus_config_.c_str()); - - this->config_ = std::make_shared(bus_config_); - this->config_->init_config(); - - if (!this->load_master_from_config()) - { - return false; - } - if (!this->load_drivers_from_config()) - { - return false; - } - - if (!this->load_manager()) - { - return false; - } - - return true; -} - -void LifecycleDeviceContainerNode::on_load_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -{ - RCLCPP_ERROR(this->get_logger(), "Cannot load nodes."); - response->error_message = "Device container can not load nodes."; - response->success = false; -} - -void LifecycleDeviceContainerNode::on_unload_node( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -{ - RCLCPP_ERROR(this->get_logger(), "Cannot unload nodes."); - response->error_message = "Device container can not unload nodes."; - response->success = false; -} - -void LifecycleDeviceContainerNode::on_list_nodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) -{ - auto components = list_components(); - // RCLCPP_INFO(this->get_logger(), "List of active components:"); - for (auto it = components.begin(); it != components.end(); ++it) - { - // RCLCPP_INFO(this->get_logger(), "%i : %s", it->first, it->second.c_str()); - response->unique_ids.push_back(it->first); - response->full_node_names.push_back(it->second); - } -} - -int main(int argc, char const *argv[]) -{ - rclcpp::init(argc, argv); - auto exec = std::make_shared(); - auto lifecycle_device_container_node = std::make_shared(exec); - exec->add_node(lifecycle_device_container_node); - std::thread spinThread([&exec]() { - exec->spin(); - }); - std::this_thread::sleep_for(100ms); - if (lifecycle_device_container_node->init()) - { - RCLCPP_INFO(lifecycle_device_container_node->get_logger(), "Initialisation successful."); - } - else - { - RCLCPP_INFO(lifecycle_device_container_node->get_logger(), "Initialisation failed."); - } - - spinThread.join(); - return 0; -} diff --git a/canopen_core/src/lifecycle_device_manager_node.cpp b/canopen_core/src/lifecycle_device_manager_node.cpp deleted file mode 100644 index fb60699d..00000000 --- a/canopen_core/src/lifecycle_device_manager_node.cpp +++ /dev/null @@ -1,314 +0,0 @@ -#include "canopen_core/lifecycle_device_manager_node.hpp" - -namespace ros2_canopen -{ - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleDeviceManagerNode::on_configure(const rclcpp_lifecycle::State &state) - { - if (!this->has_parameter("container_name")) - { - this->declare_parameter("container_name", "lifecycle_device_container_node"); - } - - this->get_parameter("container_name", this->container_name_); - - bool res = this->load_from_config(); - if (!res) - { - RCLCPP_ERROR(this->get_logger(), "Failed to load from config"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleDeviceManagerNode::on_activate(const rclcpp_lifecycle::State &state) - { - if (!this->bring_up_all()) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleDeviceManagerNode::on_deactivate(const rclcpp_lifecycle::State &state) - { - if (this->bring_down_all()) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleDeviceManagerNode::on_cleanup(const rclcpp_lifecycle::State &state) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleDeviceManagerNode::on_shutdown(const rclcpp_lifecycle::State &state) - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - void - LifecycleDeviceManagerNode::init(std::shared_ptr config) - { - this->config_ = config; - } - - bool - LifecycleDeviceManagerNode::load_from_config() - { - - std::vector devices; - uint32_t count = this->config_->get_all_devices(devices); - RCLCPP_INFO(this->get_logger(), "Configuring for %u devices.", count); - - // Find master in configuration - for (auto it = devices.begin(); it != devices.end(); it++) - { - uint8_t node_id = config_->get_entry(*it, "node_id").value(); - std::string change_state_client_name = *it; - std::string get_state_client_name = *it; - get_state_client_name += "/get_state"; - change_state_client_name += "/change_state"; - RCLCPP_INFO(this->get_logger(), "Found %s (node_id=%hu)", it->c_str(), node_id); - device_names_to_ids.emplace(*it, node_id); - rclcpp::Client::SharedPtr get_state_client = - this->create_client(get_state_client_name, rmw_qos_profile_services_default, cbg_clients); - - rclcpp::Client::SharedPtr change_state_client = - this->create_client(change_state_client_name, rmw_qos_profile_services_default, cbg_clients); - - this->drivers_get_state_clients.emplace(node_id, get_state_client); - this->drivers_change_state_clients.emplace(node_id, change_state_client); - - if (it->find("master") != std::string::npos) - { - this->master_id_ = node_id; - } - } - return true; - } - - unsigned int - LifecycleDeviceManagerNode::get_state(uint8_t node_id, std::chrono::seconds time_out) - { - auto request = std::make_shared(); - auto client = this->drivers_get_state_clients[node_id]; - if (!client->wait_for_service(time_out)) - { - RCLCPP_ERROR( - get_logger(), - "Service %s is not available.", - client->get_service_name()); - return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; - } - - // We send the service request for asking the current - // state of the lc_talker node. - auto future_result = client->async_send_request(request); - - // Let's wait until we have the answer from the node. - // If the request times out, we return an unknown state. - auto future_status = wait_for_result(future_result, time_out); - - if (future_status != std::future_status::ready) - { - RCLCPP_ERROR( - get_logger(), "Server time out while getting current state for node %hhu", node_id); - return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; - } - auto result = future_result.get()->current_state; - return result.id; - } - - bool - LifecycleDeviceManagerNode::change_state(uint8_t node_id, uint8_t transition, std::chrono::seconds time_out) - { - auto client = this->drivers_change_state_clients[node_id]; - auto request = std::make_shared(); - request->transition.id = transition; - if (!client->wait_for_service(time_out)) - { - RCLCPP_ERROR( - get_logger(), - "Service %s is not available.", - client->get_service_name()); - return false; - } - - // We send the request with the transition we want to invoke. - auto future_result = client->async_send_request(request); - - // Let's wait until we have the answer from the node. - // If the request times out, we return an unknown state. - auto future_status = wait_for_result(future_result, time_out); - - if (future_status != std::future_status::ready) - { - RCLCPP_ERROR( - get_logger(), "Server time out while getting current state for node %hhu", node_id); - return false; - } - - if (future_result.get()->success) - { - RCLCPP_INFO( - get_logger(), "Transition %d successfully triggered.", static_cast(transition)); - return true; - } - else - { - RCLCPP_WARN( - get_logger(), "Failed to trigger transition %u", static_cast(transition)); - return false; - } - return false; - } - - bool - LifecycleDeviceManagerNode::bring_up_master() - { - auto state = this->get_state(master_id_); - if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - RCLCPP_ERROR(this->get_logger(), "Failed to bring up master. Master not in unconfigured state."); - return false; - } - RCLCPP_INFO(this->get_logger(), "Master (node_id=%hu) has state unconfigured.", master_id_); - if (!this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)) - { - RCLCPP_ERROR(this->get_logger(), "Failed to bring up master. Configure Transition failed."); - return false; - } - RCLCPP_INFO(this->get_logger(), "Master (node_id=%hu) has state inactive.", master_id_); - if (!this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) - { - RCLCPP_ERROR(this->get_logger(), "Failed to bring up master. Activate Transition failed."); - return false; - } - RCLCPP_INFO(this->get_logger(), "Master (node_id=%hu) has state active.", master_id_); - return true; - } - - bool - LifecycleDeviceManagerNode::bring_down_master() - { - this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); - this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); - - auto state = this->get_state(master_id_); - - if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - return false; - } - - return true; - } - - bool - LifecycleDeviceManagerNode::bring_up_driver(std::string device_name) - { - - auto node_id = this->device_names_to_ids[device_name]; - RCLCPP_INFO(this->get_logger(), "Bringing up %s with id %u", device_name.c_str(), node_id); - auto master_state = this->get_state(master_id_); - auto state = this->get_state(node_id); - - if (master_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - RCLCPP_ERROR(this->get_logger(), "Failed to bring up %s. Master not in active state.", device_name.c_str()); - return false; - } - - if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - RCLCPP_ERROR(this->get_logger(), "Failed to bring up %s. Not in unconfigured state.", device_name.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "%s (node_id=%hu) has state unconfigured. Attempting to configure.", device_name.c_str(), node_id); - if (!this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)) - { - RCLCPP_ERROR(this->get_logger(), "Failed to bring up %s. Configure Transition failed.", device_name.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "%s (node_id=%hu) has state inactive. Attempting to activate.", device_name.c_str(), node_id); - if (!this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) - { - RCLCPP_ERROR(this->get_logger(), "Failed to bring up %s. Activate Transition failed.", device_name.c_str()); - return false; - } - RCLCPP_INFO(this->get_logger(), "%s (node_id=%hu) has state active.", device_name.c_str(), node_id); - return true; - } - - bool - LifecycleDeviceManagerNode::bring_down_driver(std::string device_name) - { - auto node_id = this->device_names_to_ids[device_name]; - auto master_state = this->get_state(master_id_); - - this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); - this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); - auto state = this->get_state(node_id); - if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - return false; - } - return true; - } - - bool - LifecycleDeviceManagerNode::bring_up_all() - { - if (!this->bring_up_master()) - { - return false; - } - for (auto it = this->device_names_to_ids.begin(); it != this->device_names_to_ids.end(); ++it) - { - if (it->first.find("master") == std::string::npos) - { - if (!this->bring_up_driver(it->first)) - { - return false; - } - } - else{ - RCLCPP_INFO(this->get_logger(), "Skipped master."); - } - } - return true; - } - - bool - LifecycleDeviceManagerNode::bring_down_all() - { - for (auto it = this->device_names_to_ids.begin(); it != this->device_names_to_ids.end(); ++it) - { - if (it->first.find("master") != std::string::npos) - { - if (!this->bring_down_driver(it->first)) - { - return false; - } - } - } - if (!this->bring_down_master()) - { - return false; - } - - return true; - } - -} - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleDeviceManagerNode) \ No newline at end of file diff --git a/canopen_core/src/lifecycle_manager.cpp b/canopen_core/src/lifecycle_manager.cpp new file mode 100644 index 00000000..cd9b9b43 --- /dev/null +++ b/canopen_core/src/lifecycle_manager.cpp @@ -0,0 +1,304 @@ +#include "canopen_core/lifecycle_manager.hpp" + +namespace ros2_canopen +{ + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_configure(const rclcpp_lifecycle::State & state) +{ + this->get_parameter("container_name", this->container_name_); + + bool res = this->load_from_config(); + if (!res) + { + RCLCPP_ERROR(this->get_logger(), "Failed to load from config"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_activate(const rclcpp_lifecycle::State & state) +{ + if (!this->bring_up_all()) + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_deactivate(const rclcpp_lifecycle::State & state) +{ + if (!this->bring_down_all()) + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_cleanup(const rclcpp_lifecycle::State & state) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_shutdown(const rclcpp_lifecycle::State & state) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +void LifecycleManager::init(std::shared_ptr config) +{ + this->config_ = config; +} + +bool LifecycleManager::load_from_config() +{ + std::vector devices; + uint32_t count = this->config_->get_all_devices(devices); + RCLCPP_INFO(this->get_logger(), "Configuring for %u devices.", count); + + // Find master in configuration + for (auto it = devices.begin(); it != devices.end(); it++) + { + uint8_t node_id = config_->get_entry(*it, "node_id").value(); + std::string change_state_client_name = *it; + std::string get_state_client_name = *it; + get_state_client_name += "/get_state"; + change_state_client_name += "/change_state"; + RCLCPP_INFO(this->get_logger(), "Found %s (node_id=%hu)", it->c_str(), node_id); + device_names_to_ids.emplace(*it, node_id); + rclcpp::Client::SharedPtr get_state_client = + this->create_client( + get_state_client_name, rmw_qos_profile_services_default, cbg_clients); + + rclcpp::Client::SharedPtr change_state_client = + this->create_client( + change_state_client_name, rmw_qos_profile_services_default, cbg_clients); + + this->drivers_get_state_clients.emplace(node_id, get_state_client); + this->drivers_change_state_clients.emplace(node_id, change_state_client); + + if (it->find("master") != std::string::npos) + { + this->master_id_ = node_id; + } + } + return true; +} + +unsigned int LifecycleManager::get_state(uint8_t node_id, std::chrono::seconds time_out) +{ + auto request = std::make_shared(); + auto client = this->drivers_get_state_clients[node_id]; + if (!client->wait_for_service(time_out)) + { + RCLCPP_ERROR(get_logger(), "Service %s is not available.", client->get_service_name()); + return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; + } + + // We send the service request for asking the current + // state of the lc_talker node. + auto future_result = client->async_send_request(request); + + // Let's wait until we have the answer from the node. + // If the request times out, we return an unknown state. + auto future_status = wait_for_result(future_result, time_out); + + if (future_status != std::future_status::ready) + { + RCLCPP_ERROR( + get_logger(), "Server time out while getting current state for node %hhu", node_id); + return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; + } + auto result = future_result.get()->current_state; + return result.id; +} + +bool LifecycleManager::change_state( + uint8_t node_id, uint8_t transition, std::chrono::seconds time_out) +{ + auto client = this->drivers_change_state_clients[node_id]; + auto request = std::make_shared(); + request->transition.id = transition; + if (!client->wait_for_service(time_out)) + { + RCLCPP_ERROR(get_logger(), "Service %s is not available.", client->get_service_name()); + return false; + } + + // We send the request with the transition we want to invoke. + auto future_result = client->async_send_request(request); + + // Let's wait until we have the answer from the node. + // If the request times out, we return an unknown state. + auto future_status = wait_for_result(future_result, time_out); + + if (future_status != std::future_status::ready) + { + RCLCPP_ERROR( + get_logger(), "Server time out while getting current state for node %hhu", node_id); + return false; + } + + if (future_result.get()->success) + { + return true; + } + else + { + RCLCPP_WARN( + get_logger(), "Failed to trigger transition %u", static_cast(transition)); + return false; + } + return false; +} + +bool LifecycleManager::bring_up_master() +{ + auto state = this->get_state(master_id_, 3s); + if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + this->get_logger(), "Failed to bring up master. Master not in unconfigured state."); + return false; + } + RCLCPP_DEBUG(this->get_logger(), "Master (node_id=%hu) has state unconfigured.", master_id_); + if (!this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, 3s)) + { + RCLCPP_ERROR(this->get_logger(), "Failed to bring up master. Configure Transition failed."); + return false; + } + RCLCPP_DEBUG(this->get_logger(), "Master (node_id=%hu) has state inactive.", master_id_); + if (!this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, 3s)) + { + RCLCPP_ERROR(this->get_logger(), "Failed to bring up master. Activate Transition failed."); + return false; + } + RCLCPP_DEBUG(this->get_logger(), "Master (node_id=%hu) has state active.", master_id_); + return true; +} + +bool LifecycleManager::bring_down_master() +{ + this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, 3s); + this->change_state(master_id_, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, 3s); + + auto state = this->get_state(master_id_, 3s); + + if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + return false; + } + + return true; +} + +bool LifecycleManager::bring_up_driver(std::string device_name) +{ + auto node_id = this->device_names_to_ids[device_name]; + RCLCPP_DEBUG(this->get_logger(), "Bringing up %s with id %u", device_name.c_str(), node_id); + auto master_state = this->get_state(master_id_, 3s); + if (master_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR( + this->get_logger(), "Failed to bring up %s. Master not in active state.", + device_name.c_str()); + return false; + } + auto state = this->get_state(node_id, 3s); + if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + this->get_logger(), "Failed to bring up %s. Not in unconfigured state.", device_name.c_str()); + return false; + } + RCLCPP_DEBUG( + this->get_logger(), "%s (node_id=%hu) has state unconfigured. Attempting to configure.", + device_name.c_str(), node_id); + if (!this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, 3s)) + { + RCLCPP_ERROR( + this->get_logger(), "Failed to bring up %s. Configure Transition failed.", + device_name.c_str()); + return false; + } + RCLCPP_DEBUG( + this->get_logger(), "%s (node_id=%hu) has state inactive. Attempting to activate.", + device_name.c_str(), node_id); + if (!this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, 3s)) + { + RCLCPP_ERROR( + this->get_logger(), "Failed to bring up %s. Activate Transition failed.", + device_name.c_str()); + return false; + } + RCLCPP_DEBUG( + this->get_logger(), "%s (node_id=%hu) has state active.", device_name.c_str(), node_id); + return true; +} + +bool LifecycleManager::bring_down_driver(std::string device_name) +{ + auto node_id = this->device_names_to_ids[device_name]; + + this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, 3s); + this->change_state(node_id, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, 3s); + auto state = this->get_state(node_id, 3s); + if (state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + return false; + } + return true; +} + +bool LifecycleManager::bring_up_all() +{ + if (!this->bring_up_master()) + { + return false; + } + for (auto it = this->device_names_to_ids.begin(); it != this->device_names_to_ids.end(); ++it) + { + if (it->first.find("master") == std::string::npos) + { + if (!this->bring_up_driver(it->first)) + { + return false; + } + } + else + { + RCLCPP_DEBUG(this->get_logger(), "Skipped master."); + } + } + return true; +} + +bool LifecycleManager::bring_down_all() +{ + RCLCPP_INFO(this->get_logger(), "Bring Down all"); + for (auto it = this->device_names_to_ids.begin(); it != this->device_names_to_ids.end(); ++it) + { + if (it->first.compare("master") != 0) + { + if (!this->bring_down_driver(it->first)) + { + return false; + } + } + } + if (!this->bring_down_master()) + { + return false; + } + + return true; +} + +} // namespace ros2_canopen + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleManager) diff --git a/canopen_core/src/lifecycle_master_node.cpp b/canopen_core/src/lifecycle_master_node.cpp deleted file mode 100644 index ce5bb029..00000000 --- a/canopen_core/src/lifecycle_master_node.cpp +++ /dev/null @@ -1,181 +0,0 @@ -#include "canopen_core/lifecycle_master_node.hpp" - -namespace ros2_canopen -{ - void LifecycleMasterNode::init() - { - this->activated.store(false); - //declare parameters - this->declare_parameter("master_config", ""); - this->declare_parameter("master_bin", ""); - this->declare_parameter("can_interface_name", ""); - this->declare_parameter("node_id", 1); - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleMasterNode::on_configure(const rclcpp_lifecycle::State &) - { - this->activated.store(false); - // Fetch parameters - this->get_parameter("master_config", dcf_txt_); - this->get_parameter("master_bin", dcf_bin_); - this->get_parameter("can_interface_name", can_interface_name_); - this->get_parameter("node_id", node_id_); - - //declare services - sdo_read_service = this->create_service( - std::string(this->get_name()).append("/sdo_read").c_str(), - std::bind( - &ros2_canopen::LifecycleMasterNode::on_sdo_read, - this, - std::placeholders::_1, - std::placeholders::_2)); - - sdo_write_service = this->create_service( - std::string(this->get_name()).append("/sdo_write").c_str(), - std::bind( - &ros2_canopen::LifecycleMasterNode::on_sdo_write, - this, - std::placeholders::_1, - std::placeholders::_2)); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleMasterNode::on_activate(const rclcpp_lifecycle::State &state) - { - this->activated.store(true); - io_guard_ = std::make_unique(); - ctx_ = std::make_unique(); - poll_ = std::make_unique(*ctx_); - loop_ = std::make_unique(poll_->get_poll()); - - exec_ = std::make_shared(loop_->get_executor()); - timer_ = std::make_unique(*poll_, *exec_, CLOCK_MONOTONIC); - ctrl_ = std::make_unique(can_interface_name_.c_str()); - chan_ = std::make_unique(*poll_, *exec_); - chan_->open(*ctrl_); - - sigset_ = std::make_unique(*poll_, *exec_); - // Watch for Ctrl+C or process termination. - sigset_->insert(SIGHUP); - sigset_->insert(SIGINT); - sigset_->insert(SIGTERM); - - sigset_->submit_wait( - [&](int /*signo*/) - { - // If the signal is raised again, terminate immediately. - sigset_->clear(); - - // Perform a clean shutdown. - ctx_->shutdown(); - }); - - master_ = std::make_shared( - *exec_, *timer_, *chan_, - dcf_txt_, dcf_bin_, node_id_); - master_->Reset(); - - spinner_ = std::thread( - [this]() - { - loop_->run(); - }); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleMasterNode::on_deactivate(const rclcpp_lifecycle::State &state) - { - this->activated.store(false); - exec_->post( - [&](){ - ctx_->shutdown(); - } - ); - RCLCPP_INFO(this->get_logger(), "Waiting for CANopen loop to shutdown."); - spinner_.join(); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleMasterNode::on_cleanup(const rclcpp_lifecycle::State &state) - { - this->activated.store(false); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleMasterNode::on_shutdown(const rclcpp_lifecycle::State &state) - { - this->activated.store(false); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - void LifecycleMasterNode::init_driver(std::shared_ptr node_instance, uint8_t node_id) - { - node_instance->init_from_master(exec_, master_, config_); - } - - void LifecycleMasterNode::on_sdo_read( - const std::shared_ptr request, - std::shared_ptr response) - { - if (this->activated.load()) - { - ros2_canopen::CODataTypes datatype = static_cast(request->type); - ros2_canopen::COData data = {request->index, request->subindex, 0U, datatype}; - std::future f = this->master_->async_read_sdo(request->nodeid, data); - f.wait(); - try - { - response->data = f.get().data_; - response->success = true; - } - catch (std::exception &e) - { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "LifecycleMasterNode is not in active state. SDO read service is not available."); - response->success = false; - } - } - - void LifecycleMasterNode::on_sdo_write( - const std::shared_ptr request, - std::shared_ptr response) - { - if (this->activated.load()) - { - ros2_canopen::CODataTypes datatype = static_cast(request->type); - ros2_canopen::COData data = {request->index, request->subindex, request->data, datatype}; - std::future f = this->master_->async_write_sdo(request->nodeid, data); - f.wait(); - try - { - response->success = f.get(); - } - catch (std::exception &e) - { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "LifecycleMasterNode is not in active state. SDO write service is not available."); - response->success = false; - } - } -} - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleMasterNode) \ No newline at end of file diff --git a/canopen_core/src/master_error.cpp b/canopen_core/src/master_error.cpp new file mode 100644 index 00000000..41c72bde --- /dev/null +++ b/canopen_core/src/master_error.cpp @@ -0,0 +1,12 @@ +#include "canopen_core/master_error.hpp" +#include +namespace ros2_canopen +{ + +char * MasterException::what() +{ + char * res = new char[1000]; + strcpy(res, what_.c_str()); + return res; +} +} // namespace ros2_canopen diff --git a/canopen_core/src/master_node.cpp b/canopen_core/src/master_node.cpp index 8c6b78b0..0954380f 100644 --- a/canopen_core/src/master_node.cpp +++ b/canopen_core/src/master_node.cpp @@ -1,128 +1,70 @@ #include "canopen_core/master_node.hpp" -namespace ros2_canopen -{ - void MasterNode::init(std::string dcf_txt, std::string dcf_bin, std::string can_interface_name, uint8_t nodeid, - std::shared_ptr config) - { - MasterInterface::init(dcf_txt, dcf_bin, can_interface_name, nodeid, config); - - io_guard_ = std::make_unique(); - ctx_ = std::make_unique(); - poll_ = std::make_unique(*ctx_); - loop_ = std::make_unique(poll_->get_poll()); +using namespace ros2_canopen; - exec_ = std::make_shared(loop_->get_executor()); - timer_ = std::make_unique(*poll_, *exec_, CLOCK_MONOTONIC); - ctrl_ = std::make_unique(can_interface_name.c_str()); - chan_ = std::make_unique(*poll_, *exec_); - chan_->open(*ctrl_); +void CanopenMaster::init() +{ + node_canopen_master_->init(); + node_canopen_master_->configure(); + node_canopen_master_->activate(); +} +void CanopenMaster::shutdown() { node_canopen_master_->shutdown(); } - sigset_ = std::make_unique(*poll_, *exec_); - // Watch for Ctrl+C or process termination. - sigset_->insert(SIGHUP); - sigset_->insert(SIGINT); - sigset_->insert(SIGTERM); +std::shared_ptr CanopenMaster::get_master() +{ + return node_canopen_master_->get_master(); +} - sigset_->submit_wait( - [&](int /*signo*/) - { - // If the signal is raised again, terminate immediately. - sigset_->clear(); +std::shared_ptr CanopenMaster::get_executor() +{ + return node_canopen_master_->get_executor(); +} - // Perform a clean shutdown. - ctx_->shutdown(); - }); +void LifecycleCanopenMaster::init() { node_canopen_master_->init(); } - master_ = std::make_shared( - *exec_, *timer_, *chan_, - dcf_txt, dcf_bin, nodeid); - master_->Reset(); +void LifecycleCanopenMaster::shutdown() { node_canopen_master_->shutdown(); } - spinner_ = std::thread( - [this]() - { - loop_->run(); - }); +std::shared_ptr LifecycleCanopenMaster::get_master() +{ + return node_canopen_master_->get_master(); +} - /// @todo add services - sdo_read_service = this->create_service( - std::string(this->get_name()).append("/sdo_read").c_str(), - std::bind( - &ros2_canopen::MasterNode::on_sdo_read, - this, - std::placeholders::_1, - std::placeholders::_2)); +std::shared_ptr LifecycleCanopenMaster::get_executor() +{ + return node_canopen_master_->get_executor(); +} - sdo_write_service = this->create_service( - std::string(this->get_name()).append("/sdo_write").c_str(), - std::bind( - &ros2_canopen::MasterNode::on_sdo_write, - this, - std::placeholders::_1, - std::placeholders::_2)); - } - void MasterNode::add_driver(std::shared_ptr node_instance, uint8_t node_id) - { - std::shared_ptr> prom = std::make_shared>(); - auto f = prom->get_future(); - exec_->post([this, node_id, node_instance, prom]() - { - node_instance->init(*this->exec_, *(this->master_), node_id, config_); - prom->set_value(); }); - f.wait(); - } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenMaster::on_configure(const rclcpp_lifecycle::State & state) +{ + node_canopen_master_->configure(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} - void MasterNode::remove_driver(std::shared_ptr node_instance, uint8_t node_id) - { - std::shared_ptr> prom = std::make_shared>(); - auto f = prom->get_future(); - exec_->post([this, node_id, node_instance, prom]() - { - node_instance->remove(*this->exec_, *(this->master_), node_id); - prom->set_value(); }); - f.wait(); - } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenMaster::on_activate(const rclcpp_lifecycle::State & state) +{ + node_canopen_master_->activate(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} - void MasterNode::on_sdo_read( - const std::shared_ptr request, - std::shared_ptr response) - { - ros2_canopen::CODataTypes datatype = static_cast(request->type); - ros2_canopen::COData data = {request->index, request->subindex, 0U, datatype}; - std::future f = this->master_->async_read_sdo(request->nodeid, data); - f.wait(); - try - { - response->data = f.get().data_; - response->success = true; - } - catch (std::exception &e) - { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } - } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenMaster::on_deactivate(const rclcpp_lifecycle::State & state) +{ + node_canopen_master_->deactivate(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} - void MasterNode::on_sdo_write( - const std::shared_ptr request, - std::shared_ptr response) - { - ros2_canopen::CODataTypes datatype = static_cast(request->type); - ros2_canopen::COData data = {request->index, request->subindex, request->data, datatype}; - std::future f = this->master_->async_write_sdo(request->nodeid, data); - f.wait(); - try - { - response->success = f.get(); - } - catch (std::exception &e) - { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } - } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenMaster::on_cleanup(const rclcpp_lifecycle::State & state) +{ + node_canopen_master_->cleanup(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::MasterNode) \ No newline at end of file +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleCanopenMaster::on_shutdown(const rclcpp_lifecycle::State & state) +{ + node_canopen_master_->shutdown(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} diff --git a/canopen_core/src/node_interfaces/node_canopen_driver.cpp b/canopen_core/src/node_interfaces/node_canopen_driver.cpp new file mode 100644 index 00000000..f0c2c205 --- /dev/null +++ b/canopen_core/src/node_interfaces/node_canopen_driver.cpp @@ -0,0 +1,91 @@ +#include "canopen_core/node_interfaces/node_canopen_driver.hpp" +using namespace std::chrono_literals; + +template <> +void ros2_canopen::node_interfaces::NodeCanopenDriver::demand_set_master() +{ + RCLCPP_DEBUG(node_->get_logger(), "demand_set_master_start"); + if (!configured_.load()) + { + throw ros2_canopen::DriverException("Set Master: driver is not configured"); + } + std::string init_service_name = container_name_ + "/init_driver"; + RCLCPP_DEBUG(node_->get_logger(), "Service: %s", init_service_name.c_str()); + rclcpp::Client::SharedPtr demand_set_master_client; + // demand_set_master_client; + demand_set_master_client = node_->create_client( + init_service_name, rmw_qos_profile_services_default, client_cbg_); + + while (!demand_set_master_client->wait_for_service(non_transmit_timeout_)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR( + node_->get_logger(), "Interrupted while waiting for init_driver service. Exiting."); + } + RCLCPP_INFO(node_->get_logger(), "init_driver service not available, waiting again..."); + } + auto request = std::make_shared(); + request->nodeid = node_id_; + + auto future_result = demand_set_master_client->async_send_request(request); + + auto future_status = future_result.wait_for(non_transmit_timeout_); + RCLCPP_DEBUG(node_->get_logger(), "demand_set_master end"); + + if (future_status == std::future_status::ready) + { + future_result.get(); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Could not get result."); + throw DriverException("Could not get result."); + } +} + +template <> +void ros2_canopen::node_interfaces::NodeCanopenDriver< + rclcpp_lifecycle::LifecycleNode>::demand_set_master() +{ + RCLCPP_DEBUG(node_->get_logger(), "demand_set_master_start"); + if (!configured_.load()) + { + throw ros2_canopen::DriverException("Set Master: driver is not configured"); + } + std::string init_service_name = container_name_ + "/init_driver"; + rclcpp::Client::SharedPtr demand_set_master_client; + // demand_set_master_client; + demand_set_master_client = node_->create_client( + init_service_name, rmw_qos_profile_services_default, client_cbg_); + + while (!demand_set_master_client->wait_for_service(non_transmit_timeout_)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR( + node_->get_logger(), "Interrupted while waiting for init_driver service. Exiting."); + } + RCLCPP_INFO(node_->get_logger(), "init_driver service not available, waiting again..."); + } + auto request = std::make_shared(); + request->nodeid = node_id_; + + auto future_result = demand_set_master_client->async_send_request(request); + + auto future_status = future_result.wait_for(non_transmit_timeout_); + RCLCPP_DEBUG(node_->get_logger(), "demand_set_master end"); + + if (future_status == std::future_status::ready) + { + future_result.get(); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Could not get result."); + throw DriverException("Could not get result."); + } +} + +template class ros2_canopen::node_interfaces::NodeCanopenDriver; +template class ros2_canopen::node_interfaces::NodeCanopenDriver; diff --git a/canopen_core/src/node_interfaces/node_canopen_master.cpp b/canopen_core/src/node_interfaces/node_canopen_master.cpp new file mode 100644 index 00000000..1dc08cf9 --- /dev/null +++ b/canopen_core/src/node_interfaces/node_canopen_master.cpp @@ -0,0 +1,4 @@ +#include "canopen_core/node_interfaces/node_canopen_master.hpp" + +template class ros2_canopen::node_interfaces::NodeCanopenMaster; +template class ros2_canopen::node_interfaces::NodeCanopenMaster; diff --git a/canopen_core/test/CMakeLists.txt b/canopen_core/test/CMakeLists.txt new file mode 100644 index 00000000..65b35a4b --- /dev/null +++ b/canopen_core/test/CMakeLists.txt @@ -0,0 +1,132 @@ +ament_add_gmock(test_node_canopen_driver + test_node_canopen_driver.cpp +) +ament_target_dependencies(test_node_canopen_driver + rclcpp + rclcpp_lifecycle + lely_core_libraries + yaml_cpp_vendor + canopen_interfaces +) +target_include_directories(test_node_canopen_driver PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_node_canopen_driver + node_canopen_driver +) + +ament_add_gmock(test_node_canopen_master + test_node_canopen_master.cpp +) +ament_target_dependencies(test_node_canopen_master + rclcpp + rclcpp_lifecycle + lely_core_libraries + yaml_cpp_vendor + canopen_interfaces +) +target_include_directories(test_node_canopen_master PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_node_canopen_master + node_canopen_master +) + +# Temporary fixing ci build stability +if(DEVICE_CONTAINER_TESTING) +ament_add_gmock(test_device_container + test_device_container.cpp +) +ament_target_dependencies(test_device_container + ${dependencies} +) +target_include_directories(test_device_container PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_device_container + node_canopen_master + node_canopen_driver + device_container +) +endif() +FILE(COPY bus_configs DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) + +ament_add_gmock(test_canopen_driver +test_canopen_driver.cpp +) +ament_target_dependencies(test_canopen_driver + ${dependencies} +) +target_include_directories(test_canopen_driver PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_canopen_driver + node_canopen_driver +) + +ament_add_gmock(test_lifecycle_canopen_driver +test_lifecycle_canopen_driver.cpp +) +ament_target_dependencies(test_lifecycle_canopen_driver + ${dependencies} +) +target_include_directories(test_lifecycle_canopen_driver PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_lifecycle_canopen_driver + node_canopen_driver +) + +ament_add_gmock(test_canopen_master +test_canopen_master.cpp +) +ament_target_dependencies(test_canopen_master + ${dependencies} +) +target_include_directories(test_canopen_master PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_canopen_master + node_canopen_master +) + +ament_add_gmock(test_lifecycle_canopen_master +test_lifecycle_canopen_master.cpp +) +ament_target_dependencies(test_lifecycle_canopen_master + ${dependencies} +) +target_include_directories(test_lifecycle_canopen_master PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_lifecycle_canopen_master + node_canopen_master +) + +ament_add_gmock(test_lifecycle_manager +test_lifecycle_manager.cpp +) +ament_target_dependencies(test_lifecycle_manager + ${dependencies} +) +target_include_directories(test_lifecycle_manager PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_lifecycle_manager + device_container +) + +ament_add_gmock(test_errors +test_errors.cpp +) +ament_target_dependencies(test_errors + ${dependencies} +) +target_include_directories(test_errors PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_errors + device_container + node_canopen_master + node_canopen_driver +) diff --git a/canopen_core/test/bus_configs/bad_driver_duplicate.yml b/canopen_core/test/bus_configs/bad_driver_duplicate.yml new file mode 100644 index 00000000..70fb19e2 --- /dev/null +++ b/canopen_core/test/bus_configs/bad_driver_duplicate.yml @@ -0,0 +1,14 @@ +--- +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" + +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/bad_driver_no_driver.yml b/canopen_core/test/bus_configs/bad_driver_no_driver.yml new file mode 100644 index 00000000..7403f8b7 --- /dev/null +++ b/canopen_core/test/bus_configs/bad_driver_no_driver.yml @@ -0,0 +1,6 @@ +--- +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/bad_driver_no_id.yml b/canopen_core/test/bus_configs/bad_driver_no_id.yml new file mode 100644 index 00000000..c0a28580 --- /dev/null +++ b/canopen_core/test/bus_configs/bad_driver_no_id.yml @@ -0,0 +1,6 @@ +--- +proxy_device_1: + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/bad_driver_no_package.yml b/canopen_core/test/bus_configs/bad_driver_no_package.yml new file mode 100644 index 00000000..b5c8056c --- /dev/null +++ b/canopen_core/test/bus_configs/bad_driver_no_package.yml @@ -0,0 +1,6 @@ +--- +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" diff --git a/canopen_core/test/bus_configs/bad_master_no_driver.yml b/canopen_core/test/bus_configs/bad_master_no_driver.yml new file mode 100644 index 00000000..854b668e --- /dev/null +++ b/canopen_core/test/bus_configs/bad_master_no_driver.yml @@ -0,0 +1,4 @@ +--- +master: + node_id: 1 + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/bad_master_no_id.yml b/canopen_core/test/bus_configs/bad_master_no_id.yml new file mode 100644 index 00000000..e7160c68 --- /dev/null +++ b/canopen_core/test/bus_configs/bad_master_no_id.yml @@ -0,0 +1,4 @@ +--- +master: + driver: "ros2_canopen::CanopenMaster" + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/bad_master_no_package.yml b/canopen_core/test/bus_configs/bad_master_no_package.yml new file mode 100644 index 00000000..b330d61c --- /dev/null +++ b/canopen_core/test/bus_configs/bad_master_no_package.yml @@ -0,0 +1,4 @@ +--- +master: + node_id: 1 + driver: "ros2_canopen::CanopenMaster" diff --git a/canopen_core/test/bus_configs/bad_no_master.yml b/canopen_core/test/bus_configs/bad_no_master.yml new file mode 100644 index 00000000..8ee2401e --- /dev/null +++ b/canopen_core/test/bus_configs/bad_no_master.yml @@ -0,0 +1,14 @@ +--- +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" + +proxy_device_2: + node_id: 3 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/good_driver.yml b/canopen_core/test/bus_configs/good_driver.yml new file mode 100644 index 00000000..834e613f --- /dev/null +++ b/canopen_core/test/bus_configs/good_driver.yml @@ -0,0 +1,7 @@ +--- +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/good_master.yml b/canopen_core/test/bus_configs/good_master.yml new file mode 100644 index 00000000..94446025 --- /dev/null +++ b/canopen_core/test/bus_configs/good_master.yml @@ -0,0 +1,5 @@ +--- +master: + node_id: 1 + driver: "ros2_canopen::CanopenMaster" + package: "canopen_core" diff --git a/canopen_core/test/bus_configs/good_master_and_two_driver.yml b/canopen_core/test/bus_configs/good_master_and_two_driver.yml new file mode 100644 index 00000000..374b8dca --- /dev/null +++ b/canopen_core/test/bus_configs/good_master_and_two_driver.yml @@ -0,0 +1,19 @@ +--- +master: + node_id: 1 + driver: "ros2_canopen::CanopenMaster" + package: "canopen_core" + +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" + +proxy_device_2: + node_id: 3 + dcf: "simple.eds" + dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" + driver: "ros2_canopen::CanopenDriver" + package: "canopen_core" diff --git a/canopen_core/test/master.dcf b/canopen_core/test/master.dcf new file mode 100644 index 00000000..4f6f975e --- /dev/null +++ b/canopen_core/test/master.dcf @@ -0,0 +1,694 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=2 +NrOfTxPDO=2 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=32 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1401 +15=0x1600 +16=0x1601 +17=0x1800 +18=0x1801 +19=0x1A00 +20=0x1A01 +21=0x1F25 +22=0x1F55 +23=0x1F80 +24=0x1F81 +25=0x1F82 +26=0x1F84 +27=0x1F85 +28=0x1F86 +29=0x1F87 +30=0x1F88 +31=0x1F89 +32=0x1F8A + +[ManufacturerObjects] +SupportedObjects=12 +1=0x2000 +2=0x2001 +3=0x2200 +4=0x2201 +5=0x5800 +6=0x5801 +7=0x5A00 +8=0x5A01 +9=0x5C00 +10=0x5C01 +11=0x5E00 +12=0x5E01 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=2 +2=0x00000082 +3=0x00000083 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1401] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1401sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1401sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000183 + +[1401sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1401sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1401sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1401sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1601] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1601Value] +NrOfEntries=1 +1=0x20010120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1801] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1801sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1801sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000203 + +[1801sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1801sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1801sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1A01] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A01Value] +NrOfEntries=1 +1=0x22010120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=2 +2=0x00000005 +3=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=2 +2=0x00000360 +3=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=proxy_device_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2001] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 2 +ObjectType=0x09 + +[2001sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2001sub1] +ParameterName=proxy_device_2: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=proxy_device_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2201] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 2 +ObjectType=0x09 + +[2201sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2201sub1] +ParameterName=proxy_device_2: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5801] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5A01] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A01Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5C01] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 + +[5E01] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E01Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_core/test/simple.eds b/canopen_core/test/simple.eds new file mode 100644 index 00000000..77126767 --- /dev/null +++ b/canopen_core/test/simple.eds @@ -0,0 +1,270 @@ +[DeviceInfo] +VendorName=Lely Industries N.V. +VendorNumber=0x00000360 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=1 +NrOfTxPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0011=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=11 +1=0x1003 +2=0x1005 +3=0x1014 +4=0x1015 +5=0x1016 +6=0x1017 +7=0x1029 +8=0x1400 +9=0x1600 +10=0x1800 +11=0x1A00 + +[ManufacturerObjects] +SupportedObjects=2 +1=0x4000 +2=0x4001 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw + +[1018] +SubNumber=5 +ParameterName=Identity object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000360 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=1 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x200 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x40000020 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x40010020 + +[4000] +ParameterName=UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[4001] +ParameterName=UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 diff --git a/canopen_core/test/test_canopen_driver.cpp b/canopen_core/test/test_canopen_driver.cpp new file mode 100644 index 00000000..ef47fb59 --- /dev/null +++ b/canopen_core/test/test_canopen_driver.cpp @@ -0,0 +1,91 @@ +#include +#include "canopen_core/driver_node.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +using namespace std::chrono_literals; +using namespace ros2_canopen; +using namespace testing; + +class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenDriverInterface +{ +public: + MOCK_METHOD( + void, set_master, + (std::shared_ptr exec, std::shared_ptr master), + (override)); + MOCK_METHOD(void, demand_set_master, (), (override)); + MOCK_METHOD(void, init, (), (override)); + MOCK_METHOD(void, configure, (), (override)); + MOCK_METHOD(void, activate, (), (override)); + MOCK_METHOD(void, deactivate, (), (override)); + MOCK_METHOD(void, cleanup, (), (override)); + MOCK_METHOD(void, shutdown, (), (override)); + MOCK_METHOD(void, add_to_master, (), (override)); + MOCK_METHOD(void, remove_from_master, (), (override)); +}; + +class MockCanopenMaster : public CanopenDriver +{ + friend class CanopenDriverTest; + FRIEND_TEST(CanopenDriverTest, test_init); +}; + +class CanopenDriverTest : public testing::Test +{ +public: + std::shared_ptr canopen_driver; + std::shared_ptr node_canopen_driver; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "SetUp"); + rclcpp::init(0, nullptr); + canopen_driver = std::make_shared(); + node_canopen_driver = std::make_shared(); + canopen_driver->node_canopen_driver_ = + std::static_pointer_cast( + node_canopen_driver); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "TearDown"); + rclcpp::shutdown(); + } +}; + +TEST_F(CanopenDriverTest, test_init) +{ + EXPECT_CALL(*node_canopen_driver, init()).Times(1); + EXPECT_CALL(*node_canopen_driver, configure()).Times(1); + EXPECT_CALL(*node_canopen_driver, demand_set_master()).Times(1); + EXPECT_CALL(*node_canopen_driver, activate()).Times(1); + canopen_driver->init(); +} + +TEST_F(CanopenDriverTest, test_set_master) +{ + std::shared_ptr exec; + std::shared_ptr master; + + EXPECT_CALL(*node_canopen_driver, set_master(_, _)).Times(1); + EXPECT_NO_THROW(canopen_driver->set_master(exec, master)); +} + +TEST_F(CanopenDriverTest, test_get_node_base_interface) +{ + auto base_iface = canopen_driver->get_node_base_interface(); + EXPECT_TRUE(base_iface); +} + +TEST_F(CanopenDriverTest, test_shutdown) +{ + EXPECT_CALL(*node_canopen_driver, shutdown()); + EXPECT_NO_THROW(canopen_driver->shutdown()); +} + +TEST_F(CanopenDriverTest, test_is_lifecycle) { EXPECT_FALSE(canopen_driver->is_lifecycle()); } + +TEST_F(CanopenDriverTest, test_get_node_canopen_driver_interface) +{ + EXPECT_TRUE(canopen_driver->get_node_canopen_driver_interface() == node_canopen_driver); +} diff --git a/canopen_core/test/test_canopen_master.cpp b/canopen_core/test/test_canopen_master.cpp new file mode 100644 index 00000000..2eb16c84 --- /dev/null +++ b/canopen_core/test/test_canopen_master.cpp @@ -0,0 +1,83 @@ +#include +#include "canopen_core/master_node.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +using namespace std::chrono_literals; +using namespace ros2_canopen; +using namespace testing; + +class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenMasterInterface +{ +public: + MOCK_METHOD(void, init, (), (override)); + MOCK_METHOD(void, configure, (), (override)); + MOCK_METHOD(void, activate, (), (override)); + MOCK_METHOD(void, deactivate, (), (override)); + MOCK_METHOD(void, cleanup, (), (override)); + MOCK_METHOD(void, shutdown, (), (override)); + MOCK_METHOD(std::shared_ptr, get_master, (), (override)); + MOCK_METHOD(std::shared_ptr, get_executor, (), (override)); +}; + +class MockCanopenMaster : public CanopenMaster +{ + friend class CanopenMasterTest; + FRIEND_TEST(CanopenMasterTest, test_init); +}; + +class CanopenMasterTest : public testing::Test +{ +public: + std::shared_ptr canopen_master; + std::shared_ptr node_canopen_master; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); + rclcpp::init(0, nullptr); + canopen_master = std::make_shared(); + node_canopen_master = std::make_shared(); + canopen_master->node_canopen_master_ = + std::static_pointer_cast( + node_canopen_master); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); + rclcpp::shutdown(); + } +}; + +TEST_F(CanopenMasterTest, test_init) +{ + EXPECT_CALL(*node_canopen_master, init()).Times(1); + EXPECT_CALL(*node_canopen_master, configure()).Times(1); + EXPECT_CALL(*node_canopen_master, activate()).Times(1); + canopen_master->init(); +} + +TEST_F(CanopenMasterTest, test_get_node_base_interface) +{ + auto base_iface = canopen_master->get_node_base_interface(); + EXPECT_TRUE(base_iface); +} + +TEST_F(CanopenMasterTest, test_shutdown) +{ + EXPECT_CALL(*node_canopen_master, shutdown()); + EXPECT_NO_THROW(canopen_master->shutdown()); +} + +TEST_F(CanopenMasterTest, test_is_lifecycle) { EXPECT_FALSE(canopen_master->is_lifecycle()); } + +TEST_F(CanopenMasterTest, test_get_master) +{ + EXPECT_CALL(*node_canopen_master, get_master()); + EXPECT_NO_THROW(canopen_master->get_master()); +} + +TEST_F(CanopenMasterTest, test_get_executor) +{ + EXPECT_CALL(*node_canopen_master, get_executor()); + EXPECT_NO_THROW(canopen_master->get_executor()); +} diff --git a/canopen_core/test/test_device_container.cpp b/canopen_core/test/test_device_container.cpp new file mode 100644 index 00000000..6cbe0afe --- /dev/null +++ b/canopen_core/test/test_device_container.cpp @@ -0,0 +1,848 @@ +#include +#include "canopen_core/device_container.hpp" +#include "canopen_core/device_container_error.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +using namespace std::chrono_literals; +using namespace ros2_canopen; +using namespace testing; + +class MockNodeFactory : public rclcpp_components::NodeFactory +{ +public: + MOCK_METHOD( + rclcpp_components::NodeInstanceWrapper, create_node_instance, + (const rclcpp::NodeOptions & options), (override)); +}; + +class FakeDeviceContainer +{ +public: + virtual std::vector + get_component_resources( + const std::string & package_name, + const std::string & resource_index = "rclcpp_components") const + { + // RCLCPP_INFO(this->get_logger(), "get_component_resources"); + std::vector components; + components.push_back(rclcpp_components::ComponentManager::ComponentResource( + "ros2_canopen::CanopenMaster", "canopen_core")); + components.push_back(rclcpp_components::ComponentManager::ComponentResource( + "ros2_canopen::LifecycleCanopenMaster", "canopen_core")); + components.push_back(rclcpp_components::ComponentManager::ComponentResource( + "ros2_canopen::CanopenDriver", "canopen_core")); + components.push_back(rclcpp_components::ComponentManager::ComponentResource( + "ros2_canopen::LifecycleCanopenDriver", "canopen_core")); + // get_component_resources_mock(true); + return components; + } +}; +using namespace std::placeholders; +class MockDeviceContainer : public DeviceContainer +{ + FakeDeviceContainer fake_device_container; + +public: + FRIEND_TEST(DeviceContainerTest, test_load_component_master); + FRIEND_TEST(DeviceContainerTest, test_load_component_driver_non_lifecycle); + FRIEND_TEST(DeviceContainerTest, test_load_component_driver_lifecycle); + FRIEND_TEST(DeviceContainerTest, test_get_registered_drivers); + FRIEND_TEST(DeviceContainerTest, test_shutdown); + FRIEND_TEST(DeviceContainerTest, test_get_ids_of_drivers_with_type); + FRIEND_TEST(DeviceContainerTest, test_get_driver_type); + FRIEND_TEST(DeviceContainerTest, test_on_list_nodes); + FRIEND_TEST(DeviceContainerTest, test_load_master_good); + FRIEND_TEST(DeviceContainerTest, test_load_master_load_component_fail); + FRIEND_TEST(DeviceContainerTest, test_load_master_bad_no_id); + FRIEND_TEST(DeviceContainerTest, test_load_master_bad_no_driver); + FRIEND_TEST(DeviceContainerTest, test_load_master_bad_no_package); + FRIEND_TEST(DeviceContainerTest, test_load_driver_good); + FRIEND_TEST(DeviceContainerTest, test_load_driver_load_component_fail); + FRIEND_TEST(DeviceContainerTest, test_load_driver_bad_no_id); + FRIEND_TEST(DeviceContainerTest, test_load_driver_bad_no_package); + FRIEND_TEST(DeviceContainerTest, test_load_driver_bad_no_driver); + FRIEND_TEST(DeviceContainerTest, test_load_manager_no_lifecycle); + FRIEND_TEST(DeviceContainerTest, test_load_manager_lifecycle); + FRIEND_TEST(DeviceContainerTest, test_configure_good); + + MockDeviceContainer( + std::weak_ptr executor = + std::weak_ptr()) + : DeviceContainer(executor), fake_device_container() + { + } + MOCK_METHOD( + bool, load_component, + (std::string & package_name, std::string & driver_name, uint16_t node_id, + std::string & node_name, std::vector & params), + (override)); + MOCK_METHOD(void, configure, (), (override)); + MOCK_METHOD(bool, load_master, (), (override)); + MOCK_METHOD(bool, load_drivers, (), (override)); + MOCK_METHOD(bool, load_manager, (), (override)); + MOCK_METHOD( + std::shared_ptr, create_component_factory, + (const ComponentResource & resource), (override)); + MOCK_METHOD( + std::vector, get_component_resources, + (const std::string & package_name, const std::string & resource_index), (const, override)); + MOCK_METHOD( + void, add_node_to_executor, + (rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface), (override)); + + void DelegateToFake() + { + ON_CALL(*this, load_component) + .WillByDefault( + [this]( + std::string & package_name, std::string & driver_name, uint16_t node_id, + std::string & node_name, std::vector & params) -> bool { + return DeviceContainer::load_component( + package_name, driver_name, node_id, node_name, params); + }); + ON_CALL(*this, load_master) + .WillByDefault([this]() -> bool { return DeviceContainer::load_master(); }); + ON_CALL(*this, load_drivers) + .WillByDefault([this]() -> bool { return DeviceContainer::load_drivers(); }); + ON_CALL(*this, load_manager) + .WillByDefault([this]() -> bool { return DeviceContainer::load_manager(); }); + ON_CALL(*this, configure).WillByDefault([this]() { return DeviceContainer::configure(); }); + ON_CALL(*this, get_component_resources) + .WillByDefault( + [this](const std::string & package_name, const std::string & resource_index) + -> std::vector + { return fake_device_container.get_component_resources(package_name, resource_index); }); + ON_CALL(*this, add_node_to_executor) + .WillByDefault([this](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_interface) + { DeviceContainer::add_node_to_executor(node_interface); }); + } +}; + +class MockCanopenDriver : public CanopenDriverInterface, public rclcpp::Node +{ +public: + explicit MockCanopenDriver( + std::string name = "mock_canopen_driver", + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : rclcpp::Node(name, node_options) + { + } + MOCK_METHOD(void, init, (), (override)); + MOCK_METHOD( + void, set_master, + (std::shared_ptr exec, std::shared_ptr master), + (override)); + // MOCK_METHOD(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr, get_node_base_interface, (), + // (override)); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() + { + return rclcpp::Node::get_node_base_interface(); + } + MOCK_METHOD(void, shutdown, (), (override)); + MOCK_METHOD(bool, is_lifecycle, (), (override)); + MOCK_METHOD( + std::shared_ptr, + get_node_canopen_driver_interface, (), (override)); +}; + +class MockCanopenMaster : public CanopenMasterInterface, public rclcpp::Node +{ +public: + explicit MockCanopenMaster( + std::string name = "mock_canopen_master", + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + : rclcpp::Node(name, node_options) + { + } + MOCK_METHOD(void, init, (), (override)); + MOCK_METHOD(void, shutdown, (), (override)); + MOCK_METHOD(std::shared_ptr, get_master, (), (override)); + MOCK_METHOD(std::shared_ptr, get_executor, (), (override)); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() + { + return rclcpp::Node::get_node_base_interface(); + } + MOCK_METHOD(bool, is_lifecycle, (), (override)); +}; + +class DeviceContainerTest : public testing::Test +{ +public: + std::shared_ptr device_container; + std::shared_ptr exec; + std::thread spinThread; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "SetUp"); + rclcpp::init(0, nullptr); + exec = std::make_shared(); + device_container = std::make_shared(exec); + device_container->DelegateToFake(); + + exec->add_node(device_container); + spinThread = std::thread([this]() { exec->spin(); }); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "Teardown"); + exec->cancel(); + spinThread.join(); + RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "Executor joined."); + exec->remove_node(device_container); + RCLCPP_INFO(rclcpp::get_logger("DeviceContainerTest"), "Node removed."); + rclcpp::shutdown(); + } +}; + +TEST_F(DeviceContainerTest, test_name_and_parameters_declared) +{ + EXPECT_TRUE(std::string("device_container").compare(device_container->get_name()) == 0); + EXPECT_TRUE(device_container->has_parameter("can_interface_name")); + EXPECT_TRUE(device_container->has_parameter("master_config")); + EXPECT_TRUE(device_container->has_parameter("bus_config")); + EXPECT_TRUE(device_container->has_parameter("master_bin")); +} + +TEST_F(DeviceContainerTest, test_services_declared) +{ + auto services = device_container->get_service_names_and_types_by_node( + device_container->get_name(), device_container->get_namespace()); + bool found_init_driver = false; + bool found_unload_node = false; + bool found_load_node = false; + for (auto it = services.begin(); it != services.end(); ++it) + { + if (it->first.compare("/device_container/init_driver") == 0) + { + found_init_driver = true; + } + if (it->first.compare("/device_container/_container/unload_node") == 0) + { + found_unload_node = true; + } + if (it->first.compare("/device_container/_container/load_node") == 0) + { + found_load_node = true; + } + } + EXPECT_TRUE(found_init_driver); + EXPECT_FALSE(found_unload_node); + EXPECT_FALSE(found_load_node); +} + +TEST_F(DeviceContainerTest, test_load_component_master) +{ + std::vector params; + std::string package("canopen_core"); + std::string driver_name("ros2_canopen::CanopenMaster"); + std::string node_name("master"); + uint16_t node_id = 1; + auto node = std::make_shared(); + auto node_factory = std::make_shared(); + auto node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(node), + std::bind(&MockCanopenMaster::get_node_base_interface, node)); + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + device_container->load_component(package, driver_name, node_id, node_name, params); + + // Check that can_master_ is assigned. + EXPECT_TRUE(((long)device_container->can_master_.get() == (long)node.get())); + // Check that master is not stored in driver map. + EXPECT_FALSE((long)device_container->registered_drivers_[node_id].get() == (long)node.get()); +} + +TEST_F(DeviceContainerTest, test_load_component_driver_non_lifecycle) +{ + // Lifecycle operation false + device_container->lifecycle_operation_ = false; + std::vector params; + std::string package("canopen_core"); + std::string driver_name("ros2_canopen::CanopenDriver"); + std::string node_name("driver"); + + uint16_t node_id = 1; + auto node = std::make_shared(); + auto node_factory = std::make_shared(); + auto node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(node), + std::bind(&MockCanopenDriver::get_node_base_interface, node)); + + // Lifecycle component leads to throw + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(true)); + EXPECT_THROW( + device_container->load_component(package, driver_name, node_id, node_name, params), + DeviceContainerException); + + // Non lifecycle component leads to normal behaviour + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, node_name, params); + + // Check that can_master_ is not assigned. + EXPECT_FALSE(((long)device_container->can_master_.get() == (long)node.get())); + // Check that driver is stored in driver map. + EXPECT_TRUE((long)device_container->registered_drivers_[node_id].get() == (long)node.get()); +} + +TEST_F(DeviceContainerTest, test_load_component_driver_lifecycle) +{ + // Lifecycle operation true + device_container->lifecycle_operation_ = true; + std::vector params; + std::string package("canopen_core"); + std::string driver_name("ros2_canopen::CanopenDriver"); + std::string node_name("driver"); + uint16_t node_id = 1; + auto node = std::make_shared(); + auto node_factory = std::make_shared(); + auto node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(node), + std::bind(&MockCanopenDriver::get_node_base_interface, node)); + + // Non lifecycle component leads to throw + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); + EXPECT_THROW( + device_container->load_component(package, driver_name, node_id, node_name, params), + DeviceContainerException); + + // Lifecycle component leads to normal behaviour + node_id = 2; + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(true)); + device_container->load_component(package, driver_name, node_id, node_name, params); + + // Check that can_master_ is not assigned. + EXPECT_FALSE(((long)device_container->can_master_.get() == (long)node.get())); + // Check that driver is stored in driver map. + EXPECT_TRUE((long)device_container->registered_drivers_[node_id].get() == (long)node.get()); +} + +TEST_F(DeviceContainerTest, test_get_registered_drivers) +{ + // Lifecycle operation true + device_container->lifecycle_operation_ = false; + std::vector params; + std::string package("canopen_core"); + std::string driver_name("ros2_canopen::CanopenDriver"); + std::string node_name("driver"); + uint16_t node_id = 1; + auto node = std::make_shared(); + auto node_factory = std::make_shared(); + auto node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(node), + std::bind(&MockCanopenDriver::get_node_base_interface, node)); + + // Should be zero at start + EXPECT_EQ(device_container->get_registered_drivers().size(), 0U); + + // Load component and expect that size of registered drivers changes to 1 + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, node_name, params); + EXPECT_EQ(device_container->get_registered_drivers().size(), 1U); + + // Load component and expect that size of registered drivers changes to 2 + node_id = 2; + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, node_name, params); + EXPECT_EQ(device_container->get_registered_drivers().size(), 2U); +} + +TEST_F(DeviceContainerTest, test_count_drivers) +{ + std::vector params; + std::string package("canopen_core"); + std::string driver_name("ros2_canopen::CanopenDriver"); + std::string node_name("driver1"); + uint16_t node_id = 1; + auto node = std::make_shared(); + auto node_factory = std::make_shared(); + auto node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(node), + std::bind(&MockCanopenDriver::get_node_base_interface, node)); + + // Should be zero at start + EXPECT_EQ(device_container->count_drivers(), 0U); + + // Load component and expect that size of registered drivers changes to 1 + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, node_name, params); + EXPECT_EQ(device_container->count_drivers(), 1U); + + // Load component and expect that size of registered drivers changes to 2 + node_id = 2; + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)).Times(1).WillOnce(Return(node_instance)); + EXPECT_CALL(*node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, node_name, params); + EXPECT_EQ(device_container->count_drivers(), 2U); +} + +TEST_F(DeviceContainerTest, test_shutdown) +{ + // Lifecycle operation true + std::vector params; + std::string package("canopen_core"); + std::string driver_name("ros2_canopen::CanopenMaster"); + std::string node_name("master"); + uint16_t node_id = 1; + auto master_node = std::make_shared(); + auto driver_node = std::make_shared(); + auto node_factory = std::make_shared(); + auto master_node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(master_node), + std::bind(&MockCanopenMaster::get_node_base_interface, master_node)); + auto driver_node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(driver_node), + std::bind(&MockCanopenDriver::get_node_base_interface, driver_node)); + + // Load component and expect that size of registered drivers changes to 1 + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)) + .Times(1) + .WillOnce(Return(master_node_instance)); + device_container->load_component(package, driver_name, node_id, node_name, params); + + driver_name = "ros2_canopen::CanopenDriver"; + node_name = "driver2"; + node_id = 2; + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)) + .Times(1) + .WillOnce(Return(driver_node_instance)); + EXPECT_CALL(*driver_node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, node_name, params); + + node_id = 3; + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)) + .Times(1) + .WillOnce(Return(driver_node_instance)); + EXPECT_CALL(*driver_node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, node_name, params); + EXPECT_EQ(device_container->get_registered_drivers().size(), 2U); + + EXPECT_CALL(*driver_node, shutdown()).Times(2); + EXPECT_CALL(*master_node, shutdown()).Times(1); + device_container->shutdown(); +} + +TEST_F(DeviceContainerTest, test_get_ids_of_drivers_with_type) +{ + std::string file_name("bus_configs/good_master_and_two_driver.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + std::string driver_name("ros2_canopen::CanopenDriver"); + auto ids = device_container->get_ids_of_drivers_with_type(driver_name); + EXPECT_EQ(ids.size(), 2U); +} + +TEST_F(DeviceContainerTest, test_get_driver_type) +{ + std::string file_name("bus_configs/good_master_and_two_driver.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + uint16_t node_id = 2; + auto driver_type = device_container->get_driver_type(node_id); + EXPECT_EQ(driver_type.compare("ros2_canopen::CanopenDriver"), 0); +} + +TEST_F(DeviceContainerTest, test_on_list_nodes) +{ + // Lifecycle operation true + std::vector params; + std::string package("canopen_core"); + std::string master_name("ros2_canopen::CanopenMaster"); + std::string driver_name("ros2_canopen::CanopenDriver"); + std::string master_node_name("master"); + std::string driver_node_name("driver"); + uint16_t node_id; + auto master_node = std::make_shared(master_node_name); + auto driver_node = std::make_shared(driver_node_name); + device_container->lifecycle_manager_ = std::make_unique(rclcpp::NodeOptions()); + + auto node_factory = std::make_shared(); + auto master_node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(master_node), + std::bind(&MockCanopenMaster::get_node_base_interface, master_node)); + auto driver_node_instance = rclcpp_components::NodeInstanceWrapper( + std::static_pointer_cast(driver_node), + std::bind(&MockCanopenDriver::get_node_base_interface, driver_node)); + + node_id = 1; + device_container->can_master_id_ = node_id; + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)) + .Times(1) + .WillOnce(Return(master_node_instance)); + device_container->load_component(package, master_name, node_id, master_node_name, params); + + node_id = 2; + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)); + EXPECT_CALL(*device_container, get_component_resources(_, _)); + EXPECT_CALL(*device_container, create_component_factory(_)) + .Times(1) + .WillOnce(Return(node_factory)); + EXPECT_CALL(*node_factory, create_node_instance(_)) + .Times(1) + .WillOnce(Return(driver_node_instance)); + EXPECT_CALL(*driver_node, is_lifecycle()).Times(1).WillOnce(Return(false)); + device_container->load_component(package, driver_name, node_id, driver_node_name, params); + + const std::shared_ptr request_header(nullptr); + auto request = std::make_shared(); + auto response = std::make_shared(); + + device_container->on_list_nodes(request_header, request, response); + + for (size_t i = 0; i < response->full_node_names.size(); i++) + { + if (response->full_node_names[i].compare(std::string("/").append(master_node_name)) == 0) + { + EXPECT_EQ(response->unique_ids[i], 1U); + } + else if (response->full_node_names[i].compare(std::string("/").append(driver_node_name)) == 0) + { + EXPECT_EQ(response->unique_ids[i], 2U); + } + else if (response->full_node_names[i].compare(std::string("/").append(driver_node_name)) == 0) + { + EXPECT_EQ(response->unique_ids[i], 256U); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("TEST"), response->full_node_names[i].c_str()); + FAIL(); + } + } +} + +TEST_F(DeviceContainerTest, test_load_master_good) +{ + std::string file_name("bus_configs/good_master.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + auto can_master = std::make_shared("master"); + + EXPECT_CALL(*device_container, load_master()); + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)) + .WillOnce( + [this, can_master]( + std::string & package_name, std::string & driver_name, uint16_t node_id, + std::string & node_name, std::vector & params) -> bool + { + device_container->can_master_ = + std::static_pointer_cast(can_master); + return true; + }); + EXPECT_CALL(*device_container, add_node_to_executor(_)); + EXPECT_CALL(*can_master, init()); + EXPECT_CALL(*can_master, is_lifecycle()); + EXPECT_TRUE(device_container->load_master()); +} + +TEST_F(DeviceContainerTest, test_load_master_load_component_fail) +{ + std::string file_name("bus_configs/good_master.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + auto can_master = std::make_shared("master"); + + EXPECT_CALL(*device_container, load_master()); + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)).WillOnce(Return(false)); + EXPECT_FALSE(device_container->load_master()); +} + +TEST_F(DeviceContainerTest, test_load_master_bad_no_driver) +{ + std::string file_name("bus_configs/bad_master_no_driver.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + auto can_master = std::make_shared("master"); + + EXPECT_CALL(*device_container, load_master()); + EXPECT_FALSE(device_container->load_master()); +} + +TEST_F(DeviceContainerTest, test_load_master_bad_no_id) +{ + std::string file_name("bus_configs/bad_master_no_id.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + auto can_master = std::make_shared("master"); + + EXPECT_CALL(*device_container, load_master()); + EXPECT_FALSE(device_container->load_master()); +} + +TEST_F(DeviceContainerTest, test_load_master_bad_no_package) +{ + std::string file_name("bus_configs/bad_master_no_package.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + auto can_master = std::make_shared("master"); + + EXPECT_CALL(*device_container, load_master()); + EXPECT_FALSE(device_container->load_master()); +} + +TEST_F(DeviceContainerTest, test_load_driver_good) +{ + std::string file_name("bus_configs/good_driver.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + auto driver = std::make_shared("driver"); + + EXPECT_CALL(*device_container, load_drivers()); + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)) + .WillOnce( + [this, driver]( + std::string & package_name, std::string & driver_name, uint16_t node_id, + std::string & node_name, std::vector & params) -> bool + { + device_container->registered_drivers_[2] = + std::static_pointer_cast(driver); + return true; + }); + EXPECT_CALL(*device_container, add_node_to_executor(_)); + EXPECT_CALL(*driver, init()); + EXPECT_TRUE(device_container->load_drivers()); +} + +TEST_F(DeviceContainerTest, test_load_driver_load_component_fail) +{ + std::string file_name("bus_configs/good_driver.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + auto driver = std::make_shared("driver"); + + EXPECT_CALL(*device_container, load_drivers()); + EXPECT_CALL(*device_container, load_component(_, _, _, _, _)).WillOnce(Return(false)); + EXPECT_FALSE(device_container->load_drivers()); +} + +TEST_F(DeviceContainerTest, test_load_driver_bad_no_id) +{ + std::string file_name("bus_configs/bad_driver_no_id.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + + EXPECT_CALL(*device_container, load_drivers()); + EXPECT_FALSE(device_container->load_drivers()); +} + +TEST_F(DeviceContainerTest, test_load_driver_bad_no_driver) +{ + std::string file_name("bus_configs/bad_driver_no_driver.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + + EXPECT_CALL(*device_container, load_drivers()); + EXPECT_FALSE(device_container->load_drivers()); +} + +TEST_F(DeviceContainerTest, test_load_driver_bad_no_package) +{ + std::string file_name("bus_configs/bad_driver_no_package.yml"); + device_container->config_ = std::make_shared(file_name); + device_container->config_->init_config(); + + EXPECT_CALL(*device_container, load_drivers()); + EXPECT_FALSE(device_container->load_drivers()); +} + +TEST_F(DeviceContainerTest, test_load_manager_no_lifecycle) +{ + device_container->lifecycle_operation_ = false; + EXPECT_CALL(*device_container, load_manager()); + EXPECT_TRUE(device_container->load_manager()); + EXPECT_FALSE(device_container->lifecycle_manager_); +} + +TEST_F(DeviceContainerTest, test_load_manager_lifecycle) +{ + device_container->lifecycle_operation_ = true; + EXPECT_CALL(*device_container, load_manager()); + EXPECT_CALL(*device_container, add_node_to_executor(_)); + EXPECT_TRUE(device_container->load_manager()); + EXPECT_TRUE(device_container->lifecycle_manager_); +} + +TEST_F(DeviceContainerTest, test_configure_good) +{ + device_container->set_parameter(rclcpp::Parameter("can_interface_name", "vcan0")); + device_container->set_parameter(rclcpp::Parameter("master_config", "master.dcf")); + device_container->set_parameter( + rclcpp::Parameter("bus_config", "bus_configs/good_master_and_two_driver.yml")); + + EXPECT_CALL(*device_container, configure()); + EXPECT_NO_THROW(device_container->configure()); +} + +TEST_F(DeviceContainerTest, test_init) +{ + device_container->set_parameter(rclcpp::Parameter("can_interface_name", "vcan0")); + device_container->set_parameter(rclcpp::Parameter("master_config", "master.dcf")); + device_container->set_parameter( + rclcpp::Parameter("bus_config", "bus_configs/good_master_and_two_driver.yml")); + + EXPECT_CALL(*device_container, configure()).WillOnce(Return()); + EXPECT_CALL(*device_container, load_master()).WillOnce(Return(true)); + EXPECT_CALL(*device_container, load_drivers()).WillOnce(Return(true)); + EXPECT_CALL(*device_container, load_manager()).WillOnce(Return(true)); + EXPECT_NO_THROW(device_container->init()); +} + +TEST_F(DeviceContainerTest, test_init_with_fparam) +{ + EXPECT_CALL(*device_container, load_master()).WillOnce(Return(true)); + EXPECT_CALL(*device_container, load_drivers()).WillOnce(Return(true)); + EXPECT_CALL(*device_container, load_manager()).WillOnce(Return(true)); + EXPECT_NO_THROW(device_container->init( + "vcan0", "master.dcf", "bus_configs/good_master_and_two_driver.yml", "")); +} + +// TEST(ComponentLoad, test_device_container_configure) +// { +// rclcpp::init(0, nullptr); +// auto exec = std::make_shared(); +// auto device_container = std::make_shared(exec); +// exec->add_node(device_container); + +// device_container->set_parameter(Parameter("bus_config", "bus.yml")); +// device_container->set_parameter(Parameter("master_config", "master.dcf")); +// device_container->set_parameter(Parameter("can_interface_name", "can0")); + +// exec->spin_some(100ms); + +// device_container->configure(); + +// auto time_now = std::chrono::steady_clock::now(); +// auto time_future = time_now + 100ms; +// while (time_future > std::chrono::steady_clock::now()) +// { +// exec->spin_some(100ms); +// } +// rclcpp::shutdown(); +// } + +// TEST(ComponentLoad, test_load_master) +// { +// rclcpp::init(0, nullptr); +// auto exec = std::make_shared(); +// auto device_container = std::make_shared(exec); +// exec->add_node(device_container); + +// device_container->set_parameter(Parameter("bus_config", "bus.yml")); +// device_container->set_parameter(Parameter("master_config", "master.dcf")); +// device_container->set_parameter(Parameter("can_interface_name", "vcan0")); + +// exec->spin_some(100ms); + +// device_container->configure(); +// device_container->load_master(); + +// std::thread spinner( +// [exec]() +// { +// exec->spin(); +// RCLCPP_INFO(rclcpp::get_logger("test"), "Executor done."); +// }); +// std::this_thread::sleep_for(500ms); +// device_container->shutdown(); +// rclcpp::shutdown(); +// spinner.join(); +// RCLCPP_INFO(rclcpp::get_logger("test"), "rclcpp::shutdown"); +// } + +// TEST(ComponentLoad, test_load_component_2) +// { +// rclcpp::init(0, nullptr); +// auto exec = std::make_shared(); +// auto device_container = std::make_shared(exec); +// exec->add_node(device_container); + +// device_container->set_parameter(Parameter("bus_config", "bus.yml")); +// device_container->set_parameter(Parameter("master_config", "master.dcf")); +// device_container->set_parameter(Parameter("can_interface_name", "vcan0")); + +// std::thread spinner ( +// [exec](){ +// exec->spin(); +// RCLCPP_INFO(rclcpp::get_logger("test"), "Executor done."); +// } +// ); +// device_container->configure(); +// device_container->load_master(); +// EXPECT_THROW(device_container->load_drivers(), std::system_error); +// std::this_thread::sleep_for(500ms); +// device_container->shutdown(); +// rclcpp::shutdown(); +// spinner.join(); + +// } diff --git a/canopen_core/test/test_errors.cpp b/canopen_core/test/test_errors.cpp new file mode 100644 index 00000000..09ae656c --- /dev/null +++ b/canopen_core/test/test_errors.cpp @@ -0,0 +1,27 @@ +#include +#include "canopen_core/device_container_error.hpp" +#include "canopen_core/driver_error.hpp" +#include "canopen_core/master_error.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +using namespace std::chrono_literals; +using namespace ros2_canopen; +using namespace testing; + +TEST(DriverErrorTest, test_what) +{ + DriverException test_obj("hello"); + EXPECT_TRUE(std::string("hello").compare(test_obj.what()) == 0); +} + +TEST(MasterErrorTest, test_what) +{ + MasterException test_obj("hello"); + EXPECT_TRUE(std::string("hello").compare(test_obj.what()) == 0); +} + +TEST(DeviceContainerErrorTest, test_what) +{ + DeviceContainerException test_obj("hello"); + EXPECT_TRUE(std::string("hello").compare(test_obj.what()) == 0); +} diff --git a/canopen_core/test/test_lifecycle_canopen_driver.cpp b/canopen_core/test/test_lifecycle_canopen_driver.cpp new file mode 100644 index 00000000..7526a1f2 --- /dev/null +++ b/canopen_core/test/test_lifecycle_canopen_driver.cpp @@ -0,0 +1,124 @@ +#include +#include "canopen_core/driver_node.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +using namespace std::chrono_literals; +using namespace ros2_canopen; +using namespace testing; + +class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenDriverInterface +{ +public: + MOCK_METHOD( + void, set_master, + (std::shared_ptr exec, std::shared_ptr master), + (override)); + MOCK_METHOD(void, demand_set_master, (), (override)); + MOCK_METHOD(void, init, (), (override)); + MOCK_METHOD(void, configure, (), (override)); + MOCK_METHOD(void, activate, (), (override)); + MOCK_METHOD(void, deactivate, (), (override)); + MOCK_METHOD(void, cleanup, (), (override)); + MOCK_METHOD(void, shutdown, (), (override)); + MOCK_METHOD(void, add_to_master, (), (override)); + MOCK_METHOD(void, remove_from_master, (), (override)); +}; + +class MockCanopenMaster : public LifecycleCanopenDriver +{ + friend class CanopenDriverTest; + FRIEND_TEST(CanopenDriverTest, test_init); +}; + +class CanopenDriverTest : public testing::Test +{ +public: + std::shared_ptr canopen_driver; + std::shared_ptr node_canopen_driver; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "SetUp"); + rclcpp::init(0, nullptr); + canopen_driver = std::make_shared(); + node_canopen_driver = std::make_shared(); + canopen_driver->node_canopen_driver_ = + std::static_pointer_cast( + node_canopen_driver); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenDriverTest"), "TearDown"); + rclcpp::shutdown(); + } +}; + +TEST_F(CanopenDriverTest, test_init) +{ + EXPECT_CALL(*node_canopen_driver, init()).Times(1); + canopen_driver->init(); +} + +TEST_F(CanopenDriverTest, test_set_master) +{ + std::shared_ptr exec; + std::shared_ptr master; + + EXPECT_CALL(*node_canopen_driver, set_master(_, _)).Times(1); + EXPECT_NO_THROW(canopen_driver->set_master(exec, master)); +} + +TEST_F(CanopenDriverTest, test_get_node_base_interface) +{ + auto base_iface = canopen_driver->get_node_base_interface(); + EXPECT_TRUE(base_iface); +} + +TEST_F(CanopenDriverTest, test_shutdown) +{ + EXPECT_CALL(*node_canopen_driver, shutdown()); + EXPECT_NO_THROW(canopen_driver->shutdown()); +} + +TEST_F(CanopenDriverTest, test_is_lifecycle) { EXPECT_TRUE(canopen_driver->is_lifecycle()); } + +TEST_F(CanopenDriverTest, test_get_node_canopen_driver_interface) +{ + EXPECT_TRUE(canopen_driver->get_node_canopen_driver_interface() == node_canopen_driver); +} + +TEST_F(CanopenDriverTest, test_on_configure) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_driver, configure()); + EXPECT_CALL(*node_canopen_driver, demand_set_master()); + canopen_driver->on_configure(state); +} + +TEST_F(CanopenDriverTest, test_on_activate) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_driver, activate()); + canopen_driver->on_activate(state); +} + +TEST_F(CanopenDriverTest, test_on_deactivate) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_driver, deactivate()); + canopen_driver->on_deactivate(state); +} + +TEST_F(CanopenDriverTest, test_on_cleanup) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_driver, cleanup()); + canopen_driver->on_cleanup(state); +} + +TEST_F(CanopenDriverTest, test_on_shutdown) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_driver, shutdown()); + canopen_driver->on_shutdown(state); +} diff --git a/canopen_core/test/test_lifecycle_canopen_master.cpp b/canopen_core/test/test_lifecycle_canopen_master.cpp new file mode 100644 index 00000000..68c53d77 --- /dev/null +++ b/canopen_core/test/test_lifecycle_canopen_master.cpp @@ -0,0 +1,116 @@ +#include +#include "canopen_core/master_node.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +using namespace std::chrono_literals; +using namespace ros2_canopen; +using namespace testing; + +class MockNodeCanopenMaster : public ros2_canopen::node_interfaces::NodeCanopenMasterInterface +{ +public: + MOCK_METHOD(void, init, (), (override)); + MOCK_METHOD(void, configure, (), (override)); + MOCK_METHOD(void, activate, (), (override)); + MOCK_METHOD(void, deactivate, (), (override)); + MOCK_METHOD(void, cleanup, (), (override)); + MOCK_METHOD(void, shutdown, (), (override)); + MOCK_METHOD(std::shared_ptr, get_master, (), (override)); + MOCK_METHOD(std::shared_ptr, get_executor, (), (override)); +}; + +class MockCanopenMaster : public LifecycleCanopenMaster +{ + friend class CanopenMasterTest; + FRIEND_TEST(CanopenMasterTest, test_init); +}; + +class CanopenMasterTest : public testing::Test +{ +public: + std::shared_ptr canopen_master; + std::shared_ptr node_canopen_master; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); + rclcpp::init(0, nullptr); + canopen_master = std::make_shared(); + node_canopen_master = std::make_shared(); + canopen_master->node_canopen_master_ = + std::static_pointer_cast( + node_canopen_master); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); + rclcpp::shutdown(); + } +}; + +TEST_F(CanopenMasterTest, test_init) +{ + EXPECT_CALL(*node_canopen_master, init()).Times(1); + canopen_master->init(); +} + +TEST_F(CanopenMasterTest, test_get_node_base_interface) +{ + auto base_iface = canopen_master->get_node_base_interface(); + EXPECT_TRUE(base_iface); +} + +TEST_F(CanopenMasterTest, test_shutdown) +{ + EXPECT_CALL(*node_canopen_master, shutdown()); + EXPECT_NO_THROW(canopen_master->shutdown()); +} + +TEST_F(CanopenMasterTest, test_is_lifecycle) { EXPECT_TRUE(canopen_master->is_lifecycle()); } + +TEST_F(CanopenMasterTest, test_get_master) +{ + EXPECT_CALL(*node_canopen_master, get_master()); + EXPECT_NO_THROW(canopen_master->get_master()); +} + +TEST_F(CanopenMasterTest, test_get_executor) +{ + EXPECT_CALL(*node_canopen_master, get_executor()); + EXPECT_NO_THROW(canopen_master->get_executor()); +} + +TEST_F(CanopenMasterTest, test_on_configure) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_master, configure()); + canopen_master->on_configure(state); +} + +TEST_F(CanopenMasterTest, test_on_activate) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_master, activate()); + canopen_master->on_activate(state); +} + +TEST_F(CanopenMasterTest, test_on_deactivate) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_master, deactivate()); + canopen_master->on_deactivate(state); +} + +TEST_F(CanopenMasterTest, test_on_cleanup) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_master, cleanup()); + canopen_master->on_cleanup(state); +} + +TEST_F(CanopenMasterTest, test_on_shutdown) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*node_canopen_master, shutdown()); + canopen_master->on_shutdown(state); +} diff --git a/canopen_core/test/test_lifecycle_manager.cpp b/canopen_core/test/test_lifecycle_manager.cpp new file mode 100644 index 00000000..fdf7fa26 --- /dev/null +++ b/canopen_core/test/test_lifecycle_manager.cpp @@ -0,0 +1,400 @@ +#include +#include "canopen_core/lifecycle_manager.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +using namespace std::chrono_literals; +using namespace ros2_canopen; +using namespace testing; + +class MockLifecycleManager : public LifecycleManager +{ +public: + FRIEND_TEST(LifecycleManagerTest, test_constructor); + FRIEND_TEST(LifecycleManagerTest, test_init); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_master); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_master_false_initial_state); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_master_failed_activate); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_master_failed_configure); + FRIEND_TEST(LifecycleManagerTest, test_bring_down_master); + FRIEND_TEST(LifecycleManagerTest, test_bring_down_master_false_state); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_false_state_master); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_false_state_driver); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_fail_configure); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_driver_fail_activate); + FRIEND_TEST(LifecycleManagerTest, test_bring_down_driver); + FRIEND_TEST(LifecycleManagerTest, test_bring_down_driver_failed); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_all); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_all_fail_master); + FRIEND_TEST(LifecycleManagerTest, test_bring_up_all_fail_driver); + FRIEND_TEST(LifecycleManagerTest, test_bring_down_all); + FRIEND_TEST(LifecycleManagerTest, test_bring_down_all_fail_master); + FRIEND_TEST(LifecycleManagerTest, test_bring_down_all_fail_driver); + FRIEND_TEST(LifecycleManagerTest, test_on_configure); + FRIEND_TEST(LifecycleManagerTest, test_on_configure_fail); + FRIEND_TEST(LifecycleManagerTest, test_on_activate); + FRIEND_TEST(LifecycleManagerTest, test_on_activate_fail); + FRIEND_TEST(LifecycleManagerTest, test_on_deactivate); + FRIEND_TEST(LifecycleManagerTest, test_on_deactivate_fail); + FRIEND_TEST(LifecycleManagerTest, test_on_cleanup); + FRIEND_TEST(LifecycleManagerTest, test_on_shutdown); + + MockLifecycleManager() : LifecycleManager(rclcpp::NodeOptions()) {} + MOCK_METHOD(unsigned int, get_state, (uint8_t node_id, std::chrono::seconds time_out)); + MOCK_METHOD( + bool, change_state, (uint8_t node_id, uint8_t transition, std::chrono::seconds time_out)); + MOCK_METHOD(bool, bring_up_master, (), (override)); + MOCK_METHOD(bool, bring_down_master, (), (override)); + MOCK_METHOD(bool, bring_up_driver, (std::string device_name), (override)); + MOCK_METHOD(bool, bring_down_driver, (std::string device_name), (override)); + MOCK_METHOD(bool, load_from_config, (), (override)); + MOCK_METHOD(bool, bring_up_all, (), (override)); + MOCK_METHOD(bool, bring_down_all, (), (override)); + + void DelegateToBase() + { + ON_CALL(*this, get_state) + .WillByDefault( + [this](uint8_t node_id, std::chrono::seconds time_out) -> unsigned int + { return LifecycleManager::get_state(node_id, time_out); }); + ON_CALL(*this, change_state) + .WillByDefault( + [this](uint8_t node_id, uint8_t transition, std::chrono::seconds time_out) -> unsigned int + { return LifecycleManager::change_state(node_id, transition, time_out); }); + ON_CALL(*this, bring_up_master) + .WillByDefault([this]() -> bool { return LifecycleManager::bring_up_master(); }); + ON_CALL(*this, bring_down_master) + .WillByDefault([this]() -> bool { return LifecycleManager::bring_down_master(); }); + ON_CALL(*this, bring_up_driver) + .WillByDefault( + [this](std::string device_name) -> bool + { return LifecycleManager::bring_up_driver(device_name); }); + ON_CALL(*this, bring_down_driver) + .WillByDefault( + [this](std::string device_name) -> bool + { return LifecycleManager::bring_down_driver(device_name); }); + ON_CALL(*this, load_from_config) + .WillByDefault([this]() -> bool { return LifecycleManager::load_from_config(); }); + ON_CALL(*this, bring_up_all) + .WillByDefault([this]() -> bool { return LifecycleManager::bring_up_all(); }); + ON_CALL(*this, bring_down_all) + .WillByDefault([this]() -> bool { return LifecycleManager::bring_down_all(); }); + } +}; + +class LifecycleManagerTest : public testing::Test +{ +public: + std::shared_ptr lifecycle_manager; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); + rclcpp::init(0, nullptr); + lifecycle_manager = std::make_shared(); + lifecycle_manager->DelegateToBase(); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); + rclcpp::shutdown(); + } + + void SetUpConfigurationManager() + { + std::string file_name("bus_configs/good_master_and_two_driver.yml"); + auto cmanager = std::make_shared(file_name); + cmanager->init_config(); + lifecycle_manager->init(cmanager); + EXPECT_CALL(*lifecycle_manager, load_from_config()); + lifecycle_manager->load_from_config(); + } +}; + +TEST_F(LifecycleManagerTest, test_constructor) +{ + EXPECT_TRUE(lifecycle_manager->has_parameter("container_name")); + EXPECT_TRUE(lifecycle_manager->cbg_clients); +} + +TEST_F(LifecycleManagerTest, test_init) +{ + std::string file_name("bus_configs/good_master_and_two_driver.yml"); + auto cmanager = std::make_shared(file_name); + cmanager->init_config(); + lifecycle_manager->init(cmanager); + EXPECT_TRUE(lifecycle_manager->config_); +} + +TEST_F(LifecycleManagerTest, test_bring_up_master) +{ + EXPECT_CALL(*lifecycle_manager, bring_up_master()); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(1U)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); + EXPECT_TRUE(lifecycle_manager->bring_up_master()); +} + +TEST_F(LifecycleManagerTest, test_bring_up_master_false_initial_state) +{ + EXPECT_CALL(*lifecycle_manager, bring_up_master()); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(0U)); + EXPECT_FALSE(lifecycle_manager->bring_up_master()); +} + +TEST_F(LifecycleManagerTest, test_bring_up_master_failed_configure) +{ + EXPECT_CALL(*lifecycle_manager, bring_up_master()); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(1U)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(1).WillOnce(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_up_master()); +} + +TEST_F(LifecycleManagerTest, test_bring_up_master_failed_activate) +{ + EXPECT_CALL(*lifecycle_manager, bring_up_master()); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)).Times(1).WillOnce(Return(1U)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) + .Times(2) + .WillOnce(Return(true)) + .WillOnce(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_up_master()); +} + +TEST_F(LifecycleManagerTest, test_bring_down_master) +{ + EXPECT_CALL(*lifecycle_manager, bring_down_master()); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(1) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + EXPECT_TRUE(lifecycle_manager->bring_down_master()); +} + +TEST_F(LifecycleManagerTest, test_bring_down_master_false_state) +{ + EXPECT_CALL(*lifecycle_manager, bring_down_master()); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(1) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + EXPECT_FALSE(lifecycle_manager->bring_down_master()); +} + +TEST_F(LifecycleManagerTest, test_bring_up_driver) +{ + SetUpConfigurationManager(); + EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(2) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(2).WillRepeatedly(Return(true)); + EXPECT_TRUE(lifecycle_manager->bring_up_driver("proxy_device_2")); +} + +TEST_F(LifecycleManagerTest, test_bring_up_driver_false_state_master) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(1) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); +} + +TEST_F(LifecycleManagerTest, test_bring_up_driver_false_state_driver) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(2) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); +} + +TEST_F(LifecycleManagerTest, test_bring_up_driver_fail_configure) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(2) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)).Times(1).WillOnce(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); +} + +TEST_F(LifecycleManagerTest, test_bring_up_driver_fail_activate) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(2) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) + .Times(2) + .WillOnce(Return(true)) + .WillOnce(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_up_driver("proxy_device_2")); +} + +TEST_F(LifecycleManagerTest, test_bring_down_driver) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) + .Times(2) + .WillOnce(Return(true)) + .WillOnce(Return(true)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(1) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)); + EXPECT_TRUE(lifecycle_manager->bring_down_driver("proxy_device_2")); +} + +TEST_F(LifecycleManagerTest, test_bring_down_driver_failed) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)); + EXPECT_CALL(*lifecycle_manager, change_state(_, _, _)) + .Times(2) + .WillOnce(Return(true)) + .WillOnce(Return(true)); + EXPECT_CALL(*lifecycle_manager, get_state(_, _)) + .Times(1) + .WillOnce(Return(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); + EXPECT_FALSE(lifecycle_manager->bring_down_driver("proxy_device_2")); +} + +TEST_F(LifecycleManagerTest, test_bring_up_all) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_up_all); + EXPECT_CALL(*lifecycle_manager, bring_up_master()).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)).Times(2).WillRepeatedly(Return(true)); + EXPECT_TRUE(lifecycle_manager->bring_up_all()); +} + +TEST_F(LifecycleManagerTest, test_bring_up_all_fail_master) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_up_all); + EXPECT_CALL(*lifecycle_manager, bring_up_master()).Times(1).WillOnce(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_up_all()); +} + +TEST_F(LifecycleManagerTest, test_bring_up_all_fail_driver) +{ + SetUpConfigurationManager(); + + EXPECT_CALL(*lifecycle_manager, bring_up_all); + EXPECT_CALL(*lifecycle_manager, bring_up_master()).Times(1).WillOnce(Return(true)); + EXPECT_CALL(*lifecycle_manager, bring_up_driver(_)).Times(1).WillRepeatedly(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_up_all()); +} + +TEST_F(LifecycleManagerTest, test_bring_down_all) +{ + SetUpConfigurationManager(); + EXPECT_CALL(*lifecycle_manager, bring_down_all); + EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)).Times(2).WillRepeatedly(Return(true)); + EXPECT_CALL(*lifecycle_manager, bring_down_master()).Times(1).WillOnce(Return(true)); + EXPECT_TRUE(lifecycle_manager->bring_down_all()); +} + +TEST_F(LifecycleManagerTest, test_bring_down_all_fail_driver) +{ + SetUpConfigurationManager(); + EXPECT_CALL(*lifecycle_manager, bring_down_all); + EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)).Times(1).WillRepeatedly(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_down_all()); +} + +TEST_F(LifecycleManagerTest, test_bring_down_all_fail_master) +{ + SetUpConfigurationManager(); + EXPECT_CALL(*lifecycle_manager, bring_down_all); + EXPECT_CALL(*lifecycle_manager, bring_down_driver(_)).Times(2).WillRepeatedly(Return(true)); + EXPECT_CALL(*lifecycle_manager, bring_down_master()).Times(1).WillOnce(Return(false)); + EXPECT_FALSE(lifecycle_manager->bring_down_all()); +} + +TEST_F(LifecycleManagerTest, test_on_configure) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*lifecycle_manager, load_from_config()).Times(1).WillOnce(Return(true)); + EXPECT_EQ( + lifecycle_manager->on_configure(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +TEST_F(LifecycleManagerTest, test_on_configure_fail) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*lifecycle_manager, load_from_config()).Times(1).WillOnce(Return(false)); + EXPECT_EQ( + lifecycle_manager->on_configure(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR); +} + +TEST_F(LifecycleManagerTest, test_on_activate) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*lifecycle_manager, bring_up_all()).Times(1).WillOnce(Return(true)); + EXPECT_EQ( + lifecycle_manager->on_activate(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +TEST_F(LifecycleManagerTest, test_on_activate_fail) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*lifecycle_manager, bring_up_all()).Times(1).WillOnce(Return(false)); + EXPECT_EQ( + lifecycle_manager->on_activate(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR); +} + +TEST_F(LifecycleManagerTest, test_on_deactivate) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*lifecycle_manager, bring_down_all()).Times(1).WillOnce(Return(true)); + EXPECT_EQ( + lifecycle_manager->on_deactivate(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +TEST_F(LifecycleManagerTest, test_on_deactivate_fail) +{ + rclcpp_lifecycle::State state; + EXPECT_CALL(*lifecycle_manager, bring_down_all()).Times(1).WillOnce(Return(false)); + EXPECT_EQ( + lifecycle_manager->on_deactivate(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR); +} + +TEST_F(LifecycleManagerTest, test_on_cleanup) +{ + rclcpp_lifecycle::State state; + EXPECT_EQ( + lifecycle_manager->on_cleanup(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +TEST_F(LifecycleManagerTest, test_on_shutdown) +{ + rclcpp_lifecycle::State state; + EXPECT_EQ( + lifecycle_manager->on_shutdown(state), + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} diff --git a/canopen_core/test/test_node_canopen_driver.cpp b/canopen_core/test/test_node_canopen_driver.cpp new file mode 100644 index 00000000..557fe273 --- /dev/null +++ b/canopen_core/test/test_node_canopen_driver.cpp @@ -0,0 +1,298 @@ +#include "canopen_core/driver_node.hpp" +#include "canopen_core/node_interfaces/node_canopen_driver.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +using namespace ros2_canopen; +using namespace ros2_canopen::node_interfaces; +using namespace testing; +template +class MockNodeCanopenDriver : public NodeCanopenDriver +{ +public: + FRIEND_TEST(NodeCanopenDriverTest, test_construct); + FRIEND_TEST(NodeCanopenDriverTest, test_init); + FRIEND_TEST(NodeCanopenDriverTest, test_init_fail_configured); + FRIEND_TEST(NodeCanopenDriverTest, test_init_fail_activated); + FRIEND_TEST(NodeCanopenDriverTest, test_set_master); + FRIEND_TEST(NodeCanopenDriverTest, test_configure); + FRIEND_TEST(NodeCanopenDriverTest, test_configure_fail_not_initialsed); + FRIEND_TEST(NodeCanopenDriverTest, test_configure_fail_configured); + FRIEND_TEST(NodeCanopenDriverTest, test_configure_fail_activated); + FRIEND_TEST(NodeCanopenDriverTest, test_activate); + FRIEND_TEST(NodeCanopenDriverTest, test_activate_failures); + FRIEND_TEST(NodeCanopenDriverTest, test_deactivate); + FRIEND_TEST(NodeCanopenDriverTest, test_deactivate_failures); + FRIEND_TEST(NodeCanopenDriverTest, test_cleanup); + FRIEND_TEST(NodeCanopenDriverTest, test_cleanup_failures); + FRIEND_TEST(NodeCanopenDriverTest, test_shutdown); + + MockNodeCanopenDriver(rclcpp::Node * node) : NodeCanopenDriver(node) {} + MOCK_METHOD0_T(add_to_master, void()); + MOCK_METHOD0_T(remove_from_master, void()); + + void DelegateToBase() + { + // ON_CALL(*this, load_component) + // .WillByDefault( + // [this]( + // std::string & package_name, std::string & driver_name, uint16_t node_id, + // std::string & node_name, std::vector & params) -> bool + // { return DeviceContainer::load_component(package_name, driver_name, node_id, + // node_name, params); }); + } +}; + +class NodeCanopenDriverTest : public testing::Test +{ +public: + std::shared_ptr> node_canopen_driver; + rclcpp::Node::SharedPtr node; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); + rclcpp::init(0, nullptr); + node = std::make_shared("driver"); + node_canopen_driver = std::make_shared>(node.get()); + node_canopen_driver->DelegateToBase(); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); + rclcpp::shutdown(); + } +}; + +TEST_F(NodeCanopenDriverTest, test_construct) { EXPECT_EQ(node.get(), node_canopen_driver->node_); } + +TEST_F(NodeCanopenDriverTest, test_init) +{ + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + node_canopen_driver->init(); + EXPECT_TRUE(node_canopen_driver->initialised_.load()); + EXPECT_TRUE(node->has_parameter("container_name")); + EXPECT_TRUE(node->has_parameter("node_id")); + EXPECT_TRUE(node->has_parameter("non_transmit_timeout")); + EXPECT_TRUE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenDriverTest, test_init_fail_configured) +{ + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(true); + EXPECT_THROW(node_canopen_driver->init(), DriverException); + EXPECT_FALSE(node_canopen_driver->initialised_.load()); + EXPECT_FALSE(node->has_parameter("container_name")); + EXPECT_FALSE(node->has_parameter("node_id")); + EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); + EXPECT_FALSE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenDriverTest, test_init_fail_activated) +{ + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(true); + EXPECT_THROW(node_canopen_driver->init(), DriverException); + EXPECT_FALSE(node_canopen_driver->initialised_.load()); + EXPECT_FALSE(node->has_parameter("container_name")); + EXPECT_FALSE(node->has_parameter("node_id")); + EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); + EXPECT_FALSE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenDriverTest, test_set_master) +{ + std::shared_ptr exec; + std::shared_ptr master; + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + + node_canopen_driver->set_master(exec, master); + EXPECT_TRUE(node_canopen_driver->master_set_.load()); + + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(true); + + EXPECT_THROW(node_canopen_driver->set_master(exec, master), DriverException); + + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + + EXPECT_THROW(node_canopen_driver->set_master(exec, master), DriverException); +} + +TEST_F(NodeCanopenDriverTest, test_configure) +{ + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + node_canopen_driver->init(); + node->set_parameter(rclcpp::Parameter( + "config", + "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " + "\"simple.eds\"\ndcf_path: \"\"\n")); + std::cout << node->get_parameter("config").as_string() << std::endl; + node_canopen_driver->configure(); + EXPECT_TRUE(node_canopen_driver->configured_.load()); +} + +TEST_F(NodeCanopenDriverTest, test_configure_fail_not_initialsed) +{ + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + EXPECT_THROW(node_canopen_driver->configure(), DriverException); + EXPECT_FALSE(node_canopen_driver->configured_.load()); +} + +TEST_F(NodeCanopenDriverTest, test_configure_fail_configured) +{ + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + node_canopen_driver->init(); + node_canopen_driver->configured_.store(true); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); + EXPECT_THROW(node_canopen_driver->configure(), DriverException); +} + +TEST_F(NodeCanopenDriverTest, test_configure_fail_activated) +{ + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + node_canopen_driver->init(); + node_canopen_driver->activated_.store(true); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); + EXPECT_THROW(node_canopen_driver->configure(), DriverException); + EXPECT_FALSE(node_canopen_driver->configured_.load()); +} + +TEST_F(NodeCanopenDriverTest, test_activate) +{ + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + EXPECT_CALL(*node_canopen_driver, add_to_master()).Times(1).WillOnce(Return()); + node_canopen_driver->activate(); + EXPECT_TRUE(node_canopen_driver->activated_.load()); +} + +TEST_F(NodeCanopenDriverTest, test_activate_failures) +{ + node_canopen_driver->initialised_.store(false); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + EXPECT_THROW(node_canopen_driver->activate(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(false); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + EXPECT_THROW(node_canopen_driver->activate(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + EXPECT_THROW(node_canopen_driver->activate(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(true); + EXPECT_THROW(node_canopen_driver->activate(), DriverException); +} + +TEST_F(NodeCanopenDriverTest, test_deactivate) +{ + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(true); + EXPECT_CALL(*node_canopen_driver, remove_from_master()).Times(1).WillOnce(Return()); + node_canopen_driver->deactivate(); + EXPECT_FALSE(node_canopen_driver->activated_.load()); +} + +TEST_F(NodeCanopenDriverTest, test_deactivate_failures) +{ + node_canopen_driver->initialised_.store(false); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(true); + EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(false); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(true); + EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(true); + EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->master_set_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + EXPECT_THROW(node_canopen_driver->deactivate(), DriverException); +} + +TEST_F(NodeCanopenDriverTest, test_cleanup) +{ + node_canopen_driver->initialised_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + node_canopen_driver->cleanup(); + EXPECT_FALSE(node_canopen_driver->configured_.load()); +} + +TEST_F(NodeCanopenDriverTest, test_cleanup_failures) +{ + node_canopen_driver->initialised_.store(false); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + EXPECT_THROW(node_canopen_driver->cleanup(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + EXPECT_THROW(node_canopen_driver->cleanup(), DriverException); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(true); + EXPECT_THROW(node_canopen_driver->cleanup(), DriverException); +} + +TEST_F(NodeCanopenDriverTest, test_shutdown) +{ + node_canopen_driver->master_set_.store(true); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(true); + EXPECT_CALL(*node_canopen_driver, remove_from_master()).Times(1).WillOnce(Return()); + EXPECT_NO_THROW(node_canopen_driver->shutdown()); + EXPECT_FALSE(node_canopen_driver->master_set_.load()); + EXPECT_FALSE(node_canopen_driver->initialised_.load()); + EXPECT_FALSE(node_canopen_driver->configured_.load()); + EXPECT_FALSE(node_canopen_driver->activated_.load()); + + node_canopen_driver->master_set_.store(true); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->configured_.store(true); + node_canopen_driver->activated_.store(false); + EXPECT_NO_THROW(node_canopen_driver->shutdown()); + EXPECT_FALSE(node_canopen_driver->master_set_.load()); + EXPECT_FALSE(node_canopen_driver->initialised_.load()); + EXPECT_FALSE(node_canopen_driver->configured_.load()); + EXPECT_FALSE(node_canopen_driver->activated_.load()); + + node_canopen_driver->master_set_.store(true); + node_canopen_driver->initialised_.store(true); + node_canopen_driver->configured_.store(false); + node_canopen_driver->activated_.store(false); + EXPECT_NO_THROW(node_canopen_driver->shutdown()); + EXPECT_FALSE(node_canopen_driver->master_set_.load()); + EXPECT_FALSE(node_canopen_driver->initialised_.load()); + EXPECT_FALSE(node_canopen_driver->configured_.load()); + EXPECT_FALSE(node_canopen_driver->activated_.load()); +} diff --git a/canopen_core/test/test_node_canopen_master.cpp b/canopen_core/test/test_node_canopen_master.cpp new file mode 100644 index 00000000..a0f4a705 --- /dev/null +++ b/canopen_core/test/test_node_canopen_master.cpp @@ -0,0 +1,445 @@ +#include "canopen_core/master_node.hpp" +#include "canopen_core/node_interfaces/node_canopen_master.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +using namespace ros2_canopen; +using namespace ros2_canopen::node_interfaces; +using namespace testing; +template +class MockNodeCanopenMaster : public NodeCanopenMaster +{ +public: + FRIEND_TEST(NodeCanopenMasterTest, test_construct); + FRIEND_TEST(NodeCanopenMasterTest, test_init); + FRIEND_TEST(NodeCanopenMasterTest, test_init_fail_configured); + FRIEND_TEST(NodeCanopenMasterTest, test_init_fail_activated); + FRIEND_TEST(NodeCanopenMasterTest, test_set_master); + FRIEND_TEST(NodeCanopenMasterTest, test_configure); + FRIEND_TEST(NodeCanopenMasterTest, test_configure_fail_not_initialsed); + FRIEND_TEST(NodeCanopenMasterTest, test_configure_fail_configured); + FRIEND_TEST(NodeCanopenMasterTest, test_configure_fail_activated); + FRIEND_TEST(NodeCanopenMasterTest, test_activate); + FRIEND_TEST(NodeCanopenMasterTest, test_activate_failures); + FRIEND_TEST(NodeCanopenMasterTest, test_deactivate); + FRIEND_TEST(NodeCanopenMasterTest, test_deactivate_failures); + FRIEND_TEST(NodeCanopenMasterTest, test_cleanup); + FRIEND_TEST(NodeCanopenMasterTest, test_cleanup_failures); + FRIEND_TEST(NodeCanopenMasterTest, test_shutdown); + FRIEND_TEST(NodeCanopenMasterTest, test_get_master); + FRIEND_TEST(NodeCanopenMasterTest, test_get_executor); + + FRIEND_TEST(NodeCanopenLMasterTest, test_construct); + FRIEND_TEST(NodeCanopenLMasterTest, test_init); + FRIEND_TEST(NodeCanopenLMasterTest, test_init_fail_configured); + FRIEND_TEST(NodeCanopenLMasterTest, test_init_fail_activated); + FRIEND_TEST(NodeCanopenLMasterTest, test_set_master); + FRIEND_TEST(NodeCanopenLMasterTest, test_configure); + FRIEND_TEST(NodeCanopenLMasterTest, test_configure_fail_not_initialsed); + FRIEND_TEST(NodeCanopenLMasterTest, test_configure_fail_configured); + FRIEND_TEST(NodeCanopenLMasterTest, test_configure_fail_activated); + FRIEND_TEST(NodeCanopenLMasterTest, test_activate); + FRIEND_TEST(NodeCanopenLMasterTest, test_activate_failures); + FRIEND_TEST(NodeCanopenLMasterTest, test_deactivate); + FRIEND_TEST(NodeCanopenLMasterTest, test_deactivate_failures); + FRIEND_TEST(NodeCanopenLMasterTest, test_cleanup); + FRIEND_TEST(NodeCanopenLMasterTest, test_cleanup_failures); + FRIEND_TEST(NodeCanopenLMasterTest, test_shutdown); + FRIEND_TEST(NodeCanopenLMasterTest, test_get_master); + FRIEND_TEST(NodeCanopenLMasterTest, test_get_executor); + + MockNodeCanopenMaster(NODETYPE * node) : NodeCanopenMaster(node) {} + + void DelegateToBase() + { + // ON_CALL(*this, load_component) + // .WillByDefault( + // [this]( + // std::string & package_name, std::string & driver_name, uint16_t node_id, + // std::string & node_name, std::vector & params) -> bool + // { return DeviceContainer::load_component(package_name, driver_name, node_id, + // node_name, params); }); + } +}; + +class NodeCanopenMasterTest : public testing::Test +{ +public: + std::shared_ptr> node_canopen_master; + rclcpp::Node::SharedPtr node; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); + rclcpp::init(0, nullptr); + node = std::make_shared("driver"); + node_canopen_master = std::make_shared>(node.get()); + node_canopen_master->DelegateToBase(); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); + rclcpp::shutdown(); + } +}; + +TEST_F(NodeCanopenMasterTest, test_construct) { EXPECT_EQ(node.get(), node_canopen_master->node_); } + +TEST_F(NodeCanopenMasterTest, test_init) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + EXPECT_TRUE(node_canopen_master->initialised_.load()); + EXPECT_TRUE(node->has_parameter("container_name")); + EXPECT_TRUE(node->has_parameter("node_id")); + EXPECT_TRUE(node->has_parameter("non_transmit_timeout")); + EXPECT_TRUE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenMasterTest, test_init_fail_configured) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->init(), MasterException); + EXPECT_FALSE(node_canopen_master->initialised_.load()); + EXPECT_FALSE(node->has_parameter("container_name")); + EXPECT_FALSE(node->has_parameter("node_id")); + EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); + EXPECT_FALSE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenMasterTest, test_init_fail_activated) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->init(), MasterException); + EXPECT_FALSE(node_canopen_master->initialised_.load()); + EXPECT_FALSE(node->has_parameter("container_name")); + EXPECT_FALSE(node->has_parameter("node_id")); + EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); + EXPECT_FALSE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenMasterTest, test_configure) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenMaster\"\npackage:\"canopen_core\"\n")); + node_canopen_master->configure(); + EXPECT_TRUE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenMasterTest, test_configure_fail_not_initialsed) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->configure(), MasterException); + EXPECT_FALSE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenMasterTest, test_configure_fail_configured) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + node_canopen_master->configured_.store(true); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); + EXPECT_THROW(node_canopen_master->configure(), MasterException); +} + +TEST_F(NodeCanopenMasterTest, test_configure_fail_activated) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + node_canopen_master->activated_.store(true); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); + EXPECT_THROW(node_canopen_master->configure(), MasterException); + EXPECT_FALSE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenMasterTest, test_activate_failures) +{ + node_canopen_master->initialised_.store(false); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->activate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->activate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->activate(), MasterException); +} + +TEST_F(NodeCanopenMasterTest, test_deactivate_failures) +{ + node_canopen_master->initialised_.store(false); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->deactivate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->deactivate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->deactivate(), MasterException); +} + +TEST_F(NodeCanopenMasterTest, test_cleanup) +{ + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + node_canopen_master->cleanup(); + EXPECT_FALSE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenMasterTest, test_cleanup_failures) +{ + node_canopen_master->initialised_.store(false); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->cleanup(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->cleanup(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->cleanup(), MasterException); +} + +TEST_F(NodeCanopenMasterTest, test_shutdown) +{ + node_canopen_master->master_set_.store(true); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_NO_THROW(node_canopen_master->shutdown()); + EXPECT_FALSE(node_canopen_master->master_set_.load()); + EXPECT_FALSE(node_canopen_master->initialised_.load()); + EXPECT_FALSE(node_canopen_master->configured_.load()); + EXPECT_FALSE(node_canopen_master->activated_.load()); +} + +TEST_F(NodeCanopenMasterTest, test_get_master) +{ + node_canopen_master->master_set_.store(true); + EXPECT_NO_THROW(node_canopen_master->get_master()); + + node_canopen_master->master_set_.store(false); + EXPECT_ANY_THROW(node_canopen_master->get_master()); +} + +TEST_F(NodeCanopenMasterTest, test_get_executor) +{ + node_canopen_master->master_set_.store(true); + EXPECT_NO_THROW(node_canopen_master->get_executor()); + + node_canopen_master->master_set_.store(false); + EXPECT_ANY_THROW(node_canopen_master->get_executor()); +} + +class NodeCanopenLMasterTest : public testing::Test +{ +public: + std::shared_ptr> node_canopen_master; + rclcpp_lifecycle::LifecycleNode::SharedPtr node; + void SetUp() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "SetUp"); + rclcpp::init(0, nullptr); + node = std::make_shared("driver"); + node_canopen_master = + std::make_shared>(node.get()); + node_canopen_master->DelegateToBase(); + } + + void TearDown() override + { + RCLCPP_INFO(rclcpp::get_logger("CanopenMasterTest"), "TearDown"); + rclcpp::shutdown(); + } +}; + +TEST_F(NodeCanopenLMasterTest, test_construct) +{ + EXPECT_EQ(node.get(), node_canopen_master->node_); +} + +TEST_F(NodeCanopenLMasterTest, test_init) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + EXPECT_TRUE(node_canopen_master->initialised_.load()); + EXPECT_TRUE(node->has_parameter("container_name")); + EXPECT_TRUE(node->has_parameter("node_id")); + EXPECT_TRUE(node->has_parameter("non_transmit_timeout")); + EXPECT_TRUE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenLMasterTest, test_init_fail_configured) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->init(), MasterException); + EXPECT_FALSE(node_canopen_master->initialised_.load()); + EXPECT_FALSE(node->has_parameter("container_name")); + EXPECT_FALSE(node->has_parameter("node_id")); + EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); + EXPECT_FALSE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenLMasterTest, test_init_fail_activated) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->init(), MasterException); + EXPECT_FALSE(node_canopen_master->initialised_.load()); + EXPECT_FALSE(node->has_parameter("container_name")); + EXPECT_FALSE(node->has_parameter("node_id")); + EXPECT_FALSE(node->has_parameter("non_transmit_timeout")); + EXPECT_FALSE(node->has_parameter("config")); +} + +TEST_F(NodeCanopenLMasterTest, test_configure) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenMaster\"\npackage:\"canopen_core\"\n")); + node_canopen_master->configure(); + EXPECT_TRUE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenLMasterTest, test_configure_fail_not_initialsed) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->configure(), MasterException); + EXPECT_FALSE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenLMasterTest, test_configure_fail_configured) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + node_canopen_master->configured_.store(true); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); + EXPECT_THROW(node_canopen_master->configure(), MasterException); +} + +TEST_F(NodeCanopenLMasterTest, test_configure_fail_activated) +{ + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + node_canopen_master->init(); + node_canopen_master->activated_.store(true); + node->set_parameter(rclcpp::Parameter( + "config", "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage:\"canopen_core\"\n")); + EXPECT_THROW(node_canopen_master->configure(), MasterException); + EXPECT_FALSE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenLMasterTest, test_activate_failures) +{ + node_canopen_master->initialised_.store(false); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->activate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->activate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->activate(), MasterException); +} + +TEST_F(NodeCanopenLMasterTest, test_deactivate_failures) +{ + node_canopen_master->initialised_.store(false); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->deactivate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->deactivate(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->deactivate(), MasterException); +} + +TEST_F(NodeCanopenLMasterTest, test_cleanup) +{ + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + node_canopen_master->cleanup(); + EXPECT_FALSE(node_canopen_master->configured_.load()); +} + +TEST_F(NodeCanopenLMasterTest, test_cleanup_failures) +{ + node_canopen_master->initialised_.store(false); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->cleanup(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_THROW(node_canopen_master->cleanup(), MasterException); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(true); + node_canopen_master->activated_.store(true); + EXPECT_THROW(node_canopen_master->cleanup(), MasterException); +} + +TEST_F(NodeCanopenLMasterTest, test_shutdown) +{ + node_canopen_master->master_set_.store(true); + node_canopen_master->initialised_.store(true); + node_canopen_master->configured_.store(false); + node_canopen_master->activated_.store(false); + EXPECT_NO_THROW(node_canopen_master->shutdown()); + EXPECT_FALSE(node_canopen_master->master_set_.load()); + EXPECT_FALSE(node_canopen_master->initialised_.load()); + EXPECT_FALSE(node_canopen_master->configured_.load()); + EXPECT_FALSE(node_canopen_master->activated_.load()); +} + +TEST_F(NodeCanopenLMasterTest, test_get_master) +{ + node_canopen_master->master_set_.store(true); + EXPECT_NO_THROW(node_canopen_master->get_master()); + + node_canopen_master->master_set_.store(false); + EXPECT_ANY_THROW(node_canopen_master->get_master()); +} + +TEST_F(NodeCanopenLMasterTest, test_get_executor) +{ + node_canopen_master->master_set_.store(true); + EXPECT_NO_THROW(node_canopen_master->get_executor()); + + node_canopen_master->master_set_.store(false); + EXPECT_ANY_THROW(node_canopen_master->get_executor()); +} diff --git a/canopen_mock_slave/CMakeLists.txt b/canopen_fake_slaves/CMakeLists.txt similarity index 86% rename from canopen_mock_slave/CMakeLists.txt rename to canopen_fake_slaves/CMakeLists.txt index 30853a29..c342f411 100644 --- a/canopen_mock_slave/CMakeLists.txt +++ b/canopen_fake_slaves/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(canopen_mock_slave) +project(canopen_fake_slaves) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -7,10 +7,17 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) find_package(lely_core_libraries REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(lifecycle_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +set(dependencies + lely_core_libraries + lifecycle_msgs + rclcpp + rclcpp_lifecycle +) add_executable( basic_slave_node @@ -24,16 +31,14 @@ target_include_directories(basic_slave_node PUBLIC ament_target_dependencies( basic_slave_node - "rclcpp" - "lely_core_libraries" - "rclcpp_lifecycle" - "lifecycle_msgs" + ${dependencies} ) add_executable( cia402_slave_node "src/cia402_slave.cpp" + "src/motion_generator.cpp" ) target_compile_features(cia402_slave_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_include_directories(cia402_slave_node PUBLIC @@ -42,19 +47,14 @@ target_include_directories(cia402_slave_node PUBLIC ament_target_dependencies( cia402_slave_node - "rclcpp" - "lely_core_libraries" - "rclcpp_lifecycle" - "lifecycle_msgs" + ${dependencies} ) - - install(TARGETS basic_slave_node DESTINATION lib/${PROJECT_NAME}) - install(TARGETS cia402_slave_node - DESTINATION lib/${PROJECT_NAME}) +install(TARGETS cia402_slave_node +DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch/ @@ -66,8 +66,6 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/config/ ) - - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -80,4 +78,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +ament_export_dependencies( + ${dependencies} +) + ament_package() diff --git a/canopen_mock_slave/LICENSE b/canopen_fake_slaves/LICENSE similarity index 100% rename from canopen_mock_slave/LICENSE rename to canopen_fake_slaves/LICENSE diff --git a/canopen_fake_slaves/Readme.md b/canopen_fake_slaves/Readme.md new file mode 100644 index 00000000..9cb4dff3 --- /dev/null +++ b/canopen_fake_slaves/Readme.md @@ -0,0 +1,7 @@ +# CANopen Mock Slaves + +## Motion Controller Slaves + +Supported modes: +* Cyclic Position +* Profiled Position (Thanks to motion generator https://github.com/EFeru/MotionGenerator) diff --git a/canopen_mock_slave/config/cia402_slave.eds b/canopen_fake_slaves/config/cia402_slave.eds similarity index 93% rename from canopen_mock_slave/config/cia402_slave.eds rename to canopen_fake_slaves/config/cia402_slave.eds index 3373b54f..d757e1dc 100644 --- a/canopen_mock_slave/config/cia402_slave.eds +++ b/canopen_fake_slaves/config/cia402_slave.eds @@ -1440,4 +1440,3 @@ DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 - diff --git a/canopen_mock_slave/config/simple_slave.eds b/canopen_fake_slaves/config/simple_slave.eds similarity index 99% rename from canopen_mock_slave/config/simple_slave.eds rename to canopen_fake_slaves/config/simple_slave.eds index 80c20bac..77126767 100644 --- a/canopen_mock_slave/config/simple_slave.eds +++ b/canopen_fake_slaves/config/simple_slave.eds @@ -267,4 +267,4 @@ PDOMapping=1 ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr -PDOMapping=1 \ No newline at end of file +PDOMapping=1 diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp new file mode 100644 index 00000000..2282c0e2 --- /dev/null +++ b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp @@ -0,0 +1,87 @@ + +#ifndef SLAVE_HPP +#define SLAVE_HPP +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace ros2_canopen +{ +class BaseSlave : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit BaseSlave(const std::string & node_name, bool intra_process_comms = false) + : rclcpp_lifecycle::LifecycleNode( + node_name, rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) + { + this->declare_parameter("node_id", 2); + this->declare_parameter("slave_config", "slave.eds"); + this->declare_parameter("can_interface_name", "vcan0"); + this->activated.store(false); + } + + virtual ~BaseSlave() + { + if (this->run_thread.joinable()) + { + run_thread.join(); + } + } + + virtual void run() = 0; + +protected: + std::thread run_thread; + int node_id_; + std::string slave_config_; + std::string can_interface_name_; + std::atomic activated; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &) + { + this->activated.store(false); + RCLCPP_INFO(this->get_logger(), "Reaching inactive state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &) + { + this->activated.store(true); + get_parameter("node_id", node_id_); + get_parameter("slave_config", slave_config_); + get_parameter("can_interface_name", can_interface_name_); + run_thread = std::thread(std::bind(&BaseSlave::run, this)); + RCLCPP_INFO(this->get_logger(), "Reaching active state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &) + { + this->activated.store(false); + RCLCPP_INFO(this->get_logger(), "Reaching inactive state."); + if (run_thread.joinable()) + { + run_thread.join(); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &) + { + this->activated.store(false); + RCLCPP_INFO(this->get_logger(), "Reaching unconfigured state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(this->get_logger(), "Shutdown"); + } +}; +} // namespace ros2_canopen + +#endif diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp new file mode 100644 index 00000000..89cef4ef --- /dev/null +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -0,0 +1,115 @@ +#ifndef BASIC_SLAVE_HPP +#define BASIC_SLAVE_HPP +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "canopen_fake_slaves/base_slave.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using namespace lely; +using namespace std::chrono_literals; +namespace ros2_canopen +{ +class SimpleSlave : public canopen::BasicSlave +{ +public: + using BasicSlave::BasicSlave; + +protected: + /** + * @brief This function is called when a value is written to the local object dictionary by an SDO + * or RPDO. Also copies the RPDO value to TPDO. + * @param idx The index of the PDO. + * @param subidx The subindex of the PDO. + */ + void OnWrite(uint16_t idx, uint8_t subidx) noexcept override + { + // uint32_t val = (*this)[idx][subidx]; + //(*this)[0x4001][0] = val; + // this->TpdoEvent(0); + } +}; + +class BasicSlave : public BaseSlave +{ +public: + explicit BasicSlave(const std::string & node_name, bool intra_process_comms = false) + : BaseSlave(node_name, intra_process_comms) + { + } + +protected: + class ActiveCheckTask : public ev::CoTask + { + public: + ActiveCheckTask(io::Context * ctx, ev::Executor * exec, BasicSlave * slave) : CoTask(*exec) + { + slave_ = slave; + exec_ = exec; + ctx_ = ctx; + } + + protected: + BasicSlave * slave_; + ev::Executor * exec_; + io::Context * ctx_; + virtual void operator()() noexcept + { + if (slave_->activated.load()) + { + } + ctx_->shutdown(); + } + }; + + void run() override + { + io::IoGuard io_guard; + io::Context ctx; + io::Poll poll(ctx); + ev::Loop loop(poll.get_poll()); + auto exec = loop.get_executor(); + io::Timer timer(poll, exec, CLOCK_MONOTONIC); + io::CanController ctrl(can_interface_name_.c_str()); + io::CanChannel chan(poll, exec); + chan.open(ctrl); + + auto sigset_ = lely::io::SignalSet(poll, exec); + // Watch for Ctrl+C or process termination. + sigset_.insert(SIGHUP); + sigset_.insert(SIGINT); + sigset_.insert(SIGTERM); + + sigset_.submit_wait( + [&](int /*signo*/) + { + // If the signal is raised again, terminate immediately. + sigset_.clear(); + + // Perform a clean shutdown. + ctx.shutdown(); + }); + + SimpleSlave slave(timer, chan, slave_config_.c_str(), "", node_id_); + slave.Reset(); + ActiveCheckTask checktask(&ctx, &exec, this); + + // timer.submit_wait() + RCLCPP_INFO(this->get_logger(), "Created slave for node_id %i.", node_id_); + loop.run(); + ctx.shutdown(); + RCLCPP_INFO(this->get_logger(), "Stopped CANopen Event Loop."); + rclcpp::shutdown(); + } +}; +} // namespace ros2_canopen + +#endif diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp new file mode 100644 index 00000000..3496e768 --- /dev/null +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -0,0 +1,655 @@ +#ifndef CIA402_SLAVE_HPP +#define CIA402_SLAVE_HPP +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "canopen_fake_slaves/base_slave.hpp" +#include "canopen_fake_slaves/motion_generator.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using namespace lely; +using namespace std::chrono_literals; + +namespace ros2_canopen +{ + +class CIA402MockSlave : public canopen::BasicSlave +{ +public: + explicit CIA402MockSlave( + io::TimerBase & timer, io::CanChannelBase & chan, const ::std::string & dcf_txt, + const ::std::string & dcf_bin = "", uint8_t id = 0xff) + : canopen::BasicSlave(timer, chan, dcf_txt, dcf_bin, id) + { + state.store(InternalState::Not_Ready_To_Switch_On); + status_word = 0x0; + operation_mode.store(No_Mode); + control_cycle_period = 0.01; + actual_position = 0.0; + } + + virtual ~CIA402MockSlave() + { + if (profiled_position_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined profiled_position_mode thread."); + profiled_position_mode.join(); + } + if (cyclic_position_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread."); + cyclic_position_mode.join(); + } + } + +protected: + enum InternalState + { + Unknown = 0, + Start = 0, + Not_Ready_To_Switch_On = 1, + Switch_On_Disabled = 2, + Ready_To_Switch_On = 3, + Switched_On = 4, + Operation_Enable = 5, + Quick_Stop_Active = 6, + Fault_Reaction_Active = 7, + Fault = 8, + }; + + enum StatusWord + { + SW_Ready_To_Switch_On = 0, + SW_Switched_On = 1, + SW_Operation_enabled = 2, + SW_Fault = 3, + SW_Voltage_enabled = 4, + SW_Quick_stop = 5, + SW_Switch_on_disabled = 6, + SW_Warning = 7, + SW_Manufacturer_specific0 = 8, + SW_Remote = 9, + SW_Target_reached = 10, + SW_Internal_limit = 11, + SW_Operation_mode_specific0 = 12, + SW_Operation_mode_specific1 = 13, + SW_Manufacturer_specific1 = 14, + SW_Manufacturer_specific2 = 15 + }; + enum ControlWord + { + CW_Switch_On = 0, + CW_Enable_Voltage = 1, + CW_Quick_Stop = 2, + CW_Enable_Operation = 3, + CW_Operation_mode_specific0 = 4, + CW_Operation_mode_specific1 = 5, + CW_Operation_mode_specific2 = 6, + CW_Fault_Reset = 7, + CW_Halt = 8, + CW_Operation_mode_specific3 = 9, + // CW_Reserved1=10, + CW_Manufacturer_specific0 = 11, + CW_Manufacturer_specific1 = 12, + CW_Manufacturer_specific2 = 13, + CW_Manufacturer_specific3 = 14, + CW_Manufacturer_specific4 = 15, + }; + + enum OperationMode + { + No_Mode = 0, + Profiled_Position = 1, + Velocity = 2, + Profiled_Velocity = 3, + Profiled_Torque = 4, + Reserved = 5, + Homing = 6, + Interpolated_Position = 7, + Cyclic_Synchronous_Position = 8, + Cyclic_Synchronous_Velocity = 9, + Cyclic_Synchronous_Torque = 10, + }; + std::atomic is_relative; + std::atomic is_running; + std::atomic is_halt; + std::atomic is_new_set_point; + std::atomic operation_mode; + std::atomic old_operation_mode; + + std::mutex w_mutex; + uint16_t status_word; + uint16_t control_word; + std::atomic state; + + std::thread profiled_position_mode; + std::thread profiled_velocity_mode; + std::thread cyclic_position_mode; + std::thread cyclic_velocity_mode; + + double cycle_time; + + std::mutex in_mode_mutex; + double actual_position; + double actual_speed; + double acceleration; + double control_cycle_period; + + void run_profiled_position_mode() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_profiled_position_mode"); + double profile_speed = static_cast(((uint32_t)(*this)[0x6081][0])) / 1000; + double profile_accerlation = static_cast(((uint32_t)(*this)[0x6083][0])) / 1000; + double actual_position = static_cast(((int32_t)(*this)[0x6064][0])) / 1000.0; + double target_position = static_cast(((int32_t)(*this)[0x607A][0])) / 1000.0; + double actual_speed = static_cast(((int32_t)(*this)[0x606C][0])) / 1000.0; + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Profile_Speed %f, Profile Acceleration: %f", + profile_speed, profile_accerlation); + + while ((state.load() == InternalState::Operation_Enable) && + (operation_mode.load() == Profiled_Position) && (rclcpp::ok())) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + target_position = static_cast(((int32_t)(*this)[0x607A][0])) / 1000.0; + if (target_position != actual_position) + { + clear_status_bit(SW_Operation_mode_specific0); + clear_status_bit(SW_Target_reached); + { + std::scoped_lock lock(w_mutex); + (*this)[0x6041][0] = status_word; + this->TpdoEvent(1); + } + is_new_set_point.store(false); + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Move from %f to %f", actual_position, + target_position); + { + MotionGenerator gen(profile_accerlation, profile_speed, actual_position); + + while (!gen.getFinished()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + actual_position = gen.update(target_position); + actual_speed = gen.getVelocity(); + (*this)[0x6064][0] = (int32_t)(actual_position * 1000); + (*this)[0x606C][0] = (int32_t)(actual_speed * 1000); + } + } + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Reached target position %f", actual_position); + clear_status_bit(SW_Operation_mode_specific0); + set_status_bit(SW_Target_reached); + { + std::scoped_lock lock(w_mutex); + (*this)[0x6041][0] = status_word; + this->TpdoEvent(1); + } + } + } + } + void run_cyclic_position_mode() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_cyclic_position_mode"); + int32_t min_pos = (int32_t)(*this)[0x607D][1]; + int32_t max_pos = (int32_t)(*this)[0x607D][2]; + uint8_t int_period = (*this)[0x60C2][1]; + int32_t offset = (*this)[0x60B0][0]; + int8_t index = (*this)[0x60C2][2]; + + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Lower Software Limit: %d", min_pos); + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Upper Software Limit: %d", max_pos); + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Control Cycle: %hhu + 10^%hhd", int_period, index); + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Offset: %d", offset); + + double cp_min_position = min_pos / 1000; + double cp_max_position = max_pos / 1000; + double cp_interpolation_period = int_period * std::pow(10.0, index); + double cp_offset = (double)(offset / 1000.0); + int ccp_millis = (int)(control_cycle_period * std::pow(10.0, 3)); + int32_t act_pos; + while ((state.load() == InternalState::Operation_Enable) && + (operation_mode.load() == Cyclic_Synchronous_Position) && (rclcpp::ok())) + { + act_pos = (*this)[0x607A][0]; + double target_position = (act_pos) / 1000 - cp_offset; // m + double position_delta = target_position - actual_position; // m + double speed = position_delta / cp_interpolation_period; // m/s + double increment = control_cycle_period * speed; // m + (*this)[0x606C][0] = (int32_t)speed * 1000; + if ( + (target_position < cp_max_position) && (target_position > cp_min_position) && + (std::abs(position_delta) > 0.001)) + { + while ((std::abs(actual_position - target_position) > 0.001) && (rclcpp::ok())) + { + std::this_thread::sleep_for(std::chrono::milliseconds(ccp_millis)); + actual_position += increment; + (*this)[0x6064][0] = (int32_t)actual_position * 1000; + if (std::abs(actual_position - target_position) < 0.001) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Reached Target %f", target_position); + } + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(ccp_millis)); + } + } + + void run_position_mode() {} + + void set_new_status_word_and_state() + { + switch (state.load()) + { + case InternalState::Not_Ready_To_Switch_On: + on_not_ready_to_switch_on(); + break; + case InternalState::Switch_On_Disabled: + on_switch_on_disabled(); + break; + case InternalState::Ready_To_Switch_On: + on_ready_to_switch_on(); + break; + case InternalState::Switched_On: + on_switched_on(); + break; + case InternalState::Operation_Enable: + on_operation_enabled(); + break; + case InternalState::Quick_Stop_Active: + on_quickstop_active(); + break; + case InternalState::Fault_Reaction_Active: + break; + case InternalState::Fault: + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Fault"); + break; + default: + break; + } + } + + void set_status_bit(int bit) + { + std::scoped_lock lock(w_mutex); + status_word |= 1UL << bit; + } + + void clear_status_bit(int bit) + { + std::scoped_lock lock(w_mutex); + status_word &= ~(1UL << bit); + } + + void set_switch_on_disabled() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switch_On_Disabled"); + state.store(InternalState::Switch_On_Disabled); + clear_status_bit(SW_Ready_To_Switch_On); + clear_status_bit(SW_Switched_On); + clear_status_bit(SW_Operation_enabled); + clear_status_bit(SW_Fault); + set_status_bit(SW_Switch_on_disabled); + } + + void set_ready_to_switch_on() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Ready_To_Switch_On"); + state.store(InternalState::Ready_To_Switch_On); + set_status_bit(SW_Ready_To_Switch_On); + clear_status_bit(SW_Switched_On); + clear_status_bit(SW_Operation_enabled); + clear_status_bit(SW_Fault); + set_status_bit(SW_Quick_stop); + clear_status_bit(SW_Switch_on_disabled); + } + + void set_switch_on() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switched_On"); + state.store(InternalState::Switched_On); + set_status_bit(SW_Ready_To_Switch_On); + set_status_bit(SW_Switched_On); + clear_status_bit(SW_Operation_enabled); + clear_status_bit(SW_Fault); + set_status_bit(SW_Quick_stop); + clear_status_bit(SW_Switch_on_disabled); + } + + void set_operation_enabled() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Operation_Enable"); + state.store(InternalState::Operation_Enable); + set_status_bit(SW_Ready_To_Switch_On); + set_status_bit(SW_Switched_On); + set_status_bit(SW_Operation_enabled); + clear_status_bit(SW_Fault); + set_status_bit(SW_Quick_stop); + clear_status_bit(SW_Switch_on_disabled); + } + + void set_quick_stop() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Quick_Stop_Active"); + state.store(InternalState::Quick_Stop_Active); + set_status_bit(SW_Ready_To_Switch_On); + set_status_bit(SW_Switched_On); + set_status_bit(SW_Operation_enabled); + clear_status_bit(SW_Fault); + clear_status_bit(SW_Quick_stop); + clear_status_bit(SW_Switch_on_disabled); + } + + void on_not_ready_to_switch_on() { set_switch_on_disabled(); } + + void on_switch_on_disabled() + { + if (is_shutdown()) + { + set_ready_to_switch_on(); + } + } + + void on_ready_to_switch_on() + { + if (is_disable_voltage()) + { + set_switch_on_disabled(); + } + if (is_switch_on()) + { + set_switch_on(); + } + if (is_faul_reset()) + { + set_ready_to_switch_on(); + } + } + + void on_switched_on() + { + if (is_disable_voltage()) + { + set_switch_on_disabled(); + } + if (is_shutdown()) + { + set_ready_to_switch_on(); + } + if (is_enable_operation()) + { + set_operation_enabled(); + } + } + + void on_operation_enabled() + { + if (is_disable_voltage()) + { + set_switch_on_disabled(); + } + if (is_shutdown()) + { + set_ready_to_switch_on(); + } + if (is_switch_on()) + { + set_switch_on(); + } + if (is_quickstop()) + { + set_quick_stop(); + } + { + std::scoped_lock lock(w_mutex); + is_relative.store(((control_word >> 6) & 1U) == 1U); + is_halt.store(((control_word >> 8) & 1U) == 1U); + is_new_set_point.store(((control_word >> 4) & 1U) == 1U); + } + + if (old_operation_mode.load() != operation_mode.load()) + { + if (profiled_position_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined profiled_position_mode thread."); + profiled_position_mode.join(); + } + if (cyclic_position_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread."); + cyclic_position_mode.join(); + } + old_operation_mode.store(operation_mode.load()); + switch (operation_mode.load()) + { + case Cyclic_Synchronous_Position: + start_sync_pos_mode(); + break; + case Profiled_Position: + start_profile_pos_mode(); + break; + default: + break; + } + } + } + + void start_sync_pos_mode() + { + cyclic_position_mode = std::thread(std::bind(&CIA402MockSlave::run_cyclic_position_mode, this)); + } + + void start_profile_pos_mode() + { + profiled_position_mode = + std::thread(std::bind(&CIA402MockSlave::run_profiled_position_mode, this)); + } + + void on_quickstop_active() + { + if (is_enable_operation()) + { + set_operation_enabled(); + } + if (is_disable_voltage()) + { + set_switch_on_disabled(); + } + } + + bool is_shutdown() + { + std::scoped_lock lock(w_mutex); + bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; + bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; + bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; + bool so_unset = ((control_word >> CW_Switch_On) & 1U) == 0U; + + if (fr_unset && qs_set && ev_set && so_unset) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Shutdown."); + return true; + } + return false; + } + + bool is_disable_voltage() + { + std::scoped_lock lock(w_mutex); + bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; + bool ev_unset = ((control_word >> CW_Enable_Voltage) & 1U) == 0U; + + if (fr_unset && ev_unset) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Disable Voltage."); + return true; + } + return false; + } + + bool is_switch_on() + { + std::scoped_lock lock(w_mutex); + bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; + bool eo_unset = ((control_word >> CW_Enable_Operation) & 1U) == 0U; + bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; + bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; + bool so_set = ((control_word >> CW_Switch_On) & 1U) == 1U; + if (fr_unset && eo_unset && qs_set && ev_set && so_set) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Switch On."); + return true; + } + return false; + } + + bool is_enable_operation() + { + std::scoped_lock lock(w_mutex); + bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; + bool eo_set = ((control_word >> CW_Enable_Operation) & 1U) == 1U; + bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; + bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; + bool so_set = ((control_word >> CW_Switch_On) & 1U) == 1U; + if (fr_unset && eo_set && qs_set && ev_set && so_set) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Enable Operation."); + return true; + } + return false; + } + + bool is_quickstop() + { + std::scoped_lock lock(w_mutex); + bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; + bool qs_unset = ((control_word >> CW_Quick_Stop) & 1U) == 0U; + bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; + if (fr_unset && qs_unset && ev_set) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Quick Stop."); + return true; + } + return false; + } + + bool is_faul_reset() + { + std::scoped_lock lock(w_mutex); + bool fr_set = ((control_word >> CW_Fault_Reset) & 1U) == 1U; + if (fr_set) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Fault Reset."); + return true; + } + return false; + } + // This function gets called every time a value is written to the local object + // dictionary by an SDO or RPDO. + void OnWrite(uint16_t idx, uint8_t subidx) noexcept override + { + // System State + if (idx == 0x6040 && subidx == 0) + { + { + std::scoped_lock lock(w_mutex); + control_word = (*this)[0x6040][0]; + } + set_new_status_word_and_state(); + { + std::scoped_lock lock(w_mutex); + (*this)[0x6041][0] = status_word; + this->TpdoEvent(1); + } + } + // Operation Mode + if (idx == 0x6060 && subidx == 0) + { + int8_t mode = (*this)[0x6060][0]; + switch (mode) + { + case No_Mode: + case Profiled_Position: + case Velocity: + case Profiled_Velocity: + case Profiled_Torque: + case Reserved: + case Homing: + case Interpolated_Position: + case Cyclic_Synchronous_Position: + case Cyclic_Synchronous_Velocity: + case Cyclic_Synchronous_Torque: + operation_mode.store(mode); + break; + default: + std::cout << "Error: Master tried to set unknown operation mode." << std::endl; + } + // RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switched to mode %hhi.", mode); + (*this)[0x6061][0] = (int8_t)(mode); + this->TpdoEvent(1); + } + } +}; + +class CIA402Slave : public BaseSlave +{ +public: + explicit CIA402Slave(const std::string & node_name, bool intra_process_comms = false) + : BaseSlave(node_name, intra_process_comms) + { + } + + void run() override + { + io::IoGuard io_guard; + io::Context ctx; + io::Poll poll(ctx); + ev::Loop loop(poll.get_poll()); + auto exec = loop.get_executor(); + io::Timer timer(poll, exec, CLOCK_MONOTONIC); + io::CanController ctrl(can_interface_name_.c_str()); + io::CanChannel chan(poll, exec); + chan.open(ctrl); + + auto sigset_ = lely::io::SignalSet(poll, exec); + // Watch for Ctrl+C or process termination. + sigset_.insert(SIGHUP); + sigset_.insert(SIGINT); + sigset_.insert(SIGTERM); + + sigset_.submit_wait( + [&](int /*signo*/) + { + // If the signal is raised again, terminate immediately. + sigset_.clear(); + + // Perform a clean shutdown. + ctx.shutdown(); + }); + + ros2_canopen::CIA402MockSlave slave(timer, chan, slave_config_.c_str(), "", node_id_); + slave.Reset(); + + RCLCPP_INFO(this->get_logger(), "Created cia402 slave for node_id %i.", node_id_); + + loop.run(); + ctx.shutdown(); + RCLCPP_INFO(this->get_logger(), "Stopped CANopen Event Loop."); + rclcpp::shutdown(); + } +}; +} // namespace ros2_canopen +#endif diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp new file mode 100644 index 00000000..831026c9 --- /dev/null +++ b/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp @@ -0,0 +1,109 @@ +#ifndef __MOTIONGENERATOR_H__ +#define __MOTIONGENERATOR_H__ + +#include +/** + * Generates the analytical solution for the trapezoidal motion. + * + *

+ * Usage: + * // Includes + * #include "MotionGenerator.h" + * + * Initialization + * + * @param int aVelMax maximum velocity (units/s) + * @param int aAccMax maximum acceleration (units/s^2) + * @param int aInitPos initial position (units) + * + // Define the MotionGenerator object + MotionGenerator *trapezoidalProfile = new MotionGenerator(100, 400, 0); + // Retrieve calculated position + double positionRef = 1000; + double position = trapezoidalProfile->update(positionRef) + // Retrieve current velocity + double velocity = trapezoidalProfile->getVelocity(); + // Retrieve current acceleration + double acceleration = trapezoidalProfile->getAcceleration(); + // Check if profile is finished + if (trapezoidalProfile->getFinished()) {}; + // Reset internal state + trapezoidalProfile->reset(); + * + * @author AerDronix + * @web https://aerdronix.wordpress.com/ + * @version 1.0 + * @since 2016-12-22 + */ + +class MotionGenerator +{ +public: + /** + * Constructor + * + * @param int aVelocityMax maximum velocity + * @param int aAccelerationMax maximum acceleration + */ + MotionGenerator(double aMaxVel, double aMaxAcc, double aInitPos); + + void init(); + + /** + * Updates the state, generating new setpoints + * + * @param aSetpoint The current setpoint. + */ + double update(double aPosRef); + double getVelocity(); + double getAcceleration(); + + bool getFinished(); + void setMaxVelocity(double aMaxVel); + void setMaxAcceleration(double aMaxAcc); + void setInitPosition(double aInitPos); + void reset(); + +private: + /** + * Increments the state number. + * + * @see + currentState + */ + void calculateTrapezoidalProfile(double); + short int sign(double aVal); + + double maxVel; + double maxAcc; + double initPos; + double pos; + double vel; + double acc; + double oldPos; + double oldPosRef; + double oldVel; + + double dBrk; + double dAcc; + double dVel; + double dDec; + double dTot; + + double tBrk; + double tAcc; + double tVel; + double tDec; + + double velSt; + + std::chrono::time_point oldTime; + std::chrono::time_point lastTime; + std::chrono::duration deltaTime; + + short int signM; // 1 = positive change, -1 = negative change + bool shape; // true = trapezoidal, false = triangular + + bool isFinished; +}; +#endif diff --git a/canopen_fake_slaves/launch/basic_slave.launch.py b/canopen_fake_slaves/launch/basic_slave.launch.py new file mode 100644 index 00000000..a5797738 --- /dev/null +++ b/canopen_fake_slaves/launch/basic_slave.launch.py @@ -0,0 +1,95 @@ +import os +import sys + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) # noqa +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "launch")) # noqa + +import launch +import launch.actions +import launch.events +from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution +from launch.actions import DeclareLaunchArgument + +import launch_ros +import launch_ros.events +import launch_ros.events.lifecycle + +import lifecycle_msgs.msg + + +def generate_launch_description(): + path_to_test = os.path.dirname(__file__) + + node_id_arg = DeclareLaunchArgument( + "node_id", + default_value=TextSubstitution(text="2"), + description="CANopen node id the mock slave shall have.", + ) + + slave_config_arg = DeclareLaunchArgument( + "slave_config", + default_value=TextSubstitution( + text=os.path.join(path_to_test, "..", "config", "simple_slave.eds") + ), + description="Path to eds file to be used for the slave.", + ) + + can_interface_name_arg = DeclareLaunchArgument( + "can_interface_name", + default_value=TextSubstitution(text="vcan0"), + description="CAN interface to be used by mock slave.", + ) + + node_name_arg = DeclareLaunchArgument( + "node_name", + default_value=TextSubstitution(text="basic_slave_node"), + description="Name of the node.", + ) + + slave_node = launch_ros.actions.LifecycleNode( + name=LaunchConfiguration("node_name"), + namespace="", + package="canopen_fake_slaves", + output="screen", + executable="basic_slave_node", + parameters=[ + { + "slave_config": LaunchConfiguration("slave_config"), + "node_id": LaunchConfiguration("node_id"), + "can_interface_name": LaunchConfiguration("can_interface_name"), + } + ], + ) + slave_inactive_state_handler = launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=slave_node, + goal_state="inactive", + handle_once=True, + entities=[ + launch.actions.LogInfo( + msg="node 'basic_slave_node' reached the 'inactive' state, 'activating'." + ), + launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action(slave_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ) + ), + ], + ), + ) + slave_configure = launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action(slave_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + ld = launch.LaunchDescription() + ld.add_action(node_id_arg) + ld.add_action(slave_config_arg) + ld.add_action(can_interface_name_arg) + ld.add_action(node_name_arg) + ld.add_action(slave_inactive_state_handler) + ld.add_action(slave_node) + ld.add_action(slave_configure) + return ld diff --git a/canopen_fake_slaves/launch/cia402_slave.launch.py b/canopen_fake_slaves/launch/cia402_slave.launch.py new file mode 100644 index 00000000..eae159b6 --- /dev/null +++ b/canopen_fake_slaves/launch/cia402_slave.launch.py @@ -0,0 +1,95 @@ +import os +import sys + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) # noqa +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "launch")) # noqa + +import launch +import launch.actions +import launch.events +from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution +from launch.actions import DeclareLaunchArgument + +import launch_ros +import launch_ros.events +import launch_ros.events.lifecycle + +import lifecycle_msgs.msg + + +def generate_launch_description(): + path_to_test = os.path.dirname(__file__) + + node_id_arg = DeclareLaunchArgument( + "node_id", + default_value=TextSubstitution(text="2"), + description="CANopen node id the mock slave shall have.", + ) + + slave_config_arg = DeclareLaunchArgument( + "slave_config", + default_value=TextSubstitution( + text=os.path.join(path_to_test, "..", "config", "cia402_slave.eds") + ), + description="Path to eds file to be used for the slave.", + ) + + can_interface_name_arg = DeclareLaunchArgument( + "can_interface_name", + default_value=TextSubstitution(text="vcan0"), + description="CAN interface to be used by mock slave.", + ) + + node_name_arg = DeclareLaunchArgument( + "node_name", + default_value=TextSubstitution(text="basic_slave_node"), + description="Name of the node.", + ) + + slave_node = launch_ros.actions.LifecycleNode( + name=LaunchConfiguration("node_name"), + namespace="", + package="canopen_fake_slaves", + output="screen", + executable="cia402_slave_node", + parameters=[ + { + "slave_config": LaunchConfiguration("slave_config"), + "node_id": LaunchConfiguration("node_id"), + "can_interface_name": LaunchConfiguration("can_interface_name"), + } + ], + ) + slave_inactive_state_handler = launch.actions.RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=slave_node, + goal_state="inactive", + handle_once=True, + entities=[ + launch.actions.LogInfo( + msg="node 'basic_slave_node' reached the 'inactive' state, 'activating'." + ), + launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action(slave_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + ) + ), + ], + ), + ) + slave_configure = launch.actions.EmitEvent( + event=launch_ros.events.lifecycle.ChangeState( + lifecycle_node_matcher=launch.events.matches_action(slave_node), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + ld = launch.LaunchDescription() + ld.add_action(node_id_arg) + ld.add_action(slave_config_arg) + ld.add_action(can_interface_name_arg) + ld.add_action(node_name_arg) + ld.add_action(slave_inactive_state_handler) + ld.add_action(slave_node) + ld.add_action(slave_configure) + return ld diff --git a/canopen_mock_slave/package.xml b/canopen_fake_slaves/package.xml similarity index 91% rename from canopen_mock_slave/package.xml rename to canopen_fake_slaves/package.xml index ea7469d5..4a51d10c 100644 --- a/canopen_mock_slave/package.xml +++ b/canopen_fake_slaves/package.xml @@ -1,7 +1,7 @@ - canopen_mock_slave + canopen_fake_slaves 0.0.0 Package with mock canopen slave Christoph Hellmann Santos @@ -10,8 +10,9 @@ ament_cmake lely_core_libraries - rclcpp_lifecycle lifecycle_msgs + rclcpp + rclcpp_lifecycle ament_lint_auto diff --git a/canopen_fake_slaves/src/basic_slave.cpp b/canopen_fake_slaves/src/basic_slave.cpp new file mode 100644 index 00000000..f609fbb0 --- /dev/null +++ b/canopen_fake_slaves/src/basic_slave.cpp @@ -0,0 +1,14 @@ +#include "canopen_fake_slaves/basic_slave.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::InitOptions options; + options.shutdown_on_signal = true; + rclcpp::init(argc, argv, options, rclcpp::SignalHandlerOptions::All); + rclcpp::executors::SingleThreadedExecutor executor; + auto canopen_slave = std::make_shared("basic_slave"); + executor.add_node(canopen_slave->get_node_base_interface()); + executor.spin(); + return 0; +} diff --git a/canopen_fake_slaves/src/cia402_slave.cpp b/canopen_fake_slaves/src/cia402_slave.cpp new file mode 100644 index 00000000..093b54b9 --- /dev/null +++ b/canopen_fake_slaves/src/cia402_slave.cpp @@ -0,0 +1,13 @@ +#include "canopen_fake_slaves/cia402_slave.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto canopen_slave = std::make_shared("cia402_slave"); + executor.add_node(canopen_slave->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/canopen_fake_slaves/src/motion_generator.cpp b/canopen_fake_slaves/src/motion_generator.cpp new file mode 100644 index 00000000..662c2948 --- /dev/null +++ b/canopen_fake_slaves/src/motion_generator.cpp @@ -0,0 +1,216 @@ +#include "canopen_fake_slaves/motion_generator.hpp" +#include +#include + +MotionGenerator::MotionGenerator(double aMaxVel, double aMaxAcc, double aInitPos) +: maxVel(aMaxVel), maxAcc(aMaxAcc), initPos(aInitPos), oldPosRef(aInitPos) +{ + init(); +} + +void MotionGenerator::init() +{ + // Time variables + oldTime = std::chrono::high_resolution_clock::now(); + lastTime = oldTime; + deltaTime = std::chrono::milliseconds(0); + + // State variables + reset(); + + // Misc + signM = 1; // 1 = positive change, -1 = negative change + shape = true; // true = trapezoidal, false = triangular + isFinished = false; +} + +double MotionGenerator::update(double posRef) +{ + if (oldPosRef != posRef) // reference changed + { + isFinished = false; + // Shift state variables + oldPosRef = posRef; + oldPos = pos; + oldVel = vel; + oldTime = lastTime; + + // Calculate braking time and distance (in case is needed) + tBrk = abs(oldVel) / maxAcc; + dBrk = tBrk * abs(oldVel) / 2; + + // Calculate Sign of motion + signM = sign(posRef - (oldPos + sign(oldVel) * dBrk)); + + if (signM != sign(oldVel)) // means brake is needed + { + tAcc = (maxVel / maxAcc); + dAcc = tAcc * (maxVel / 2); + } + else + { + tBrk = 0; + dBrk = 0; + tAcc = (maxVel - abs(oldVel)) / maxAcc; + dAcc = tAcc * (maxVel + abs(oldVel)) / 2; + } + + // Calculate total distance to go after braking + dTot = abs(posRef - oldPos + signM * dBrk); + + tDec = maxVel / maxAcc; + dDec = tDec * (maxVel) / 2; + dVel = dTot - (dAcc + dDec); + tVel = dVel / maxVel; + + if (tVel > 0) // trapezoidal shape + shape = true; + else // triangular shape + { + shape = false; + // Recalculate distances and periods + if (signM != sign(oldVel)) // means brake is needed + { + velSt = sqrt(maxAcc * (dTot)); + tAcc = (velSt / maxAcc); + dAcc = tAcc * (velSt / 2); + } + else + { + tBrk = 0; + dBrk = 0; + dTot = abs(posRef - oldPos); // recalculate total distance + velSt = sqrt(0.5 * oldVel * oldVel + maxAcc * dTot); + tAcc = (velSt - abs(oldVel)) / maxAcc; + dAcc = tAcc * (velSt + abs(oldVel)) / 2; + } + tDec = velSt / maxAcc; + dDec = tDec * (velSt) / 2; + } + } + + auto time = std::chrono::high_resolution_clock::now(); // current time + // Calculate time since last set-point change + deltaTime = (time - oldTime); + // Calculate new setpoint + calculateTrapezoidalProfile(posRef); + // Update last time + lastTime = time; + + // calculateTrapezoidalProfile(setpoint); + return pos; +} + +void MotionGenerator::calculateTrapezoidalProfile(double posRef) +{ + double t = deltaTime.count(); // conversion from milliseconds to seconds + + if (shape) // trapezoidal shape + { + if (t <= (tBrk + tAcc)) + { + pos = oldPos + oldVel * t + signM * 0.5 * maxAcc * t * t; + vel = oldVel + signM * maxAcc * t; + acc = signM * maxAcc; + } + else if (t > (tBrk + tAcc) && t < (tBrk + tAcc + tVel)) + { + pos = oldPos + signM * (-dBrk + dAcc + maxVel * (t - tBrk - tAcc)); + vel = signM * maxVel; + acc = 0; + } + else if (t >= (tBrk + tAcc + tVel) && t < (tBrk + tAcc + tVel + tDec)) + { + pos = oldPos + signM * (-dBrk + dAcc + dVel + maxVel * (t - tBrk - tAcc - tVel) - + 0.5 * maxAcc * (t - tBrk - tAcc - tVel) * (t - tBrk - tAcc - tVel)); + vel = signM * (maxVel - maxAcc * (t - tBrk - tAcc - tVel)); + acc = -signM * maxAcc; + } + else + { + pos = posRef; + vel = 0; + acc = 0; + isFinished = true; + } + } + else // triangular shape + { + if (t <= (tBrk + tAcc)) + { + pos = oldPos + oldVel * t + signM * 0.5 * maxAcc * t * t; + vel = oldVel + signM * maxAcc * t; + acc = signM * maxAcc; + } + else if (t > (tBrk + tAcc) && t < (tBrk + tAcc + tDec)) + { + pos = oldPos + signM * (-dBrk + dAcc + velSt * (t - tBrk - tAcc) - + 0.5 * maxAcc * (t - tBrk - tAcc) * (t - tBrk - tAcc)); + vel = signM * (velSt - maxAcc * (t - tBrk - tAcc)); + acc = -signM * maxAcc; + } + else + { + pos = posRef; + vel = 0; + acc = 0; + isFinished = true; + } + } +} + +// Getters and setters +bool MotionGenerator::getFinished() { return isFinished; } + +double MotionGenerator::getVelocity() { return vel; } + +double MotionGenerator::getAcceleration() { return acc; } + +void MotionGenerator::setMaxVelocity(double aMaxVel) { maxVel = aMaxVel; } + +void MotionGenerator::setMaxAcceleration(double aMaxAcc) { maxAcc = aMaxAcc; } + +void MotionGenerator::setInitPosition(double aInitPos) +{ + initPos = aInitPos; + pos = aInitPos; + oldPos = aInitPos; + oldPosRef = aInitPos; +} + +short int MotionGenerator::sign(double aVal) +{ + if (aVal < 0) + return -1; + else if (aVal > 0) + return 1; + else + { + return 0; + } +} + +void MotionGenerator::reset() +{ + // Reset all state variables + + pos = initPos; + oldPos = initPos; + oldPosRef = initPos; + vel = 0; + acc = 0; + oldVel = 0; + + dBrk = 0; + dAcc = 0; + dVel = 0; + dDec = 0; + dTot = 0; + + tBrk = 0; + tAcc = 0; + tVel = 0; + tDec = 0; + + velSt = 0; +} diff --git a/canopen_interfaces/msg/COData.msg b/canopen_interfaces/msg/COData.msg index 137df0c5..9626a455 100644 --- a/canopen_interfaces/msg/COData.msg +++ b/canopen_interfaces/msg/COData.msg @@ -1,5 +1,3 @@ uint16 index uint8 subindex uint32 data -# 8 = uint8_t, 16 = uint16_t, 32 = uint32_t -uint8 type \ No newline at end of file diff --git a/canopen_interfaces/srv/COHeartbeatID.srv b/canopen_interfaces/srv/COHeartbeatID.srv index f5432a8f..9fbbc94d 100644 --- a/canopen_interfaces/srv/COHeartbeatID.srv +++ b/canopen_interfaces/srv/COHeartbeatID.srv @@ -1,4 +1,4 @@ uint8 nodeid uint16 heartbeat #ms --- -bool success \ No newline at end of file +bool success diff --git a/canopen_interfaces/srv/CONode.srv b/canopen_interfaces/srv/CONode.srv index f12034c0..4855dd69 100644 --- a/canopen_interfaces/srv/CONode.srv +++ b/canopen_interfaces/srv/CONode.srv @@ -1,3 +1,3 @@ uint8 nodeid --- -bool success \ No newline at end of file +bool success diff --git a/canopen_interfaces/srv/CORead.srv b/canopen_interfaces/srv/CORead.srv index 969fe844..ec712e91 100644 --- a/canopen_interfaces/srv/CORead.srv +++ b/canopen_interfaces/srv/CORead.srv @@ -1,7 +1,5 @@ uint16 index uint8 subindex -# 8 = uint8_t, 16 = uint16_t, 32 = uint32_t -uint8 type --- bool success -uint32 data \ No newline at end of file +uint32 data diff --git a/canopen_interfaces/srv/COReadID.srv b/canopen_interfaces/srv/COReadID.srv index eec8379c..c5611794 100644 --- a/canopen_interfaces/srv/COReadID.srv +++ b/canopen_interfaces/srv/COReadID.srv @@ -1,8 +1,15 @@ +uint8 CANOPEN_DATATYPE_INT8 = 0x02 +uint8 CANOPEN_DATATYPE_INT16 = 0x03 +uint8 CANOPEN_DATATYPE_INT32 = 0x04 +uint8 CANOPEN_DATATYPE_UINT8 = 0x05 +uint8 CANOPEN_DATATYPE_UINT16 = 0x06 +uint8 CANOPEN_DATATYPE_UINT32 = 0x07 + uint8 nodeid uint16 index uint8 subindex # 8 = uint8_t, 16 = uint16_t, 32 = uint32_t -uint8 type +uint8 canopen_datatype --- bool success -uint32 data \ No newline at end of file +uint32 data diff --git a/canopen_interfaces/srv/COTargetDouble.srv b/canopen_interfaces/srv/COTargetDouble.srv index 42833625..c5a4ed38 100644 --- a/canopen_interfaces/srv/COTargetDouble.srv +++ b/canopen_interfaces/srv/COTargetDouble.srv @@ -1,3 +1,3 @@ float64 target --- -bool success \ No newline at end of file +bool success diff --git a/canopen_interfaces/srv/COWrite.srv b/canopen_interfaces/srv/COWrite.srv index ef96be9e..dcf4f647 100644 --- a/canopen_interfaces/srv/COWrite.srv +++ b/canopen_interfaces/srv/COWrite.srv @@ -1,7 +1,5 @@ uint16 index uint8 subindex uint32 data -# 8 = uint8_t, 16 = uint16_t, 32 = uint32_t -uint8 type --- -bool success \ No newline at end of file +bool success diff --git a/canopen_interfaces/srv/COWriteID.srv b/canopen_interfaces/srv/COWriteID.srv index 9d113fbf..622c8b6e 100644 --- a/canopen_interfaces/srv/COWriteID.srv +++ b/canopen_interfaces/srv/COWriteID.srv @@ -1,8 +1,15 @@ -uint8 nodeid +uint8 CANOPEN_DATATYPE_INT8 = 0x02 +uint8 CANOPEN_DATATYPE_INT16 = 0x03 +uint8 CANOPEN_DATATYPE_INT32 = 0x04 +uint8 CANOPEN_DATATYPE_UINT8 = 0x05 +uint8 CANOPEN_DATATYPE_UINT16 = 0x06 +uint8 CANOPEN_DATATYPE_UINT32 = 0x07 + +int8 nodeid uint16 index uint8 subindex uint32 data # 8 = uint8_t, 16 = uint16_t, 32 = uint32_t -uint8 type +uint8 canopen_datatype --- -bool success \ No newline at end of file +bool success diff --git a/canopen_master_driver/CMakeLists.txt b/canopen_master_driver/CMakeLists.txt new file mode 100644 index 00000000..9beffec6 --- /dev/null +++ b/canopen_master_driver/CMakeLists.txt @@ -0,0 +1,153 @@ +cmake_minimum_required(VERSION 3.8) +project(canopen_master_driver) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(canopen_core REQUIRED) +find_package(canopen_interfaces REQUIRED) +find_package(lely_core_libraries REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +set(dependencies + canopen_core + canopen_interfaces + lely_core_libraries + rclcpp + rclcpp_components + rclcpp_lifecycle +) + +add_library(lely_master_bridge + src/lely_master_bridge.cpp +) +target_compile_features(lely_master_bridge PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(lely_master_bridge PUBLIC -Wl,--no-undefined) +target_include_directories(lely_master_bridge PUBLIC + $ + $) + +ament_target_dependencies( + lely_master_bridge + ${dependencies} +) + + +add_library(node_canopen_basic_master + src/node_interfaces/node_canopen_basic_master.cpp +) +target_compile_features(node_canopen_basic_master PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(node_canopen_basic_master PUBLIC -Wl,--no-undefined) +target_include_directories(node_canopen_basic_master PUBLIC + $ + $) + +target_link_libraries( + node_canopen_basic_master + lely_master_bridge +) + +ament_target_dependencies( + node_canopen_basic_master + ${dependencies} +) + + + +add_library(lifecycle_master_driver + src/lifecycle_master_driver.cpp + ) +target_compile_features(lifecycle_master_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(lifecycle_master_driver PUBLIC -Wl,--no-undefined) +target_include_directories(lifecycle_master_driver PUBLIC + $ + $) +target_link_libraries( + lifecycle_master_driver + node_canopen_basic_master + lely_master_bridge +) + +ament_target_dependencies( + lifecycle_master_driver + ${dependencies} +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(lifecycle_master_driver PRIVATE "CANOPEN_MASTER_DRIVER_BUILDING_LIBRARY") + +rclcpp_components_register_nodes(lifecycle_master_driver "ros2_canopen::LifecycleMasterDriver") +set(node_plugins "${node_plugins}ros2_canopen::LifecycleMasterDriver;$\n") + + + + +add_library(master_driver + src/master_driver.cpp + ) +target_compile_features(master_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(master_driver PUBLIC -Wl,--no-undefined) +target_include_directories(master_driver PUBLIC + $ + $) +target_link_libraries( + master_driver + node_canopen_basic_master + lely_master_bridge +) + +ament_target_dependencies( + master_driver + ${dependencies} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(master_driver PRIVATE "CANOPEN_MASTER_DRIVER_BUILDING_LIBRARY") + +rclcpp_components_register_nodes(master_driver "ros2_canopen::MasterDriver") +set(node_plugins "${node_plugins}ros2_canopen::MasterDriver;$\n") + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS lifecycle_master_driver master_driver node_canopen_basic_master lely_master_bridge + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + master_driver + lifecycle_master_driver + node_canopen_basic_master + lely_master_bridge +) + +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_export_dependencies( + ${dependencies} +) + +ament_package() diff --git a/canopen_master_driver/LICENSE b/canopen_master_driver/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/canopen_master_driver/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/canopen_master_driver/include/canopen_master_driver/lely_master_bridge.hpp b/canopen_master_driver/include/canopen_master_driver/lely_master_bridge.hpp new file mode 100644 index 00000000..1c7cbcf0 --- /dev/null +++ b/canopen_master_driver/include/canopen_master_driver/lely_master_bridge.hpp @@ -0,0 +1,154 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LELY_MASTER_BRIDGE_HPP +#define LELY_MASTER_BRIDGE_HPP + +#include +#include +#include "lely/coapp/master.hpp" + +#include +#include +#include +#include +#include +#include "canopen_core/exchange.hpp" + +using namespace std::literals::chrono_literals; +namespace ros2_canopen +{ +/** + * @brief Lely Master Bridge + * + * Creates services to pass values between the CANopen executor and ROS executor. + * This is important because certain functions can only be called from within + * the thread of the CANopen executor. + * + */ +class LelyMasterBridge : public lely::canopen::AsyncMaster +{ + std::shared_ptr> + sdo_read_data_promise; ///< Pointer to Promise for read service calls. + std::shared_ptr> + sdo_write_data_promise; ///< Pointer to Promise for write service calls + std::promise nmt_promise; ///< Pointer to Promise for nmt service calls + std::mutex sdo_mutex; ///< Mutex to guard sdo objects + bool running; ///< Bool to indicate whether an sdo call is running + std::condition_variable sdo_cond; ///< Condition variable to sync service calls (one at a time) + uint8_t node_id; ///< Node id of the master + +public: + /** + * @brief Construct a new Lely Master Bridge object + * + * @param [in] exec Pointer to the executor + * @param [in] timer Pointer to the timer + * @param [in] chan Pointer to the channel + * @param [in] dcf_txt Path to the DCF file + * @param [in] dcf_bin Path to the DCF bin file + * @param [in] id CANopen node id of the master + */ + LelyMasterBridge( + ev_exec_t * exec, lely::io::TimerBase & timer, lely::io::CanChannelBase & chan, + const std::string & dcf_txt, const std::string & dcf_bin = "", uint8_t id = (uint8_t)255U) + : lely::canopen::AsyncMaster(exec, timer, chan, dcf_txt, dcf_bin, id), node_id(id) + { + } + + /** + * @brief Asynchronous call for writing to an SDO + * + * @param [in] id CANopen node id of the target device + * @param [in] data Data to write + * @param [in] datatype Datatype of the data to write + * @return std::future A future that indicates if the + * write Operation was successful. + */ + std::future async_write_sdo(uint8_t id, COData data, uint8_t datatype); + + /** + * @brief + * + * @param [in] id CANopen node id of the target device + * @param [in] data Data to read, value is disregarded + * @param [in] datatype Datatype of the data to read + * @return std::future A future that returns the read result + * once the process finished. + */ + std::future async_read_sdo(uint8_t id, COData data, uint8_t datatype); + + /** + * @brief async_write_nmt + * + * @param id + * @param command + * @return std::future + * + */ + std::future async_write_nmt(uint8_t id, uint8_t command); + + template + void submit_write_sdo(uint8_t id, uint16_t idx, uint8_t subidx, T value) + { + this->SubmitWrite( + this->GetExecutor(), id, idx, subidx, value, + [this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable + { + if (ec) + { + this->sdo_write_data_promise->set_exception( + lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload")); + } + else + { + this->sdo_write_data_promise->set_value(true); + } + std::unique_lock lck(this->sdo_mutex); + this->running = false; + this->sdo_cond.notify_one(); + }, + 20ms); + } + + template + void submit_read_sdo(uint8_t id, uint16_t idx, uint8_t subidx) + { + this->SubmitRead( + this->GetExecutor(), id, idx, subidx, + [this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable + { + if (ec) + { + this->sdo_read_data_promise->set_exception( + lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload")); + } + else + { + COData codata = {idx, subidx, 0U}; + std::memcpy(&codata.data_, &value, sizeof(T)); + this->sdo_read_data_promise->set_value(codata); + } + std::unique_lock lck(this->sdo_mutex); + this->running = false; + this->sdo_cond.notify_one(); + }, + 20ms); + } +}; + +} // namespace ros2_canopen + +#endif // LELY_MASTER_BRIDGE_HPP diff --git a/canopen_master_driver/include/canopen_master_driver/lifecycle_master_driver.hpp b/canopen_master_driver/include/canopen_master_driver/lifecycle_master_driver.hpp new file mode 100644 index 00000000..704a378d --- /dev/null +++ b/canopen_master_driver/include/canopen_master_driver/lifecycle_master_driver.hpp @@ -0,0 +1,46 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef LIFECYCLE_MASTER_DRIVER_HPP +#define LIFECYCLE_MASTER_DRIVER_HPP + +#include +#include +#include + +#include "canopen_core/master_node.hpp" +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" + +namespace ros2_canopen +{ +/** + * @brief Lifecycle Master Node + * + * This class implements the Lifecycle master interface. + * It uses the Lely Master Bridge and exposes a ROS node + * interface. + * + */ +class LifecycleMasterDriver : public ros2_canopen::LifecycleCanopenMaster +{ + std::shared_ptr> + node_canopen_basic_master_; + +public: + explicit LifecycleMasterDriver(const rclcpp::NodeOptions & node_options); +}; + +} // namespace ros2_canopen + +#endif diff --git a/canopen_master_driver/include/canopen_master_driver/master_driver.hpp b/canopen_master_driver/include/canopen_master_driver/master_driver.hpp new file mode 100644 index 00000000..80331bd9 --- /dev/null +++ b/canopen_master_driver/include/canopen_master_driver/master_driver.hpp @@ -0,0 +1,41 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef MASTER_DRIVER_HPP +#define MASTER_DRIVER_HPP + +#include "canopen_core/master_node.hpp" +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" + +namespace ros2_canopen +{ +/** + * @brief Master Node + * + * This class implements the Lifecycle master interface. + * It uses the Lely Master Bridge and exposes a ROS node + * interface. + * + */ +class MasterDriver : public ros2_canopen::CanopenMaster +{ + std::shared_ptr> node_canopen_basic_master_; + +public: + explicit MasterDriver(const rclcpp::NodeOptions & node_options); +}; + +} // namespace ros2_canopen + +#endif diff --git a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp new file mode 100644 index 00000000..e3efc998 --- /dev/null +++ b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp @@ -0,0 +1,69 @@ +#ifndef NODE_CANOPEN_BASIC_MASTER_HPP_ +#define NODE_CANOPEN_BASIC_MASTER_HPP_ + +#include "canopen_core/node_interfaces/node_canopen_master.hpp" +#include "canopen_interfaces/srv/co_read_id.hpp" +#include "canopen_interfaces/srv/co_write_id.hpp" +#include "canopen_master_driver/lely_master_bridge.hpp" + +namespace ros2_canopen +{ +namespace node_interfaces +{ +template +class NodeCanopenBasicMaster : public ros2_canopen::node_interfaces::NodeCanopenMaster +{ + static_assert( + std::is_base_of::value || + std::is_base_of::value, + "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); + +protected: + std::shared_ptr master_bridge_; + rclcpp::Service::SharedPtr sdo_read_service; + rclcpp::Service::SharedPtr sdo_write_service; + +public: + NodeCanopenBasicMaster(NODETYPE * node) + : ros2_canopen::node_interfaces::NodeCanopenMaster(node) + { + this->activated_.load(); + RCLCPP_INFO(this->node_->get_logger(), "NodeCanopenBasicMaster"); + } + + virtual void activate(bool called_from_base) override; + virtual void deactivate(bool called_from_base) override; + virtual void init(bool called_from_base) override; + + /** + * @brief Read Service Data Object + * + * This Service is only available when the node is in active lifecycle state. + * It will return with success false in any other lifecycle state and log an + * RCLCPP_ERROR. + * + * @param request + * @param response + */ + void on_sdo_read( + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Write Service Data Object + * + * This service is only available when the node is in active lifecycle state. + * It will return with success false in any other lifecycle state and log an + * RCLCPP_ERROR. + * + * @param request + * @param response + */ + void on_sdo_write( + const std::shared_ptr request, + std::shared_ptr response); +}; +} // namespace node_interfaces +} // namespace ros2_canopen + +#endif diff --git a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp new file mode 100644 index 00000000..aa00bf11 --- /dev/null +++ b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp @@ -0,0 +1,122 @@ +#ifndef NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_ +#define NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_ + +#include "canopen_core/node_interfaces/node_canopen_master.hpp" +#include "canopen_master_driver/lely_master_bridge.hpp" +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" + +using namespace ros2_canopen::node_interfaces; + +template +void NodeCanopenBasicMaster::activate(bool called_from_base) +{ + master_bridge_ = std::make_shared( + *(this->exec_), *(this->timer_), *(this->chan_), this->master_dcf_, this->master_bin_, + this->node_id_); + this->master_ = std::static_pointer_cast(master_bridge_); +} + +template +void NodeCanopenBasicMaster::deactivate(bool called_from_base) +{ + this->master_.reset(); +} + +template +void NodeCanopenBasicMaster::on_sdo_read( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (this->activated_.load()) + { + ros2_canopen::COData data = {request->index, request->subindex, 0U}; + std::future f = + this->master_bridge_->async_read_sdo(request->nodeid, data, request->canopen_datatype); + f.wait(); + try + { + response->data = f.get().data_; + response->success = true; + } + catch (std::exception & e) + { + RCLCPP_ERROR(this->node_->get_logger(), e.what()); + response->success = false; + } + } + else + { + RCLCPP_ERROR( + this->node_->get_logger(), + "LifecycleMasterNode is not in active state. SDO read service is not available."); + response->success = false; + } +} + +template +void NodeCanopenBasicMaster::on_sdo_write( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (this->activated_.load()) + { + ros2_canopen::COData data = {request->index, request->subindex, request->data}; + std::future f = + this->master_bridge_->async_write_sdo(request->nodeid, data, request->canopen_datatype); + f.wait(); + try + { + response->success = f.get(); + } + catch (std::exception & e) + { + RCLCPP_ERROR(this->node_->get_logger(), e.what()); + response->success = false; + } + } + else + { + RCLCPP_ERROR( + this->node_->get_logger(), + "LifecycleMasterNode is not in active state. SDO write service is not available."); + response->success = false; + } +} + +template <> +void NodeCanopenBasicMaster::init(bool called_from_base) +{ + // declare services + sdo_read_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_read").c_str(), + std::bind( + &ros2_canopen::node_interfaces::NodeCanopenBasicMaster::on_sdo_read, this, + std::placeholders::_1, std::placeholders::_2)); + + sdo_write_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_write").c_str(), + std::bind( + &ros2_canopen::node_interfaces::NodeCanopenBasicMaster::on_sdo_write, this, + std::placeholders::_1, std::placeholders::_2)); +} + +template <> +void NodeCanopenBasicMaster::init(bool called_from_base) +{ + // declare services + sdo_read_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_read").c_str(), + std::bind( + &ros2_canopen::node_interfaces::NodeCanopenBasicMaster< + rclcpp_lifecycle::LifecycleNode>::on_sdo_read, + this, std::placeholders::_1, std::placeholders::_2)); + + sdo_write_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_write").c_str(), + std::bind( + &ros2_canopen::node_interfaces::NodeCanopenBasicMaster< + rclcpp_lifecycle::LifecycleNode>::on_sdo_write, + this, std::placeholders::_1, std::placeholders::_2)); +} + +#endif diff --git a/canopen_master_driver/include/canopen_master_driver/visibility_control.h b/canopen_master_driver/include/canopen_master_driver/visibility_control.h new file mode 100644 index 00000000..c36e7c6e --- /dev/null +++ b/canopen_master_driver/include/canopen_master_driver/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef CANOPEN_MASTER_DRIVER__VISIBILITY_CONTROL_H_ +#define CANOPEN_MASTER_DRIVER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define CANOPEN_MASTER_DRIVER_EXPORT __attribute__((dllexport)) +#define CANOPEN_MASTER_DRIVER_IMPORT __attribute__((dllimport)) +#else +#define CANOPEN_MASTER_DRIVER_EXPORT __declspec(dllexport) +#define CANOPEN_MASTER_DRIVER_IMPORT __declspec(dllimport) +#endif +#ifdef CANOPEN_MASTER_DRIVER_BUILDING_LIBRARY +#define CANOPEN_MASTER_DRIVER_PUBLIC CANOPEN_MASTER_DRIVER_EXPORT +#else +#define CANOPEN_MASTER_DRIVER_PUBLIC CANOPEN_MASTER_DRIVER_IMPORT +#endif +#define CANOPEN_MASTER_DRIVER_PUBLIC_TYPE CANOPEN_MASTER_DRIVER_PUBLIC +#define CANOPEN_MASTER_DRIVER_LOCAL +#else +#define CANOPEN_MASTER_DRIVER_EXPORT __attribute__((visibility("default"))) +#define CANOPEN_MASTER_DRIVER_IMPORT +#if __GNUC__ >= 4 +#define CANOPEN_MASTER_DRIVER_PUBLIC __attribute__((visibility("default"))) +#define CANOPEN_MASTER_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else +#define CANOPEN_MASTER_DRIVER_PUBLIC +#define CANOPEN_MASTER_DRIVER_LOCAL +#endif +#define CANOPEN_MASTER_DRIVER_PUBLIC_TYPE +#endif + +#endif // CANOPEN_MASTER_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml new file mode 100644 index 00000000..69f425b0 --- /dev/null +++ b/canopen_master_driver/package.xml @@ -0,0 +1,25 @@ + + + + canopen_master_driver + 0.0.0 + Basic canopen master implementation + Christoph Hellmann Santos + Apache-2.0 + + ament_cmake_ros + + canopen_core + canopen_interfaces + lely_core_libraries + rclcpp + rclcpp_components + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/canopen_master_driver/src/lely_master_bridge.cpp b/canopen_master_driver/src/lely_master_bridge.cpp new file mode 100644 index 00000000..3cdb7bfb --- /dev/null +++ b/canopen_master_driver/src/lely_master_bridge.cpp @@ -0,0 +1,135 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "canopen_master_driver/lely_master_bridge.hpp" +#include +#include + +using namespace std::literals::chrono_literals; + +namespace ros2_canopen /* constant-expression */ +{ +std::future LelyMasterBridge::async_write_sdo(uint8_t id, COData data, uint8_t datatype) +{ + std::unique_lock lck(sdo_mutex); + if (running) + { + sdo_cond.wait(lck); + } + running = true; + + sdo_write_data_promise = std::make_shared>(); + try + { + switch (datatype) + { + case CO_DEFTYPE_INTEGER8: + submit_write_sdo(id, data.index_, data.subindex_, data.data_); + break; + case CO_DEFTYPE_INTEGER16: + submit_write_sdo(id, data.index_, data.subindex_, data.data_); + break; + case CO_DEFTYPE_INTEGER32: + submit_write_sdo(id, data.index_, data.subindex_, data.data_); + break; + case CO_DEFTYPE_UNSIGNED8: + submit_write_sdo(id, data.index_, data.subindex_, data.data_); + break; + case CO_DEFTYPE_UNSIGNED16: + submit_write_sdo(id, data.index_, data.subindex_, data.data_); + break; + case CO_DEFTYPE_UNSIGNED32: + submit_write_sdo(id, data.index_, data.subindex_, data.data_); + break; + default: + throw lely::canopen::SdoError( + id, data.index_, data.subindex_, lely::canopen::SdoErrc::ERROR, "Unknown datatype"); + break; + } + } + catch (...) + { + this->sdo_write_data_promise->set_exception(std::current_exception()); + this->running = false; + } + + return sdo_write_data_promise->get_future(); +} + +std::future LelyMasterBridge::async_read_sdo(uint8_t id, COData data, uint8_t datatype) +{ + std::unique_lock lck(sdo_mutex); + if (running) + { + sdo_cond.wait(lck); + } + running = true; + + sdo_read_data_promise = std::make_shared>(); + try + { + switch (datatype) + { + case CO_DEFTYPE_INTEGER8: + submit_read_sdo(id, data.index_, data.subindex_); + break; + case CO_DEFTYPE_INTEGER16: + submit_read_sdo(id, data.index_, data.subindex_); + break; + case CO_DEFTYPE_INTEGER32: + submit_read_sdo(id, data.index_, data.subindex_); + break; + case CO_DEFTYPE_UNSIGNED8: + submit_read_sdo(id, data.index_, data.subindex_); + break; + case CO_DEFTYPE_UNSIGNED16: + submit_read_sdo(id, data.index_, data.subindex_); + break; + case CO_DEFTYPE_UNSIGNED32: + submit_read_sdo(id, data.index_, data.subindex_); + break; + default: + throw lely::canopen::SdoError( + id, data.index_, data.subindex_, lely::canopen::SdoErrc::ERROR, "Unknown datatype"); + break; + } + } + catch (...) + { + this->sdo_read_data_promise->set_exception(std::current_exception()); + this->running = false; + } + return sdo_read_data_promise->get_future(); +} + +std::future LelyMasterBridge::async_write_nmt(uint8_t id, uint8_t command) +{ + lely::canopen::NmtCommand command_ = static_cast(command); + switch (command_) + { + case lely::canopen::NmtCommand::ENTER_PREOP: + case lely::canopen::NmtCommand::RESET_COMM: + case lely::canopen::NmtCommand::RESET_NODE: + case lely::canopen::NmtCommand::START: + case lely::canopen::NmtCommand::STOP: + this->Command(command_, id); + break; + default: + break; + } + + nmt_promise.set_value(true); + return nmt_promise.get_future(); +} +} // namespace ros2_canopen diff --git a/canopen_master_driver/src/lifecycle_master_driver.cpp b/canopen_master_driver/src/lifecycle_master_driver.cpp new file mode 100644 index 00000000..fb8bfcb9 --- /dev/null +++ b/canopen_master_driver/src/lifecycle_master_driver.cpp @@ -0,0 +1,17 @@ +#include "canopen_master_driver/lifecycle_master_driver.hpp" + +namespace ros2_canopen +{ + +ros2_canopen::LifecycleMasterDriver::LifecycleMasterDriver(const rclcpp::NodeOptions & node_options) +: LifecycleCanopenMaster(node_options) +{ + node_canopen_master_ = + std::make_shared>( + this); +} + +} // namespace ros2_canopen + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleMasterDriver) diff --git a/canopen_master_driver/src/master_driver.cpp b/canopen_master_driver/src/master_driver.cpp new file mode 100644 index 00000000..b047a9f4 --- /dev/null +++ b/canopen_master_driver/src/master_driver.cpp @@ -0,0 +1,18 @@ +#include "canopen_master_driver/master_driver.hpp" + +namespace ros2_canopen +{ + +ros2_canopen::MasterDriver::MasterDriver(const rclcpp::NodeOptions & node_options) +: CanopenMaster(node_options) +{ + node_canopen_basic_master_ = + std::make_shared>(this); + node_canopen_master_ = std::static_pointer_cast( + node_canopen_basic_master_); +} + +} // namespace ros2_canopen + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::MasterDriver) diff --git a/canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp b/canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp new file mode 100644 index 00000000..f972e1af --- /dev/null +++ b/canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp @@ -0,0 +1,8 @@ +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp" + +using namespace ros2_canopen::node_interfaces; + +template class ros2_canopen::node_interfaces::NodeCanopenBasicMaster; +template class ros2_canopen::node_interfaces::NodeCanopenBasicMaster< + rclcpp_lifecycle::LifecycleNode>; diff --git a/canopen_master_driver/test/CMakeLists.txt b/canopen_master_driver/test/CMakeLists.txt new file mode 100644 index 00000000..fe490c49 --- /dev/null +++ b/canopen_master_driver/test/CMakeLists.txt @@ -0,0 +1,46 @@ +ament_add_gtest(test_node_canopen_basic_driver + test_node_canopen_basic_master.cpp +) +ament_target_dependencies(test_node_canopen_basic_driver + ${dependencies} +) +target_include_directories(test_node_canopen_basic_driver PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_node_canopen_basic_driver + master_driver + node_canopen_basic_master + lely_master_bridge +) + +ament_add_gtest(test_node_canopen_basic_driver_ros +test_node_canopen_basic_master_ros.cpp +) +ament_target_dependencies(test_node_canopen_basic_driver_ros + ${dependencies} +) +target_include_directories(test_node_canopen_basic_driver_ros PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_node_canopen_basic_driver_ros + master_driver + node_canopen_basic_master + lely_master_bridge +) + +ament_add_gtest(test_master_driver_component +test_master_driver_component.cpp +) +ament_target_dependencies(test_master_driver_component + ${dependencies} +) +target_include_directories(test_master_driver_component PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_master_driver_component + master_driver + lifecycle_master_driver + lely_master_bridge +) + +FILE(COPY ${CMAKE_CURRENT_SOURCE_DIR}/master.dcf DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/canopen_master_driver/test/master.dcf b/canopen_master_driver/test/master.dcf new file mode 100644 index 00000000..4f6f975e --- /dev/null +++ b/canopen_master_driver/test/master.dcf @@ -0,0 +1,694 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=2 +NrOfTxPDO=2 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=32 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1401 +15=0x1600 +16=0x1601 +17=0x1800 +18=0x1801 +19=0x1A00 +20=0x1A01 +21=0x1F25 +22=0x1F55 +23=0x1F80 +24=0x1F81 +25=0x1F82 +26=0x1F84 +27=0x1F85 +28=0x1F86 +29=0x1F87 +30=0x1F88 +31=0x1F89 +32=0x1F8A + +[ManufacturerObjects] +SupportedObjects=12 +1=0x2000 +2=0x2001 +3=0x2200 +4=0x2201 +5=0x5800 +6=0x5801 +7=0x5A00 +8=0x5A01 +9=0x5C00 +10=0x5C01 +11=0x5E00 +12=0x5E01 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=2 +2=0x00000082 +3=0x00000083 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1401] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1401sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1401sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000183 + +[1401sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1401sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1401sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1401sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1601] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1601Value] +NrOfEntries=1 +1=0x20010120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1801] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1801sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1801sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000203 + +[1801sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1801sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1801sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1A01] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A01Value] +NrOfEntries=1 +1=0x22010120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=2 +2=0x00000005 +3=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=2 +2=0x00000360 +3=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=proxy_device_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2001] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 2 +ObjectType=0x09 + +[2001sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2001sub1] +ParameterName=proxy_device_2: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=proxy_device_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2201] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 2 +ObjectType=0x09 + +[2201sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2201sub1] +ParameterName=proxy_device_2: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5801] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5A01] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A01Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5C01] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 + +[5E01] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E01Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_master_driver/test/test_master_driver_component.cpp b/canopen_master_driver/test/test_master_driver_component.cpp new file mode 100644 index 00000000..5e5b262e --- /dev/null +++ b/canopen_master_driver/test/test_master_driver_component.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" +#include "gtest/gtest.h" +using namespace rclcpp_components; + +TEST(MasterDriverComponent, test_load_lifecycle_master_driver) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_master_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[0]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} + +TEST(MasterDriverComponent, test_load_master_driver) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_master_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[1]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} diff --git a/canopen_master_driver/test/test_node_canopen_basic_master.cpp b/canopen_master_driver/test/test_node_canopen_basic_master.cpp new file mode 100644 index 00000000..8535e419 --- /dev/null +++ b/canopen_master_driver/test/test_node_canopen_basic_master.cpp @@ -0,0 +1,52 @@ +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" +#include "gtest/gtest.h" + +class RclCppFixture +{ +public: + RclCppFixture() + { + rclcpp::init(0, nullptr); + node = new rclcpp::Node("Node"); + interface = new ros2_canopen::node_interfaces::NodeCanopenBasicMaster(node); + } + ~RclCppFixture() + { + rclcpp::shutdown(); + delete (interface); + delete (node); + } + + rclcpp::Node * node; + ros2_canopen::node_interfaces::NodeCanopenBasicMaster * interface; +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeCanopenBasicMaster, test_bad_sequence_configure) +{ + auto iface = static_cast( + g_rclcppfixture.interface); + EXPECT_ANY_THROW(iface->configure()); +} + +TEST(NodeCanopenBasicMaster, test_bad_sequence_activate) +{ + auto iface = static_cast( + g_rclcppfixture.interface); + EXPECT_ANY_THROW(iface->activate()); +} + +TEST(NodeCanopenBasicMaster, test_good_sequence) +{ + auto iface = static_cast( + g_rclcppfixture.interface); + try + { + iface->init(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(rclcpp::get_logger("test"), e.what()); + } + EXPECT_NO_THROW(iface->configure()); +} diff --git a/canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp b/canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp new file mode 100644 index 00000000..5138ca44 --- /dev/null +++ b/canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp @@ -0,0 +1,76 @@ +#include +#include +#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" +#include "gtest/gtest.h" + +TEST(NodeCanopenBasicMaster, test_good_sequence_advanced) +{ + rclcpp::init(0, nullptr); + rclcpp::Node * node = new rclcpp::Node("Node"); + auto interface = new ros2_canopen::node_interfaces::NodeCanopenBasicMaster(node); + auto exec = std::make_shared(); + exec->add_node(node->get_node_base_interface()); + std::thread spinner = std::thread([exec] { exec->spin(); }); + + auto iface = static_cast(interface); + + EXPECT_NO_THROW(iface->init()); + + rclcpp::Parameter container_name("container_name", "none"); + rclcpp::Parameter master_dcf("master_dcf", "master.dcf"); + rclcpp::Parameter master_bin("master_bin", ""); + rclcpp::Parameter can_interface_name("can_interface_name", "vcan0"); + rclcpp::Parameter node_id("node_id", 1); + rclcpp::Parameter timeout("non_transmit_timeout", 100); + rclcpp::Parameter config("config", ""); + node->set_parameter(container_name); + node->set_parameter(master_dcf); + node->set_parameter(master_bin); + node->set_parameter(can_interface_name); + node->set_parameter(node_id); + node->set_parameter(timeout); + node->set_parameter(config); + EXPECT_NO_THROW(iface->configure()); + rclcpp::shutdown(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (spinner.joinable()) + { + spinner.join(); + } +} + +TEST(NodeCanopenBasicLifecycleMaster, test_good_sequence_advanced) +{ + rclcpp::init(0, nullptr); + rclcpp_lifecycle::LifecycleNode * node = new rclcpp_lifecycle::LifecycleNode("Node"); + auto interface = new ros2_canopen::node_interfaces::NodeCanopenBasicMaster(node); + auto exec = std::make_shared(); + exec->add_node(node->get_node_base_interface()); + std::thread spinner = std::thread([exec] { exec->spin(); }); + + auto iface = static_cast(interface); + + EXPECT_NO_THROW(iface->init()); + + rclcpp::Parameter container_name("container_name", "none"); + rclcpp::Parameter master_dcf("master_dcf", "master.dcf"); + rclcpp::Parameter master_bin("master_bin", ""); + rclcpp::Parameter can_interface_name("can_interface_name", "vcan0"); + rclcpp::Parameter node_id("node_id", 1); + rclcpp::Parameter timeout("non_transmit_timeout", 100); + rclcpp::Parameter config("config", ""); + node->set_parameter(container_name); + node->set_parameter(master_dcf); + node->set_parameter(master_bin); + node->set_parameter(can_interface_name); + node->set_parameter(node_id); + node->set_parameter(timeout); + node->set_parameter(config); + EXPECT_NO_THROW(iface->configure()); + rclcpp::shutdown(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (spinner.joinable()) + { + spinner.join(); + } +} diff --git a/canopen_mock_slave/include/canopen_mock_slave/base_slave.hpp b/canopen_mock_slave/include/canopen_mock_slave/base_slave.hpp deleted file mode 100644 index be265649..00000000 --- a/canopen_mock_slave/include/canopen_mock_slave/base_slave.hpp +++ /dev/null @@ -1,73 +0,0 @@ - -#ifndef SLAVE_HPP -#define SLAVE_HPP -#include -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -namespace ros2_canopen -{ - class BaseSlave : public rclcpp_lifecycle::LifecycleNode - { - public: - explicit BaseSlave(const std::string &node_name, bool intra_process_comms = false) - : rclcpp_lifecycle::LifecycleNode(node_name, - rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) - { - this->declare_parameter("node_id", 2); - this->declare_parameter("slave_config", "slave.eds"); - this->declare_parameter("can_interface_name", "vcan0"); - this->activated.store(false); - } - - virtual void run() = 0; - - protected: - std::thread run_thread; - int node_id_; - std::string slave_config_; - std::string can_interface_name_; - std::atomic activated; - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) - { - this->activated.store(false); - RCLCPP_INFO(this->get_logger(), "Reaching inactive state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &) - { - this->activated.store(true); - get_parameter("node_id", node_id_); - get_parameter("slave_config", slave_config_); - get_parameter("can_interface_name", can_interface_name_); - run_thread = std::thread(std::bind(&BaseSlave::run, this)); - RCLCPP_INFO(this->get_logger(), "Reaching active state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &) - { - this->activated.store(false); - RCLCPP_INFO(this->get_logger(), "Reaching inactive state."); - if(run_thread.joinable()) - { - run_thread.join(); - } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) - { - this->activated.store(false); - RCLCPP_INFO(this->get_logger(), "Reaching unconfigured state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - }; -} - -#endif \ No newline at end of file diff --git a/canopen_mock_slave/include/canopen_mock_slave/basic_slave.hpp b/canopen_mock_slave/include/canopen_mock_slave/basic_slave.hpp deleted file mode 100644 index 3f72e71b..00000000 --- a/canopen_mock_slave/include/canopen_mock_slave/basic_slave.hpp +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef BASIC_SLAVE_HPP -#define BASIC_SLAVE_HPP -#include -#include -#include -#include -#include -#include -#include -#include - - -#include - -#include "lifecycle_msgs/msg/state.hpp" -#include "canopen_mock_slave/base_slave.hpp" - -using namespace lely; -using namespace std::chrono_literals; -namespace ros2_canopen -{ - class BasicSlave : public BaseSlave - { - public: - explicit BasicSlave(const std::string &node_name, bool intra_process_comms = false) : BaseSlave(node_name, intra_process_comms) - { - } - - protected: - class ActiveCheckTask : public ev::CoTask - { - public: - ActiveCheckTask(io::Context* ctx, ev::Executor* exec, BasicSlave* slave) : CoTask(*exec) - { - slave_ = slave; - exec_ = exec; - ctx_ = ctx; - - } - - protected: - BasicSlave* slave_; - ev::Executor *exec_; - io::Context *ctx_; - virtual void operator() () noexcept - { - if(slave_->activated.load()) - { - - } - ctx_->shutdown(); - } - }; - - void run() override - { - io::IoGuard io_guard; - io::Context ctx; - io::Poll poll(ctx); - ev::Loop loop(poll.get_poll()); - auto exec = loop.get_executor(); - io::Timer timer(poll, exec, CLOCK_MONOTONIC); - io::CanController ctrl(can_interface_name_.c_str()); - io::CanChannel chan(poll, exec); - chan.open(ctrl); - canopen::BasicSlave slave(timer, chan, slave_config_.c_str(), "", node_id_); - slave.Reset(); - ActiveCheckTask checktask(&ctx, &exec, this); - //exec.post(checktask); - RCLCPP_INFO(this->get_logger(), "Created slave for node_id %i.", node_id_); - loop.run(); - ctx.shutdown(); - RCLCPP_INFO(this->get_logger(), "Stopped CANopen Event Loop."); - } - }; -} - -#endif \ No newline at end of file diff --git a/canopen_mock_slave/include/canopen_mock_slave/cia402_slave.hpp b/canopen_mock_slave/include/canopen_mock_slave/cia402_slave.hpp deleted file mode 100644 index d90705fe..00000000 --- a/canopen_mock_slave/include/canopen_mock_slave/cia402_slave.hpp +++ /dev/null @@ -1,564 +0,0 @@ -#ifndef CIA402_SLAVE_HPP -#define CIA402_SLAVE_HPP -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "lifecycle_msgs/msg/state.hpp" -#include "canopen_mock_slave/base_slave.hpp" - -using namespace lely; -using namespace std::chrono_literals; - -namespace ros2_canopen -{ - - class CIA402MockSlave : public canopen::BasicSlave - { - public: - explicit CIA402MockSlave(io::TimerBase &timer, io::CanChannelBase &chan, - const ::std::string &dcf_txt, - const ::std::string &dcf_bin = "", uint8_t id = 0xff) : canopen::BasicSlave(timer, chan, dcf_txt, dcf_bin, id) - { - state.store(InternalState::Not_Ready_To_Switch_On); - status_word = 0x0; - operation_mode.store(No_Mode); - control_cycle_period = 0.01; - actual_position = 0.0; - } - - protected: - enum InternalState - { - Unknown = 0, - Start = 0, - Not_Ready_To_Switch_On = 1, - Switch_On_Disabled = 2, - Ready_To_Switch_On = 3, - Switched_On = 4, - Operation_Enable = 5, - Quick_Stop_Active = 6, - Fault_Reaction_Active = 7, - Fault = 8, - }; - - enum StatusWord - { - SW_Ready_To_Switch_On = 0, - SW_Switched_On = 1, - SW_Operation_enabled = 2, - SW_Fault = 3, - SW_Voltage_enabled = 4, - SW_Quick_stop = 5, - SW_Switch_on_disabled = 6, - SW_Warning = 7, - SW_Manufacturer_specific0 = 8, - SW_Remote = 9, - SW_Target_reached = 10, - SW_Internal_limit = 11, - SW_Operation_mode_specific0 = 12, - SW_Operation_mode_specific1 = 13, - SW_Manufacturer_specific1 = 14, - SW_Manufacturer_specific2 = 15 - }; - enum ControlWord - { - CW_Switch_On = 0, - CW_Enable_Voltage = 1, - CW_Quick_Stop = 2, - CW_Enable_Operation = 3, - CW_Operation_mode_specific0 = 4, - CW_Operation_mode_specific1 = 5, - CW_Operation_mode_specific2 = 6, - CW_Fault_Reset = 7, - CW_Halt = 8, - CW_Operation_mode_specific3 = 9, - // CW_Reserved1=10, - CW_Manufacturer_specific0 = 11, - CW_Manufacturer_specific1 = 12, - CW_Manufacturer_specific2 = 13, - CW_Manufacturer_specific3 = 14, - CW_Manufacturer_specific4 = 15, - }; - - enum OperationMode - { - No_Mode = 0, - Profiled_Position = 1, - Velocity = 2, - Profiled_Velocity = 3, - Profiled_Torque = 4, - Reserved = 5, - Homing = 6, - Interpolated_Position = 7, - Cyclic_Synchronous_Position = 8, - Cyclic_Synchronous_Velocity = 9, - Cyclic_Synchronous_Torque = 10, - }; - std::atomic is_relative; - std::atomic is_running; - std::atomic is_halt; - std::atomic is_new_set_point; - std::atomic operation_mode; - std::atomic old_operation_mode; - - std::mutex w_mutex; - uint16_t status_word; - uint16_t control_word; - std::atomic state; - - std::thread profiled_position_mode; - std::thread profiled_velocity_mode; - std::thread cyclic_position_mode; - std::thread cyclic_velocity_mode; - - double cycle_time; - - std::mutex in_mode_mutex; - double actual_position; - double actual_speed; - double acceleration; - double control_cycle_period; - - void run_cyclic_position_mode() - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_cyclic_position_mode"); - int32_t min_pos = (int32_t)(*this)[0x607D][1]; - int32_t max_pos = (int32_t)(*this)[0x607D][2]; - uint8_t int_period = (*this)[0x60C2][1]; - int32_t offset = (*this)[0x60B0][0]; - int8_t index = (*this)[0x60C2][2]; - - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"),"Lower Software Limit: %d", min_pos); - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"),"Upper Software Limit: %d", max_pos); - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"),"Control Cycle: %hhu + 10^%hhd", int_period, index); - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"),"Offset: %d", offset); - - double cp_min_position = min_pos / 1000; - double cp_max_position = max_pos / 1000; - double cp_interpolation_period = int_period * std::pow(10.0, index); - double cp_offset = (double)(offset / 1000.0); - int ccp_millis = (int)(control_cycle_period * std::pow(10.0, 3)); - int32_t act_pos; - while ( - (state.load() == InternalState::Operation_Enable) && - (operation_mode.load() == Cyclic_Synchronous_Position) && - (rclcpp::ok())) - { - act_pos = (*this)[0x607A][0]; - double target_position = (act_pos) / 1000 - cp_offset; // m - double position_delta = target_position - actual_position; // m - double speed = position_delta / cp_interpolation_period; // m/s - double increment = control_cycle_period * speed; // m - (*this)[0x606C][0] = (int32_t)speed * 1000; - if ( - (target_position < cp_max_position) && - (target_position > cp_min_position) && - (std::abs(position_delta) > 0.001)) - { - while ( - (std::abs(actual_position - target_position) > 0.001) && - (rclcpp::ok())) - { - std::this_thread::sleep_for(std::chrono::milliseconds(ccp_millis)); - actual_position += increment; - (*this)[0x6064][0] = (int32_t)actual_position*1000; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(ccp_millis)); - } - } - - void set_new_status_word_and_state() - { - - switch (state.load()) - { - case InternalState::Not_Ready_To_Switch_On: - on_not_ready_to_switch_on(); - break; - case InternalState::Switch_On_Disabled: - on_switch_on_disabled(); - break; - case InternalState::Ready_To_Switch_On: - on_ready_to_switch_on(); - break; - case InternalState::Switched_On: - on_switched_on(); - break; - case InternalState::Operation_Enable: - on_operation_enabled(); - break; - case InternalState::Quick_Stop_Active: - on_quickstop_active(); - break; - case InternalState::Fault_Reaction_Active: - break; - case InternalState::Fault: - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Fault"); - break; - default: - break; - } - } - - void set_status_bit(int bit) - { - std::scoped_lock lock(w_mutex); - status_word |= 1UL << bit; - } - - void clear_status_bit(int bit) - { - std::scoped_lock lock(w_mutex); - status_word &= ~(1UL << bit); - } - - void set_switch_on_disabled() - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switch_On_Disabled"); - state.store(InternalState::Switch_On_Disabled); - clear_status_bit(SW_Ready_To_Switch_On); - clear_status_bit(SW_Switched_On); - clear_status_bit(SW_Operation_enabled); - clear_status_bit(SW_Fault); - set_status_bit(SW_Switch_on_disabled); - } - - void set_ready_to_switch_on() - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Ready_To_Switch_On"); - state.store(InternalState::Ready_To_Switch_On); - set_status_bit(SW_Ready_To_Switch_On); - clear_status_bit(SW_Switched_On); - clear_status_bit(SW_Operation_enabled); - clear_status_bit(SW_Fault); - set_status_bit(SW_Quick_stop); - clear_status_bit(SW_Switch_on_disabled); - } - - void set_switch_on() - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switched_On"); - state.store(InternalState::Switched_On); - set_status_bit(SW_Ready_To_Switch_On); - set_status_bit(SW_Switched_On); - clear_status_bit(SW_Operation_enabled); - clear_status_bit(SW_Fault); - set_status_bit(SW_Quick_stop); - clear_status_bit(SW_Switch_on_disabled); - } - - void set_operation_enabled() - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Operation_Enable"); - state.store(InternalState::Operation_Enable); - set_status_bit(SW_Ready_To_Switch_On); - set_status_bit(SW_Switched_On); - set_status_bit(SW_Operation_enabled); - clear_status_bit(SW_Fault); - set_status_bit(SW_Quick_stop); - clear_status_bit(SW_Switch_on_disabled); - } - - void set_quick_stop() - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Quick_Stop_Active"); - state.store(InternalState::Quick_Stop_Active); - set_status_bit(SW_Ready_To_Switch_On); - set_status_bit(SW_Switched_On); - set_status_bit(SW_Operation_enabled); - clear_status_bit(SW_Fault); - clear_status_bit(SW_Quick_stop); - clear_status_bit(SW_Switch_on_disabled); - } - - void on_not_ready_to_switch_on() - { - set_switch_on_disabled(); - } - - void on_switch_on_disabled() - { - if (is_shutdown()) - { - set_ready_to_switch_on(); - } - } - - void on_ready_to_switch_on() - { - if (is_disable_voltage()) - { - set_switch_on_disabled(); - } - if (is_switch_on()) - { - set_switch_on(); - } - if (is_faul_reset()) - { - return; - } - } - - void on_switched_on() - { - if (is_disable_voltage()) - { - set_switch_on_disabled(); - } - if (is_shutdown()) - { - set_ready_to_switch_on(); - } - if (is_enable_operation()) - { - set_operation_enabled(); - } - } - - void on_operation_enabled() - { - if (is_disable_voltage()) - { - set_switch_on_disabled(); - } - if (is_shutdown()) - { - set_ready_to_switch_on(); - } - if (is_switch_on()) - { - set_switch_on(); - } - if (is_quickstop()) - { - set_quick_stop(); - } - { - std::scoped_lock lock(w_mutex); - is_relative.store(((control_word >> 6) & 1U) == 1U); - is_halt.store(((control_word >> 8) & 1U) == 1U); - is_new_set_point.store(((control_word >> 4) & 1U) == 1U); - } - - if (old_operation_mode.load() != operation_mode.load()) - { - old_operation_mode.store(operation_mode.load()); - switch (operation_mode.load()) - { - case Cyclic_Synchronous_Position: - start_sync_pos_mode(); - break; - default: - break; - } - } - } - - void start_sync_pos_mode() - { - - cyclic_position_mode = std::thread(std::bind(&CIA402MockSlave::run_cyclic_position_mode, this)); - } - - void on_quickstop_active() - { - if (is_enable_operation()) - { - set_operation_enabled(); - } - if (is_disable_voltage()) - { - set_switch_on_disabled(); - } - } - - bool is_shutdown() - { - std::scoped_lock lock(w_mutex); - bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; - bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; - bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; - bool so_unset = ((control_word >> CW_Switch_On) & 1U) == 0U; - - if (fr_unset && qs_set && ev_set && so_unset) - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Shutdown."); - return true; - } - return false; - } - - bool is_disable_voltage() - { - std::scoped_lock lock(w_mutex); - bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; - bool ev_unset = ((control_word >> CW_Enable_Voltage) & 1U) == 0U; - - if (fr_unset && ev_unset) - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Disable Voltage."); - return true; - } - return false; - } - - bool is_switch_on() - { - std::scoped_lock lock(w_mutex); - bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; - bool eo_unset = ((control_word >> CW_Enable_Operation) & 1U) == 0U; - bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; - bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; - bool so_set = ((control_word >> CW_Switch_On) & 1U) == 1U; - if ( - fr_unset && - eo_unset && - qs_set && - ev_set && - so_set) - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Switch On."); - return true; - } - return false; - } - - bool is_enable_operation() - { - std::scoped_lock lock(w_mutex); - bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; - bool eo_set = ((control_word >> CW_Enable_Operation) & 1U) == 1U; - bool qs_set = ((control_word >> CW_Quick_Stop) & 1U) == 1U; - bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; - bool so_set = ((control_word >> CW_Switch_On) & 1U) == 1U; - if ( - fr_unset && - eo_set && - qs_set && - ev_set && - so_set) - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Enable Operation."); - return true; - } - return false; - } - - bool is_quickstop() - { - std::scoped_lock lock(w_mutex); - bool fr_unset = ((control_word >> CW_Fault_Reset) & 1U) == 0U; - bool qs_unset = ((control_word >> CW_Quick_Stop) & 1U) == 0U; - bool ev_set = ((control_word >> CW_Enable_Voltage) & 1U) == 1U; - if ( - fr_unset && - qs_unset && - ev_set) - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Quick Stop."); - return true; - } - return false; - } - - bool is_faul_reset() - { - std::scoped_lock lock(w_mutex); - bool fr_set = ((control_word >> CW_Fault_Reset) & 1U) == 1U; - if (fr_set) - { - RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Received Fault Reset."); - return true; - } - return false; - } - // This function gets called every time a value is written to the local object - // dictionary by an SDO or RPDO. - void - OnWrite(uint16_t idx, uint8_t subidx) noexcept override - { - // System State - if (idx == 0x6040 && subidx == 0) - { - { - std::scoped_lock lock(w_mutex); - control_word = (*this)[0x6040][0]; - } - set_new_status_word_and_state(); - { - std::scoped_lock lock(w_mutex); - (*this)[0x6041][0] = status_word; - this->TpdoEvent(1); - } - } - // Operation Mode - if (idx == 0x6060 && subidx == 0) - { - int8_t mode = (*this)[0x6060][0]; - switch (mode) - { - case No_Mode: - case Profiled_Position: - case Velocity: - case Profiled_Velocity: - case Profiled_Torque: - case Reserved: - case Homing: - case Interpolated_Position: - case Cyclic_Synchronous_Position: - case Cyclic_Synchronous_Velocity: - case Cyclic_Synchronous_Torque: - operation_mode.store(mode); - break; - default: - std::cout << "Error: Master tried to set unkown operation mode." << std::endl; - } - // RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Switched to mode %hhi.", mode); - (*this)[0x6061][0] = (int8_t)(mode); - this->TpdoEvent(1); - } - } - }; - - class CIA402Slave : public BaseSlave - { - public: - explicit CIA402Slave(const std::string &node_name, bool intra_process_comms = false) : BaseSlave(node_name, intra_process_comms) - { - } - - void run() override - { - io::IoGuard io_guard; - io::Context ctx; - io::Poll poll(ctx); - ev::Loop loop(poll.get_poll()); - auto exec = loop.get_executor(); - io::Timer timer(poll, exec, CLOCK_MONOTONIC); - io::CanController ctrl(can_interface_name_.c_str()); - io::CanChannel chan(poll, exec); - chan.open(ctrl); - ros2_canopen::CIA402MockSlave slave(timer, chan, slave_config_.c_str(), "", node_id_); - slave.Reset(); - - RCLCPP_INFO(this->get_logger(), "Created cia402 slave for node_id %i.", node_id_); - - loop.run(); - ctx.shutdown(); - RCLCPP_INFO(this->get_logger(), "Stopped CANopen Event Loop."); - } - }; -} -#endif \ No newline at end of file diff --git a/canopen_mock_slave/launch/basic_slave.launch.py b/canopen_mock_slave/launch/basic_slave.launch.py deleted file mode 100644 index aa4afb6f..00000000 --- a/canopen_mock_slave/launch/basic_slave.launch.py +++ /dev/null @@ -1,94 +0,0 @@ - -import os -import sys - -from sympy import true -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) # noqa -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'launch')) # noqa - -import launch -import launch.actions -import launch.events -from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution -from launch.actions import DeclareLaunchArgument - -import launch_ros -import launch_ros.events -import launch_ros.events.lifecycle - -import lifecycle_msgs.msg - - -def generate_launch_description(): - path_to_test = os.path.dirname(__file__) - - node_id_arg = DeclareLaunchArgument( - 'node_id', default_value=TextSubstitution(text='2'), description="CANopen node id the mock slave shall have." - ) - - slave_config_arg = DeclareLaunchArgument( - 'slave_config', - default_value=TextSubstitution(text=os.path.join(path_to_test, ".." , "config" , "simple_slave.eds")), - description="Path to eds file to be used for the slave." - ) - - can_interface_arg = DeclareLaunchArgument( - 'can_interface_name', - default_value=TextSubstitution(text="vcan0"), - description="CAN interface to be used by mock slave." - ) - - node_name_arg = DeclareLaunchArgument( - 'node_name', - default_value=TextSubstitution(text="basic_slave_node"), - description="Name of the node." - ) - - slave_node = launch_ros.actions.LifecycleNode( - name=LaunchConfiguration("node_name"), - namespace="", - package="canopen_mock_slave", - output="screen", - executable="basic_slave_node", - parameters=[ - { - "slave_config": LaunchConfiguration("slave_config"), - "node_id": LaunchConfiguration("node_id"), - "can_interface_name": LaunchConfiguration("can_interface_name") - } - ], - ) - slave_inactive_state_handler = launch.actions.RegisterEventHandler( - launch_ros.event_handlers.OnStateTransition( - target_lifecycle_node=slave_node, - goal_state='inactive', - handle_once=true, - entities=[ - launch.actions.LogInfo( - msg="node 'basic_slave_node' reached the 'inactive' state, 'activating'."), - launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action( - slave_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - )), - ], - - ), - - ) - slave_configure = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action( - slave_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - ld = launch.LaunchDescription() - ld.add_action(node_id_arg) - ld.add_action(slave_config_arg) - ld.add_action(can_interface_arg) - ld.add_action(node_name_arg) - ld.add_action(slave_inactive_state_handler) - ld.add_action(slave_node) - ld.add_action(slave_configure) - return ld \ No newline at end of file diff --git a/canopen_mock_slave/launch/cia402_slave.launch.py b/canopen_mock_slave/launch/cia402_slave.launch.py deleted file mode 100644 index 61d256c2..00000000 --- a/canopen_mock_slave/launch/cia402_slave.launch.py +++ /dev/null @@ -1,94 +0,0 @@ - -import os -import sys - -from sympy import true -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) # noqa -sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'launch')) # noqa - -import launch -import launch.actions -import launch.events -from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution -from launch.actions import DeclareLaunchArgument - -import launch_ros -import launch_ros.events -import launch_ros.events.lifecycle - -import lifecycle_msgs.msg - - -def generate_launch_description(): - path_to_test = os.path.dirname(__file__) - - node_id_arg = DeclareLaunchArgument( - 'node_id', default_value=TextSubstitution(text='2'), description="CANopen node id the mock slave shall have." - ) - - slave_config_arg = DeclareLaunchArgument( - 'slave_config', - default_value=TextSubstitution(text=os.path.join(path_to_test, ".." , "config" , "cia402_slave.eds")), - description="Path to eds file to be used for the slave." - ) - - can_interface_arg = DeclareLaunchArgument( - 'can_interface_name', - default_value=TextSubstitution(text="vcan0"), - description="CAN interface to be used by mock slave." - ) - - node_name_arg = DeclareLaunchArgument( - 'node_name', - default_value=TextSubstitution(text="basic_slave_node"), - description="Name of the node." - ) - - slave_node = launch_ros.actions.LifecycleNode( - name=LaunchConfiguration("node_name"), - namespace="", - package="canopen_mock_slave", - output="screen", - executable="cia402_slave_node", - parameters=[ - { - "slave_config": LaunchConfiguration("slave_config"), - "node_id": LaunchConfiguration("node_id"), - "can_interface_name": LaunchConfiguration("can_interface_name") - } - ], - ) - slave_inactive_state_handler = launch.actions.RegisterEventHandler( - launch_ros.event_handlers.OnStateTransition( - target_lifecycle_node=slave_node, - goal_state='inactive', - handle_once=true, - entities=[ - launch.actions.LogInfo( - msg="node 'basic_slave_node' reached the 'inactive' state, 'activating'."), - launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action( - slave_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - )), - ], - - ), - - ) - slave_configure = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action( - slave_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - ld = launch.LaunchDescription() - ld.add_action(node_id_arg) - ld.add_action(slave_config_arg) - ld.add_action(can_interface_arg) - ld.add_action(node_name_arg) - ld.add_action(slave_inactive_state_handler) - ld.add_action(slave_node) - ld.add_action(slave_configure) - return ld \ No newline at end of file diff --git a/canopen_mock_slave/src/basic_slave.cpp b/canopen_mock_slave/src/basic_slave.cpp deleted file mode 100644 index 070e905a..00000000 --- a/canopen_mock_slave/src/basic_slave.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "canopen_mock_slave/basic_slave.hpp" - - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto canopen_slave = std::make_shared("basic_slave"); - executor.add_node(canopen_slave->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/canopen_mock_slave/src/cia402_slave.cpp b/canopen_mock_slave/src/cia402_slave.cpp deleted file mode 100644 index 1811df41..00000000 --- a/canopen_mock_slave/src/cia402_slave.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "canopen_mock_slave/cia402_slave.hpp" - - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto canopen_slave = std::make_shared("cia402_slave"); - executor.add_node(canopen_slave->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/canopen_mock_slave/src/slave.cpp b/canopen_mock_slave/src/slave.cpp deleted file mode 100644 index c1c68a35..00000000 --- a/canopen_mock_slave/src/slave.cpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/state.hpp" - -using namespace lely; -using namespace std::chrono_literals; - -class RPDOTestSlave : public canopen::BasicSlave -{ -public: - using BasicSlave::BasicSlave; - uint32_t counter = 0; - - void - countup_task() - { - (*this)[0x4001][0] = counter; - this->TpdoEvent(1); - counter++; - this->SubmitWait(10ms, nullptr, std::bind(&RPDOTestSlave::countup_task, this)); - } -}; - -class ROS2CANopenTestNode : public rclcpp_lifecycle::LifecycleNode -{ - uint8_t id; - std::string eds; - std::string ifname; - std::string test; - std::atomic active; - std::future slave_done; - std::mutex a; - std::thread t; - - -public: - explicit ROS2CANopenTestNode(const std::string &node_name, bool intra_process_comms = false) - : rclcpp_lifecycle::LifecycleNode(node_name, - rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) - { - this->declare_parameter("slave_id", 2); - this->declare_parameter("eds", "/home/christoph/ws_ros2/src/ros2_canopen/canopen_core/ressources/simple.eds"); - this->declare_parameter("can_ifname", "vcan0"); - this->declare_parameter("test", "simple"); - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) - { - active.store(false); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &state) - { - get_parameter("slave_id", id); - get_parameter("eds", eds); - get_parameter("can_ifname", ifname); - get_parameter("test", test); - - active.store(true); - - if(test.compare("simple") == 0){ - t = std::thread(std::bind(&ROS2CANopenTestNode::run_simple, this)); - } - else if(test.compare("pdo_counter") == 0){ - t = std::thread(std::bind(&ROS2CANopenTestNode::run_pdo_counter, this)); - } - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &state) - { - active.store(false); - t.join(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) - { - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &state) - { - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - -private: - void run_simple() - { - io::IoGuard io_guard; - io::Context ctx; - io::Poll poll(ctx); - ev::Loop loop(poll.get_poll()); - auto exec = loop.get_executor(); - io::Timer timer(poll, exec, CLOCK_MONOTONIC); - io::CanController ctrl(ifname.c_str()); - io::CanChannel chan(poll, exec); - chan.open(ctrl); - - canopen::BasicSlave slave(timer, chan, eds.c_str(), "", id); - slave.Reset(); - while(active.load()){ - - loop.run_one_for(10ms); - } - ctx.shutdown(); - } - void run_pdo_counter() - { - io::IoGuard io_guard; - io::Context ctx; - io::Poll poll(ctx); - ev::Loop loop(poll.get_poll()); - auto exec = loop.get_executor(); - io::Timer timer(poll, exec, CLOCK_MONOTONIC); - io::CanController ctrl(ifname.c_str()); - io::CanChannel chan(poll, exec); - chan.open(ctrl); - - RPDOTestSlave slave(timer, chan, eds.c_str(), "", id); - //slave.submit_counter(); - ev::Task task(std::bind(&RPDOTestSlave::countup_task, &slave)); - exec.post(task); - slave.Reset(); - while(active.load()){ - - loop.run_one_for(10ms); - } - ctx.shutdown(); - } -}; - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto canopen_slave = std::make_shared("canopen_test_slave"); - executor.add_node(canopen_slave->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/canopen_proxy_driver/CMakeLists.txt b/canopen_proxy_driver/CMakeLists.txt index 388a98fc..b0b42e30 100644 --- a/canopen_proxy_driver/CMakeLists.txt +++ b/canopen_proxy_driver/CMakeLists.txt @@ -2,79 +2,98 @@ cmake_minimum_required(VERSION 3.8) project(canopen_proxy_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) + add_compile_options(-Wall -Wpedantic -Wextra -Wno-unused-parameter) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) +find_package(canopen_base_driver REQUIRED) +find_package(canopen_core REQUIRED) +find_package(canopen_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(canopen_base_driver REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -set(node_plugins "") +set(dependencies + canopen_base_driver + canopen_core + canopen_interfaces + rclcpp + rclcpp_components + rclcpp_lifecycle + std_msgs + std_srvs +) + -add_library(lifecycle_canopen_proxy_driver SHARED - src/lifecycle_canopen_proxy_driver.cpp + + +add_library(node_canopen_proxy_driver + src/node_interfaces/node_canopen_proxy_driver.cpp ) -target_compile_features(lifecycle_canopen_proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_compile_options(lifecycle_canopen_proxy_driver - PUBLIC -fPIC -Wl,--no-undefined) -target_include_directories(lifecycle_canopen_proxy_driver PUBLIC +target_compile_features(node_canopen_proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(node_canopen_proxy_driver PUBLIC -Wl,--no-undefined) +target_include_directories(node_canopen_proxy_driver PUBLIC $ $) + ament_target_dependencies( - lifecycle_canopen_proxy_driver - rclcpp - rclcpp_components - canopen_interfaces - std_msgs - std_srvs - lely_core_libraries - canopen_core - canopen_base_driver - rclcpp_lifecycle + node_canopen_proxy_driver + ${dependencies} ) +add_library(lifecycle_proxy_driver + src/lifecycle_proxy_driver.cpp + ) +target_compile_features(lifecycle_proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(lifecycle_proxy_driver PUBLIC -Wl,--no-undefined) +target_include_directories(lifecycle_proxy_driver PUBLIC + $ + $) + +target_link_libraries(lifecycle_proxy_driver + node_canopen_proxy_driver +) +ament_target_dependencies( + lifecycle_proxy_driver + ${dependencies} +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(lifecycle_canopen_proxy_driver PRIVATE "CANOPEN_PROXY_DRIVER_BUILDING_LIBRARY") +target_compile_definitions(lifecycle_proxy_driver PRIVATE "CANOPEN_PROXY_DRIVER_BUILDING_LIBRARY") -rclcpp_components_register_nodes(lifecycle_canopen_proxy_driver "ros2_canopen::LifecycleProxyDriver") -set(node_plugins "${node_plugins}ros2_canopen::LifecycleProxyDriver;$\n") +rclcpp_components_register_nodes(lifecycle_proxy_driver "ros2_canopen::LifecycleProxyDriver") +set(node_plugins "${node_plugins}ros2_canopen::LifecycleProxyDriver;$\n") -add_library(canopen_proxy_driver SHARED - src/canopen_proxy_driver.cpp -) -target_compile_features(canopen_proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_compile_options(canopen_proxy_driver - PUBLIC -fPIC -Wl,--no-undefined) -target_include_directories(canopen_proxy_driver PUBLIC + + +add_library(proxy_driver + src/proxy_driver.cpp + ) +target_compile_features(proxy_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(proxy_driver PUBLIC -Wl,--no-undefined) +target_include_directories(proxy_driver PUBLIC $ $) +target_link_libraries(proxy_driver + node_canopen_proxy_driver +) + ament_target_dependencies( - canopen_proxy_driver - rclcpp - rclcpp_components - canopen_interfaces - std_msgs - std_srvs - lely_core_libraries - canopen_core - canopen_base_driver + proxy_driver + ${dependencies} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(canopen_proxy_driver PRIVATE "CANOPEN_PROXY_DRIVER_BUILDING_LIBRARY") - -rclcpp_components_register_nodes(canopen_proxy_driver "ros2_canopen::ProxyDriver") -set(node_plugins "${node_plugins}ros2_canopen::ProxyDriver;$\n") +target_compile_definitions(proxy_driver PRIVATE "CANOPEN_proxy_DRIVER_BUILDING_LIBRARY") +rclcpp_components_register_nodes(proxy_driver "ros2_canopen::ProxyDriver") +set(node_plugins "${node_plugins}ros2_canopen::ProxyDriver;$\n") install( DIRECTORY include/ @@ -82,39 +101,31 @@ install( ) install( - TARGETS canopen_proxy_driver - EXPORT export_canopen_proxy_driver - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - TARGETS lifecycle_canopen_proxy_driver - EXPORT export_lifecycle_canopen_proxy_driver + TARGETS lifecycle_proxy_driver proxy_driver node_canopen_proxy_driver + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) if(BUILD_TESTING) - + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) endif() ament_export_include_directories( include ) ament_export_libraries( - canopen_proxy_driver - lifecycle_canopen_proxy_driver + lifecycle_proxy_driver + proxy_driver + node_canopen_proxy_driver ) ament_export_targets( - export_canopen_proxy_driver - export_lifecycle_canopen_proxy_driver + export_${PROJECT_NAME} ) - ament_export_dependencies( - canopen_base_driver + ${dependencies} ) ament_package() diff --git a/canopen_proxy_driver/config/concurrency_test/master.dcf b/canopen_proxy_driver/config/concurrency_test/master.dcf new file mode 100644 index 00000000..9633245e --- /dev/null +++ b/canopen_proxy_driver/config/concurrency_test/master.dcf @@ -0,0 +1,1446 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=6 +NrOfTxPDO=6 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=48 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1401 +15=0x1402 +16=0x1403 +17=0x1404 +18=0x1405 +19=0x1600 +20=0x1601 +21=0x1602 +22=0x1603 +23=0x1604 +24=0x1605 +25=0x1800 +26=0x1801 +27=0x1802 +28=0x1803 +29=0x1804 +30=0x1805 +31=0x1A00 +32=0x1A01 +33=0x1A02 +34=0x1A03 +35=0x1A04 +36=0x1A05 +37=0x1F25 +38=0x1F55 +39=0x1F80 +40=0x1F81 +41=0x1F82 +42=0x1F84 +43=0x1F85 +44=0x1F86 +45=0x1F87 +46=0x1F88 +47=0x1F89 +48=0x1F8A + +[ManufacturerObjects] +SupportedObjects=36 +1=0x2000 +2=0x2001 +3=0x2002 +4=0x2003 +5=0x2004 +6=0x2005 +7=0x2200 +8=0x2201 +9=0x2202 +10=0x2203 +11=0x2204 +12=0x2205 +13=0x5800 +14=0x5801 +15=0x5802 +16=0x5803 +17=0x5804 +18=0x5805 +19=0x5A00 +20=0x5A01 +21=0x5A02 +22=0x5A03 +23=0x5A04 +24=0x5A05 +25=0x5C00 +26=0x5C01 +27=0x5C02 +28=0x5C03 +29=0x5C04 +30=0x5C05 +31=0x5E00 +32=0x5E01 +33=0x5E02 +34=0x5E03 +35=0x5E04 +36=0x5E05 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=6 +2=0x00000082 +3=0x00000083 +4=0x00000084 +5=0x00000085 +6=0x00000086 +7=0x00000087 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1401] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1401sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1401sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000183 + +[1401sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1401sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1401sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1401sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1402] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1402sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1402sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000184 + +[1402sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1402sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1402sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1402sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1403] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1403sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1403sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000185 + +[1403sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1403sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1403sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1403sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1404] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1404sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1404sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000186 + +[1404sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1404sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1404sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1404sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1405] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1405sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1405sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000187 + +[1405sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1405sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1405sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1405sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1601] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1601Value] +NrOfEntries=1 +1=0x20010120 + +[1602] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1602Value] +NrOfEntries=1 +1=0x20020120 + +[1603] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1603Value] +NrOfEntries=1 +1=0x20030120 + +[1604] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1604Value] +NrOfEntries=1 +1=0x20040120 + +[1605] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1605Value] +NrOfEntries=1 +1=0x20050120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1801] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1801sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1801sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000203 + +[1801sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1801sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1801sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1802] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1802sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1802sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000204 + +[1802sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1802sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1802sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1802sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1802sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1803] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1803sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1803sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000205 + +[1803sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1803sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1803sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1803sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1803sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1804] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1804sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1804sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000206 + +[1804sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1804sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1804sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1804sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1804sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1805] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1805sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1805sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000207 + +[1805sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1805sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1805sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1805sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1805sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1A01] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A01Value] +NrOfEntries=1 +1=0x22010120 + +[1A02] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A02Value] +NrOfEntries=1 +1=0x22020120 + +[1A03] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A03Value] +NrOfEntries=1 +1=0x22030120 + +[1A04] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A04Value] +NrOfEntries=1 +1=0x22040120 + +[1A05] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A05Value] +NrOfEntries=1 +1=0x22050120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=6 +2=0x00000005 +3=0x00000005 +4=0x00000005 +5=0x00000005 +6=0x00000005 +7=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=6 +2=0x00000360 +3=0x00000360 +4=0x00000360 +5=0x00000360 +6=0x00000360 +7=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=motioncontroller_2: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2001] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 2 +ObjectType=0x09 + +[2001sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2001sub1] +ParameterName=motioncontroller_3: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2002] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 3 +ObjectType=0x09 + +[2002sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2002sub1] +ParameterName=motioncontroller_4: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2003] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 4 +ObjectType=0x09 + +[2003sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2003sub1] +ParameterName=motioncontroller_5: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2004] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 5 +ObjectType=0x09 + +[2004sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2004sub1] +ParameterName=motioncontroller_6: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2005] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 6 +ObjectType=0x09 + +[2005sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2005sub1] +ParameterName=motioncontroller_7: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=motioncontroller_2: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2201] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 2 +ObjectType=0x09 + +[2201sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2201sub1] +ParameterName=motioncontroller_3: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2202] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 3 +ObjectType=0x09 + +[2202sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2202sub1] +ParameterName=motioncontroller_4: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2203] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 4 +ObjectType=0x09 + +[2203sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2203sub1] +ParameterName=motioncontroller_5: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2204] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 5 +ObjectType=0x09 + +[2204sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2204sub1] +ParameterName=motioncontroller_6: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2205] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 6 +ObjectType=0x09 + +[2205sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2205sub1] +ParameterName=motioncontroller_7: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5801] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5802] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000104 + +[5803] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000105 + +[5804] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000106 + +[5805] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000107 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5A01] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A01Value] +NrOfEntries=1 +1=0x40010020 + +[5A02] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A02Value] +NrOfEntries=1 +1=0x40010020 + +[5A03] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A03Value] +NrOfEntries=1 +1=0x40010020 + +[5A04] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A04Value] +NrOfEntries=1 +1=0x40010020 + +[5A05] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A05Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5C01] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5C02] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000104 + +[5C03] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000105 + +[5C04] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000106 + +[5C05] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000107 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 + +[5E01] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E01Value] +NrOfEntries=1 +1=0x40000020 + +[5E02] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E02Value] +NrOfEntries=1 +1=0x40000020 + +[5E03] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E03Value] +NrOfEntries=1 +1=0x40000020 + +[5E04] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E04Value] +NrOfEntries=1 +1=0x40000020 + +[5E05] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E05Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_proxy_driver/config/nmt_test/master.dcf b/canopen_proxy_driver/config/nmt_test/master.dcf new file mode 100644 index 00000000..09e12f40 --- /dev/null +++ b/canopen_proxy_driver/config/nmt_test/master.dcf @@ -0,0 +1,506 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=1 +NrOfTxPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=28 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1600 +15=0x1800 +16=0x1A00 +17=0x1F25 +18=0x1F55 +19=0x1F80 +20=0x1F81 +21=0x1F82 +22=0x1F84 +23=0x1F85 +24=0x1F86 +25=0x1F87 +26=0x1F88 +27=0x1F89 +28=0x1F8A + +[ManufacturerObjects] +SupportedObjects=6 +1=0x2000 +2=0x2200 +3=0x5800 +4=0x5A00 +5=0x5C00 +6=0x5E00 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=1 +2=0x00000082 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=1 +2=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=1 +2=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=motioncontroller_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=motioncontroller_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_proxy_driver/config/pdo_test/master.dcf b/canopen_proxy_driver/config/pdo_test/master.dcf new file mode 100644 index 00000000..09e12f40 --- /dev/null +++ b/canopen_proxy_driver/config/pdo_test/master.dcf @@ -0,0 +1,506 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=1 +NrOfTxPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=28 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1600 +15=0x1800 +16=0x1A00 +17=0x1F25 +18=0x1F55 +19=0x1F80 +20=0x1F81 +21=0x1F82 +22=0x1F84 +23=0x1F85 +24=0x1F86 +25=0x1F87 +26=0x1F88 +27=0x1F89 +28=0x1F8A + +[ManufacturerObjects] +SupportedObjects=6 +1=0x2000 +2=0x2200 +3=0x5800 +4=0x5A00 +5=0x5C00 +6=0x5E00 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=1 +2=0x00000082 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=1 +2=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=1 +2=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=motioncontroller_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=motioncontroller_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_proxy_driver/config/sdo_test/master.dcf b/canopen_proxy_driver/config/sdo_test/master.dcf new file mode 100644 index 00000000..09e12f40 --- /dev/null +++ b/canopen_proxy_driver/config/sdo_test/master.dcf @@ -0,0 +1,506 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=1 +NrOfTxPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=28 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1600 +15=0x1800 +16=0x1A00 +17=0x1F25 +18=0x1F55 +19=0x1F80 +20=0x1F81 +21=0x1F82 +22=0x1F84 +23=0x1F85 +24=0x1F86 +25=0x1F87 +26=0x1F88 +27=0x1F89 +28=0x1F8A + +[ManufacturerObjects] +SupportedObjects=6 +1=0x2000 +2=0x2200 +3=0x5800 +4=0x5A00 +5=0x5C00 +6=0x5E00 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=1 +2=0x00000082 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=1 +2=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=1 +2=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=motioncontroller_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=motioncontroller_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/canopen_proxy_driver.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/canopen_proxy_driver.hpp deleted file mode 100644 index e8ab5b75..00000000 --- a/canopen_proxy_driver/include/canopen_proxy_driver/canopen_proxy_driver.hpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ -#define CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ -#include - -#include "canopen_proxy_driver/visibility_control.h" -#include "canopen_base_driver/canopen_base_driver.hpp" - -namespace ros2_canopen -{ -class ProxyDriver : public BaseDriver -{ - rclcpp::Publisher::SharedPtr nmt_state_publisher; - rclcpp::Publisher::SharedPtr rpdo_publisher; - rclcpp::Subscription::SharedPtr tpdo_subscriber; - rclcpp::Service::SharedPtr nmt_state_reset_service; - rclcpp::Service::SharedPtr nmt_state_start_service; - rclcpp::Service::SharedPtr sdo_read_service; - rclcpp::Service::SharedPtr sdo_write_service; - - std::mutex sdo_mtex; - -protected: - void on_nmt(canopen::NmtState nmt_state); - - void on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg); - - void on_rpdo(COData d); - - void on_nmt_state_reset( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - void on_nmt_state_start( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - void on_sdo_read( - const canopen_interfaces::srv::CORead::Request::SharedPtr request, - canopen_interfaces::srv::CORead::Response::SharedPtr response); - - void on_sdo_write( - const canopen_interfaces::srv::COWrite::Request::SharedPtr request, - canopen_interfaces::srv::COWrite::Response::SharedPtr response); - -public: - explicit ProxyDriver(const rclcpp::NodeOptions & options) - : BaseDriver(options) {} - - void init( - ev::Executor & exec, - canopen::AsyncMaster & master, - uint8_t node_id, - std::shared_ptr config) noexcept override - { - BaseDriver::init(exec, master, node_id, config); - nmt_state_publisher = this->create_publisher( - std::string( - this->get_name()).append("/nmt_state").c_str(), 10); - tpdo_subscriber = this->create_subscription( - std::string(this->get_name()).append("/tpdo").c_str(), - 10, - std::bind(&ProxyDriver::on_tpdo, this, std::placeholders::_1)); - - rpdo_publisher = this->create_publisher( - std::string(this->get_name()).append("/rpdo").c_str(), 10); - - nmt_state_reset_service = this->create_service( - std::string(this->get_name()).append("/nmt_reset_node").c_str(), - std::bind( - &ros2_canopen::ProxyDriver::on_nmt_state_reset, - this, - std::placeholders::_1, - std::placeholders::_2)); - - nmt_state_start_service = this->create_service( - std::string(this->get_name()).append("/nmt_start_node").c_str(), - std::bind( - &ros2_canopen::ProxyDriver::on_nmt_state_start, - this, - std::placeholders::_1, - std::placeholders::_2)); - - sdo_read_service = this->create_service( - std::string(this->get_name()).append("/sdo_read").c_str(), - std::bind( - &ros2_canopen::ProxyDriver::on_sdo_read, - this, - std::placeholders::_1, - std::placeholders::_2)); - - sdo_write_service = this->create_service( - std::string(this->get_name()).append("/sdo_write").c_str(), - std::bind( - &ros2_canopen::ProxyDriver::on_sdo_write, - this, - std::placeholders::_1, - std::placeholders::_2)); - } -}; -} // namespace ros2_canopen - -#endif // CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ \ No newline at end of file diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_canopen_proxy_driver.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_canopen_proxy_driver.hpp deleted file mode 100644 index e21dd4eb..00000000 --- a/canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_canopen_proxy_driver.hpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ -#define CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ -#include - -#include "canopen_proxy_driver/visibility_control.h" -#include "canopen_base_driver/lifecycle_canopen_base_driver.hpp" - -namespace ros2_canopen -{ - class LifecycleProxyDriver : public LifecycleBaseDriver - { - rclcpp::Publisher::SharedPtr nmt_state_publisher; - rclcpp::Publisher::SharedPtr rpdo_publisher; - rclcpp::Subscription::SharedPtr tpdo_subscriber; - rclcpp::Service::SharedPtr nmt_state_reset_service; - rclcpp::Service::SharedPtr nmt_state_start_service; - rclcpp::Service::SharedPtr sdo_read_service; - rclcpp::Service::SharedPtr sdo_write_service; - - std::mutex sdo_mtex; - - protected: - void on_nmt(canopen::NmtState nmt_state); - - void on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg); - - void on_rpdo(COData d); - - void on_nmt_state_reset( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - void on_nmt_state_start( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - void on_sdo_read( - const canopen_interfaces::srv::CORead::Request::SharedPtr request, - canopen_interfaces::srv::CORead::Response::SharedPtr response); - - void on_sdo_write( - const canopen_interfaces::srv::COWrite::Request::SharedPtr request, - canopen_interfaces::srv::COWrite::Response::SharedPtr response); - - virtual void register_ros_interface() override; - - - /** - * @brief Configures the driver - * - * Read parameters - * Initialise objects - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &state); - - /** - * @brief Activates the driver - * - * Add driver to masters event loop - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State &state); - - /** - * @brief Deactivates the driver - * - * Remove driver from masters event loop - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &state); - - /** - * @brief Cleanup the driver - * - * Delete objects - * - * @param state - * @return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &state); - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &state); - - public: - explicit LifecycleProxyDriver(const rclcpp::NodeOptions &options) - : LifecycleBaseDriver(options) {} - - }; -} // namespace ros2_canopen - -#endif // CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_proxy_driver.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_proxy_driver.hpp new file mode 100644 index 00000000..63241a8b --- /dev/null +++ b/canopen_proxy_driver/include/canopen_proxy_driver/lifecycle_proxy_driver.hpp @@ -0,0 +1,73 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_PROXY_DRIVER__CANOPEN_LIFECYCLE_PROXY_DRIVER_HPP_ +#define CANOPEN_PROXY_DRIVER__CANOPEN_LIFECYCLE_PROXY_DRIVER_HPP_ + +#include "canopen_core/driver_node.hpp" +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" + +namespace ros2_canopen +{ +/** + * @brief Lifecycle Proxy Driver + * + * A very basic driver without any functionality. + * + */ +class LifecycleProxyDriver : public ros2_canopen::LifecycleCanopenDriver +{ + std::shared_ptr> + node_canopen_proxy_driver_; + +public: + LifecycleProxyDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); + virtual bool reset_node_nmt_command() + { + return node_canopen_proxy_driver_->reset_node_nmt_command(); + } + + virtual bool start_node_nmt_command() + { + return node_canopen_proxy_driver_->start_node_nmt_command(); + } + + virtual bool tpdo_transmit(ros2_canopen::COData & data) + { + return node_canopen_proxy_driver_->tpdo_transmit(data); + } + + virtual bool sdo_write(ros2_canopen::COData & data) + { + return node_canopen_proxy_driver_->sdo_write(data); + } + + virtual bool sdo_read(ros2_canopen::COData & data) + { + return node_canopen_proxy_driver_->sdo_read(data); + } + + void register_nmt_state_cb(std::function nmt_state_cb) + { + node_canopen_proxy_driver_->register_nmt_state_cb(nmt_state_cb); + } + + void register_rpdo_cb(std::function rpdo_cb) + { + node_canopen_proxy_driver_->register_rpdo_cb(rpdo_cb); + } +}; +} // namespace ros2_canopen + +#endif // CANOPEN_PROXY_DRIVER__CANOPEN_LIFECYCLE_PROXY_DRIVER_HPP_ diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp new file mode 100644 index 00000000..0219b4c9 --- /dev/null +++ b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp @@ -0,0 +1,67 @@ +#ifndef NODE_CANOPEN_PROXY_DRIVER +#define NODE_CANOPEN_PROXY_DRIVER + +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +namespace ros2_canopen +{ +namespace node_interfaces +{ + +template +class NodeCanopenProxyDriver : public NodeCanopenBaseDriver +{ + static_assert( + std::is_base_of::value || + std::is_base_of::value, + "NODETYPE must derive from rclcpp::Node or rclcpp_lifecycle::LifecycleNode"); + +protected: + rclcpp::Publisher::SharedPtr nmt_state_publisher; + rclcpp::Publisher::SharedPtr rpdo_publisher; + rclcpp::Subscription::SharedPtr tpdo_subscriber; + rclcpp::Service::SharedPtr nmt_state_reset_service; + rclcpp::Service::SharedPtr nmt_state_start_service; + rclcpp::Service::SharedPtr sdo_read_service; + rclcpp::Service::SharedPtr sdo_write_service; + + std::mutex sdo_mtex; + + virtual void on_nmt(canopen::NmtState nmt_state) override; + virtual void on_rpdo(COData data) override; + virtual void on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg); + + void on_nmt_state_reset( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + void on_nmt_state_start( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + void on_sdo_read( + const canopen_interfaces::srv::CORead::Request::SharedPtr request, + canopen_interfaces::srv::CORead::Response::SharedPtr response); + + void on_sdo_write( + const canopen_interfaces::srv::COWrite::Request::SharedPtr request, + canopen_interfaces::srv::COWrite::Response::SharedPtr response); + +public: + NodeCanopenProxyDriver(NODETYPE * node); + + virtual void init(bool called_from_base) override; + + virtual bool reset_node_nmt_command(); + + virtual bool start_node_nmt_command(); + + virtual bool tpdo_transmit(COData & data); + + virtual bool sdo_write(COData & data); + + virtual bool sdo_read(COData & data); +}; +} // namespace node_interfaces +} // namespace ros2_canopen + +#endif diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp new file mode 100644 index 00000000..53104dd4 --- /dev/null +++ b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp @@ -0,0 +1,306 @@ +#ifndef NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_ +#define NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_ + +#include "canopen_core/driver_error.hpp" +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" + +using namespace ros2_canopen::node_interfaces; + +template +NodeCanopenProxyDriver::NodeCanopenProxyDriver(NODETYPE * node) +: ros2_canopen::node_interfaces::NodeCanopenBaseDriver(node) +{ +} + +template +void NodeCanopenProxyDriver::init(bool called_from_base) +{ + RCLCPP_ERROR(this->node_->get_logger(), "Not init implemented."); +} + +template <> +void NodeCanopenProxyDriver::init(bool called_from_base) +{ + nmt_state_publisher = this->node_->create_publisher( + std::string(this->node_->get_name()).append("/nmt_state").c_str(), 10); + tpdo_subscriber = this->node_->create_subscription( + std::string(this->node_->get_name()).append("/tpdo").c_str(), 10, + std::bind(&NodeCanopenProxyDriver::on_tpdo, this, std::placeholders::_1)); + + rpdo_publisher = this->node_->create_publisher( + std::string(this->node_->get_name()).append("/rpdo").c_str(), 10); + + nmt_state_reset_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/nmt_reset_node").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_nmt_state_reset, this, std::placeholders::_1, + std::placeholders::_2)); + + nmt_state_start_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/nmt_start_node").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_nmt_state_start, this, std::placeholders::_1, + std::placeholders::_2)); + + sdo_read_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_read").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_sdo_read, this, std::placeholders::_1, + std::placeholders::_2)); + + sdo_write_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_write").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_sdo_write, this, std::placeholders::_1, + std::placeholders::_2)); +} + +template <> +void NodeCanopenProxyDriver::init(bool called_from_base) +{ + nmt_state_publisher = this->node_->create_publisher( + std::string(this->node_->get_name()).append("/nmt_state").c_str(), 10); + tpdo_subscriber = this->node_->create_subscription( + std::string(this->node_->get_name()).append("/tpdo").c_str(), 10, + std::bind( + &NodeCanopenProxyDriver::on_tpdo, this, + std::placeholders::_1)); + + rpdo_publisher = this->node_->create_publisher( + std::string(this->node_->get_name()).append("/rpdo").c_str(), 10); + + nmt_state_reset_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/nmt_reset_node").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_nmt_state_reset, this, + std::placeholders::_1, std::placeholders::_2)); + + nmt_state_start_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/nmt_start_node").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_nmt_state_start, this, + std::placeholders::_1, std::placeholders::_2)); + + sdo_read_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_read").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_sdo_read, this, + std::placeholders::_1, std::placeholders::_2)); + + sdo_write_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/sdo_write").c_str(), + std::bind( + &NodeCanopenProxyDriver::on_sdo_write, this, + std::placeholders::_1, std::placeholders::_2)); +} + +template +void NodeCanopenProxyDriver::on_nmt(canopen::NmtState nmt_state) +{ + if (this->activated_.load()) + { + auto message = std_msgs::msg::String(); + + switch (nmt_state) + { + case canopen::NmtState::BOOTUP: + message.data = "BOOTUP"; + break; + case canopen::NmtState::PREOP: + message.data = "PREOP"; + break; + case canopen::NmtState::RESET_COMM: + message.data = "RESET_COMM"; + break; + case canopen::NmtState::RESET_NODE: + message.data = "RESET_NODE"; + break; + case canopen::NmtState::START: + message.data = "START"; + break; + case canopen::NmtState::STOP: + message.data = "STOP"; + break; + case canopen::NmtState::TOGGLE: + message.data = "TOGGLE"; + break; + default: + RCLCPP_ERROR(this->node_->get_logger(), "Unknown NMT State."); + message.data = "ERROR"; + break; + } + RCLCPP_INFO( + this->node_->get_logger(), "Slave %hhu: Switched NMT state to %s", + this->lely_driver_->get_id(), message.data.c_str()); + + nmt_state_publisher->publish(message); + } +} + +template +void NodeCanopenProxyDriver::on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg) +{ + ros2_canopen::COData data = {msg->index, msg->subindex, msg->data}; + if (!tpdo_transmit(data)) + { + RCLCPP_ERROR(this->node_->get_logger(), "Could transmit PDO because driver not activated."); + } +} + +template +bool NodeCanopenProxyDriver::tpdo_transmit(ros2_canopen::COData & data) +{ + if (this->activated_.load()) + { + RCLCPP_INFO( + this->node_->get_logger(), "Node ID %hhu: Transmit PDO index %x, subindex %hhu, data %d", + this->lely_driver_->get_id(), data.index_, data.subindex_, + data.data_); // ToDo: Remove or make debug + this->lely_driver_->tpdo_transmit(data); + return true; + } + return false; +} + +template +void NodeCanopenProxyDriver::on_rpdo(ros2_canopen::COData d) +{ + if (this->activated_.load()) + { + // RCLCPP_INFO( + // this->node_->get_logger(), "Node ID %hhu: Received PDO index %#04x, subindex %hhu, data + // %x", this->lely_driver_->get_id(), d.index_, d.subindex_, d.data_); + auto message = canopen_interfaces::msg::COData(); + message.index = d.index_; + message.subindex = d.subindex_; + message.data = d.data_; + rpdo_publisher->publish(message); + } +} + +template +void NodeCanopenProxyDriver::on_nmt_state_reset( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = reset_node_nmt_command(); +} + +template +bool NodeCanopenProxyDriver::reset_node_nmt_command() +{ + if (this->activated_.load()) + { + this->lely_driver_->nmt_command(canopen::NmtCommand::RESET_NODE); + return true; + } + RCLCPP_ERROR( + this->node_->get_logger(), "Could not reset device via NMT because driver not activated."); + return false; +} + +template +void NodeCanopenProxyDriver::on_nmt_state_start( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = start_node_nmt_command(); +} + +template +bool NodeCanopenProxyDriver::start_node_nmt_command() +{ + if (this->activated_.load()) + { + this->lely_driver_->nmt_command(canopen::NmtCommand::START); + return true; + } + RCLCPP_ERROR( + this->node_->get_logger(), "Could not start device via NMT because driver not activated."); + return false; +} + +template +void NodeCanopenProxyDriver::on_sdo_read( + const canopen_interfaces::srv::CORead::Request::SharedPtr request, + canopen_interfaces::srv::CORead::Response::SharedPtr response) +{ + ros2_canopen::COData data = {request->index, request->subindex, 0U}; + response->success = sdo_read(data); + response->data = data.data_; +} + +template +bool NodeCanopenProxyDriver::sdo_read(ros2_canopen::COData & data) +{ + if (this->activated_.load()) + { + RCLCPP_INFO( + this->node_->get_logger(), "Slave %hhu: SDO Read Call index=0x%x subindex=%hhu", + this->lely_driver_->get_id(), data.index_, data.subindex_); + + // Only allow one SDO request concurrently + std::scoped_lock lk(sdo_mtex); + // Send read request + auto f = this->lely_driver_->async_sdo_read(data); + // Wait for response + f.wait(); + // Process response + try + { + data.data_ = f.get().data_; + } + catch (std::exception & e) + { + RCLCPP_ERROR(this->node_->get_logger(), e.what()); + return false; + } + return true; + } + RCLCPP_ERROR(this->node_->get_logger(), "Could not read from SDO because driver not activated."); + return false; +} + +template +void NodeCanopenProxyDriver::on_sdo_write( + const canopen_interfaces::srv::COWrite::Request::SharedPtr request, + canopen_interfaces::srv::COWrite::Response::SharedPtr response) +{ + ros2_canopen::COData data = {request->index, request->subindex, request->data}; + response->success = sdo_write(data); +} + +template +bool NodeCanopenProxyDriver::sdo_write(ros2_canopen::COData & data) +{ + if (this->activated_.load()) + { + RCLCPP_INFO( + this->node_->get_logger(), "Slave %hhu: SDO Write Call index=0x%x subindex=%hhu data=%u", + this->lely_driver_->get_id(), data.index_, data.subindex_, data.data_); + + // Only allow one SDO request concurrently + std::scoped_lock lk(sdo_mtex); + + // Send write request + auto f = this->lely_driver_->async_sdo_write(data); + // Wait for request to complete + f.wait(); + + // Process response + try + { + return f.get(); + } + catch (std::exception & e) + { + RCLCPP_ERROR(this->node_->get_logger(), e.what()); + return false; + } + return true; + } + RCLCPP_ERROR(this->node_->get_logger(), "Could not write to SDO because driver not activated."); + return false; +} + +#endif diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/proxy_driver.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/proxy_driver.hpp new file mode 100644 index 00000000..60872923 --- /dev/null +++ b/canopen_proxy_driver/include/canopen_proxy_driver/proxy_driver.hpp @@ -0,0 +1,72 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ +#define CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ +#include "canopen_core/driver_node.hpp" +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" + +namespace ros2_canopen +{ +/** + * @brief Abstract Class for a CANopen Device Node + * + * This class provides the base functionality for creating a + * CANopen device node. It provides callbacks for nmt and rpdo. + */ +class ProxyDriver : public ros2_canopen::CanopenDriver +{ + std::shared_ptr> node_canopen_proxy_driver_; + +public: + ProxyDriver(rclcpp::NodeOptions node_options = rclcpp::NodeOptions()); + + virtual bool reset_node_nmt_command() + { + return node_canopen_proxy_driver_->reset_node_nmt_command(); + } + + virtual bool start_node_nmt_command() + { + return node_canopen_proxy_driver_->start_node_nmt_command(); + } + + virtual bool tpdo_transmit(ros2_canopen::COData & data) + { + return node_canopen_proxy_driver_->tpdo_transmit(data); + } + + virtual bool sdo_write(ros2_canopen::COData & data) + { + return node_canopen_proxy_driver_->sdo_write(data); + } + + virtual bool sdo_read(ros2_canopen::COData & data) + { + return node_canopen_proxy_driver_->sdo_read(data); + } + + void register_nmt_state_cb(std::function nmt_state_cb) + { + node_canopen_proxy_driver_->register_nmt_state_cb(nmt_state_cb); + } + + void register_rpdo_cb(std::function rpdo_cb) + { + node_canopen_proxy_driver_->register_rpdo_cb(rpdo_cb); + } +}; +} // namespace ros2_canopen + +#endif // CANOPEN_PROXY_DRIVER__CANOPEN_PROXY_DRIVER_HPP_ diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/visibility_control.h b/canopen_proxy_driver/include/canopen_proxy_driver/visibility_control.h index 12d22d7e..937d7290 100644 --- a/canopen_proxy_driver/include/canopen_proxy_driver/visibility_control.h +++ b/canopen_proxy_driver/include/canopen_proxy_driver/visibility_control.h @@ -18,31 +18,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define CANOPEN_PROXY_DRIVER_EXPORT __attribute__ ((dllexport)) - #define CANOPEN_PROXY_DRIVER_IMPORT __attribute__ ((dllimport)) - #else - #define CANOPEN_PROXY_DRIVER_EXPORT __declspec(dllexport) - #define CANOPEN_PROXY_DRIVER_IMPORT __declspec(dllimport) - #endif - #ifdef CANOPEN_PROXY_DRIVER_BUILDING_LIBRARY - #define CANOPEN_PROXY_DRIVER_PUBLIC CANOPEN_PROXY_DRIVER_EXPORT - #else - #define CANOPEN_PROXY_DRIVER_PUBLIC CANOPEN_PROXY_DRIVER_IMPORT - #endif - #define CANOPEN_PROXY_DRIVER_PUBLIC_TYPE CANOPEN_PROXY_DRIVER_PUBLIC - #define CANOPEN_PROXY_DRIVER_LOCAL +#ifdef __GNUC__ +#define CANOPEN_PROXY_DRIVER_EXPORT __attribute__((dllexport)) +#define CANOPEN_PROXY_DRIVER_IMPORT __attribute__((dllimport)) #else - #define CANOPEN_PROXY_DRIVER_EXPORT __attribute__ ((visibility("default"))) - #define CANOPEN_PROXY_DRIVER_IMPORT - #if __GNUC__ >= 4 - #define CANOPEN_PROXY_DRIVER_PUBLIC __attribute__ ((visibility("default"))) - #define CANOPEN_PROXY_DRIVER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define CANOPEN_PROXY_DRIVER_PUBLIC - #define CANOPEN_PROXY_DRIVER_LOCAL - #endif - #define CANOPEN_PROXY_DRIVER_PUBLIC_TYPE +#define CANOPEN_PROXY_DRIVER_EXPORT __declspec(dllexport) +#define CANOPEN_PROXY_DRIVER_IMPORT __declspec(dllimport) +#endif +#ifdef CANOPEN_PROXY_DRIVER_BUILDING_LIBRARY +#define CANOPEN_PROXY_DRIVER_PUBLIC CANOPEN_PROXY_DRIVER_EXPORT +#else +#define CANOPEN_PROXY_DRIVER_PUBLIC CANOPEN_PROXY_DRIVER_IMPORT +#endif +#define CANOPEN_PROXY_DRIVER_PUBLIC_TYPE CANOPEN_PROXY_DRIVER_PUBLIC +#define CANOPEN_PROXY_DRIVER_LOCAL +#else +#define CANOPEN_PROXY_DRIVER_EXPORT __attribute__((visibility("default"))) +#define CANOPEN_PROXY_DRIVER_IMPORT +#if __GNUC__ >= 4 +#define CANOPEN_PROXY_DRIVER_PUBLIC __attribute__((visibility("default"))) +#define CANOPEN_PROXY_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else +#define CANOPEN_PROXY_DRIVER_PUBLIC +#define CANOPEN_PROXY_DRIVER_LOCAL +#endif +#define CANOPEN_PROXY_DRIVER_PUBLIC_TYPE #endif #endif // CANOPEN_PROXY_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index c2bdac97..ca4efa0f 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -9,16 +9,17 @@ ament_cmake_ros + canopen_base_driver + canopen_core + canopen_interfaces rclcpp rclcpp_components + rclcpp_lifecycle std_msgs std_srvs - canopen_base_driver ament_lint_auto - - ament_cmake diff --git a/canopen_proxy_driver/readme.md b/canopen_proxy_driver/readme.md new file mode 100644 index 00000000..59854a35 --- /dev/null +++ b/canopen_proxy_driver/readme.md @@ -0,0 +1,6 @@ +# canopen_ros_proxy_driver + +## Testing +``` +colcon test --packages-select canopen_proxy_driver --event-handlers desktop_notification+ status+ summary- console_direct+ terminal_title+ +``` diff --git a/canopen_proxy_driver/src/canopen_proxy_driver.cpp b/canopen_proxy_driver/src/canopen_proxy_driver.cpp deleted file mode 100644 index bba6c7a2..00000000 --- a/canopen_proxy_driver/src/canopen_proxy_driver.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include "canopen_proxy_driver/canopen_proxy_driver.hpp" - -namespace ros2_canopen -{ -void ProxyDriver::on_nmt(canopen::NmtState nmt_state) -{ - auto message = std_msgs::msg::String(); - - switch (nmt_state) { - case canopen::NmtState::BOOTUP: - message.data = "BOOTUP"; - break; - case canopen::NmtState::PREOP: - message.data = "PREOP"; - break; - case canopen::NmtState::RESET_COMM: - message.data = "RESET_COMM"; - break; - case canopen::NmtState::RESET_NODE: - message.data = "RESET_NODE"; - break; - case canopen::NmtState::START: - message.data = "START"; - break; - case canopen::NmtState::STOP: - message.data = "STOP"; - break; - case canopen::NmtState::TOGGLE: - message.data = "TOGGLE"; - break; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown NMT State."); - message.data = "ERROR"; - break; - } - RCLCPP_INFO( - this->get_logger(), - "Slave %hhu: Switched NMT state to %s", - this->driver->get_id(), - message.data.c_str()); - - nmt_state_publisher->publish(message); -} - -void ProxyDriver::on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg) -{ - COData data = {msg->index, msg->subindex, msg->data, static_cast(msg->type)}; - driver->tpdo_transmit(data); -} - -void ProxyDriver::on_rpdo(COData d) -{ - RCLCPP_INFO( - this->get_logger(), - "Slave %hhu: Sent PDO index %hu, subindex %hhu, data %x", - this->driver->get_id(), - d.index_, - d.subindex_, - d.data_); - auto message = canopen_interfaces::msg::COData(); - message.index = d.index_; - message.subindex = d.subindex_; - message.data = d.data_; - message.type = static_cast(d.type_); - rpdo_publisher->publish(message); -} - -void ProxyDriver::on_nmt_state_reset( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - driver->nmt_command(canopen::NmtCommand::RESET_NODE); - response->success = true; -} - -void ProxyDriver::on_nmt_state_start( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - driver->nmt_command(canopen::NmtCommand::START); - response->success = true; -} - -void ProxyDriver::on_sdo_read( - const canopen_interfaces::srv::CORead::Request::SharedPtr request, - canopen_interfaces::srv::CORead::Response::SharedPtr response) -{ - RCLCPP_INFO( - this->get_logger(), "Slave %hhu: SDO Read Call index=0x%x subindex=%hhu bits=%hhu", - this->driver->get_id(), request->index, request->subindex, request->type); - - // Only allow one SDO request concurrently - std::scoped_lock lk(sdo_mtex); - // Prepare Data read - COData data = {request->index, request->subindex, 0U, static_cast(request->type)}; - // Send read request - auto f = driver->async_sdo_read(data); - // Wait for response - f.wait(); - // Process response - try { - response->data = f.get().data_; - response->success = true; - } catch (std::exception & e) { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } -} - -void ProxyDriver::on_sdo_write( - const canopen_interfaces::srv::COWrite::Request::SharedPtr request, - canopen_interfaces::srv::COWrite::Response::SharedPtr response) -{ - RCLCPP_INFO( - this->get_logger(), "Slave %hhu: SDO Read Call index=0x%x subindex=%hhu bits=%hhu data=%u", - this->driver->get_id(), request->index, request->subindex, request->type, request->data); - - // Only allow one SDO request concurrently - std::scoped_lock lk(sdo_mtex); - - // Prepare Data - COData d = - {request->index, request->subindex, request->data, static_cast(request->type)}; - // Send write request - auto f = driver->async_sdo_write(d); - // Wait for request to complete - f.wait(); - - // Process response - try { - response->success = f.get(); - } catch (std::exception & e) { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } -} -} // namespace ros2_canopen - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::ProxyDriver) \ No newline at end of file diff --git a/canopen_proxy_driver/src/lifecycle_canopen_proxy_driver.cpp b/canopen_proxy_driver/src/lifecycle_canopen_proxy_driver.cpp deleted file mode 100644 index 7eb05ebf..00000000 --- a/canopen_proxy_driver/src/lifecycle_canopen_proxy_driver.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// Copyright 2022 Christoph Hellmann Santos -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#include "canopen_proxy_driver/lifecycle_canopen_proxy_driver.hpp" - -namespace ros2_canopen -{ - void LifecycleProxyDriver::on_nmt(canopen::NmtState nmt_state) - { - if (this->activated_.load()) - { - auto message = std_msgs::msg::String(); - - switch (nmt_state) - { - case canopen::NmtState::BOOTUP: - message.data = "BOOTUP"; - break; - case canopen::NmtState::PREOP: - message.data = "PREOP"; - break; - case canopen::NmtState::RESET_COMM: - message.data = "RESET_COMM"; - break; - case canopen::NmtState::RESET_NODE: - message.data = "RESET_NODE"; - break; - case canopen::NmtState::START: - message.data = "START"; - break; - case canopen::NmtState::STOP: - message.data = "STOP"; - break; - case canopen::NmtState::TOGGLE: - message.data = "TOGGLE"; - break; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown NMT State."); - message.data = "ERROR"; - break; - } - RCLCPP_INFO( - this->get_logger(), - "Slave %hhu: Switched NMT state to %s", - this->driver_->get_id(), - message.data.c_str()); - - nmt_state_publisher->publish(message); - } - } - - void LifecycleProxyDriver::on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg) - { - if (this->activated_.load()) - { - COData data = {msg->index, msg->subindex, msg->data, static_cast(msg->type)}; - driver_->tpdo_transmit(data); - return; - } - RCLCPP_ERROR(this->get_logger(), "Could transmit PDO because driver not activated."); - } - - void LifecycleProxyDriver::on_rpdo(COData d) - { - if (this->activated_.load()) - { - RCLCPP_INFO( - this->get_logger(), - "Slave %hhu: Sent PDO index %hu, subindex %hhu, data %x", - this->driver_->get_id(), - d.index_, - d.subindex_, - d.data_); - auto message = canopen_interfaces::msg::COData(); - message.index = d.index_; - message.subindex = d.subindex_; - message.data = d.data_; - message.type = static_cast(d.type_); - rpdo_publisher->publish(message); - } - } - - void LifecycleProxyDriver::on_nmt_state_reset( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) - { - if (this->activated_.load()) - { - driver_->nmt_command(canopen::NmtCommand::RESET_NODE); - response->success = true; - return; - } - RCLCPP_ERROR(this->get_logger(), "Could not reset device via NMT because driver not activated."); - response->success = false; - } - - void LifecycleProxyDriver::on_nmt_state_start( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) - { - if (this->activated_.load()) - { - driver_->nmt_command(canopen::NmtCommand::START); - response->success = true; - return; - } - RCLCPP_ERROR(this->get_logger(), "Could not start device via NMT because driver not activated."); - response->success = false; - } - - void LifecycleProxyDriver::on_sdo_read( - const canopen_interfaces::srv::CORead::Request::SharedPtr request, - canopen_interfaces::srv::CORead::Response::SharedPtr response) - { - if (this->activated_.load()) - { - RCLCPP_INFO( - this->get_logger(), "Slave %hhu: SDO Read Call index=0x%x subindex=%hhu bits=%hhu", - this->driver_->get_id(), request->index, request->subindex, request->type); - - // Only allow one SDO request concurrently - std::scoped_lock lk(sdo_mtex); - // Prepare Data read - COData data = {request->index, request->subindex, 0U, static_cast(request->type)}; - // Send read request - auto f = driver_->async_sdo_read(data); - // Wait for response - f.wait(); - // Process response - try - { - response->data = f.get().data_; - response->success = true; - } - catch (std::exception &e) - { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } - return; - } - RCLCPP_ERROR(this->get_logger(), "Could not read from SDO because driver not activated."); - response->success = false; - } - - void LifecycleProxyDriver::on_sdo_write( - const canopen_interfaces::srv::COWrite::Request::SharedPtr request, - canopen_interfaces::srv::COWrite::Response::SharedPtr response) - { - if (this->activated_.load()) - { - RCLCPP_INFO( - this->get_logger(), "Slave %hhu: SDO Write Call index=0x%x subindex=%hhu bits=%hhu data=%u", - this->driver_->get_id(), request->index, request->subindex, request->type, request->data); - - // Only allow one SDO request concurrently - std::scoped_lock lk(sdo_mtex); - - // Prepare Data - COData d = - {request->index, request->subindex, request->data, static_cast(request->type)}; - // Send write request - auto f = driver_->async_sdo_write(d); - // Wait for request to complete - f.wait(); - - // Process response - try - { - response->success = f.get(); - } - catch (std::exception &e) - { - RCLCPP_ERROR(this->get_logger(), e.what()); - response->success = false; - } - return; - } - RCLCPP_ERROR(this->get_logger(), "Could not write to SDO because driver not activated."); - response->success = false; - } - - void LifecycleProxyDriver::register_ros_interface() - { - ros2_canopen::LifecycleDriverInterface::register_ros_interface(); - nmt_state_publisher = this->create_publisher( - std::string( - this->get_name()) - .append("/nmt_state") - .c_str(), - 10); - tpdo_subscriber = this->create_subscription( - std::string(this->get_name()).append("/tpdo").c_str(), - 10, - std::bind(&LifecycleProxyDriver::on_tpdo, this, std::placeholders::_1)); - - rpdo_publisher = this->create_publisher( - std::string(this->get_name()).append("/rpdo").c_str(), 10); - - nmt_state_reset_service = this->create_service( - std::string(this->get_name()).append("/nmt_reset_node").c_str(), - std::bind( - &ros2_canopen::LifecycleProxyDriver::on_nmt_state_reset, - this, - std::placeholders::_1, - std::placeholders::_2)); - - nmt_state_start_service = this->create_service( - std::string(this->get_name()).append("/nmt_start_node").c_str(), - std::bind( - &ros2_canopen::LifecycleProxyDriver::on_nmt_state_start, - this, - std::placeholders::_1, - std::placeholders::_2)); - - sdo_read_service = this->create_service( - std::string(this->get_name()).append("/sdo_read").c_str(), - std::bind( - &ros2_canopen::LifecycleProxyDriver::on_sdo_read, - this, - std::placeholders::_1, - std::placeholders::_2)); - - sdo_write_service = this->create_service( - std::string(this->get_name()).append("/sdo_write").c_str(), - std::bind( - &ros2_canopen::LifecycleProxyDriver::on_sdo_write, - this, - std::placeholders::_1, - std::placeholders::_2)); - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleProxyDriver::on_configure(const rclcpp_lifecycle::State &state) - { - return LifecycleBaseDriver::on_configure(state); - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleProxyDriver::on_activate(const rclcpp_lifecycle::State &state) - { - return LifecycleBaseDriver::on_activate(state); - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleProxyDriver::on_deactivate(const rclcpp_lifecycle::State &state) - { - return LifecycleBaseDriver::on_deactivate(state); - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleProxyDriver::on_cleanup(const rclcpp_lifecycle::State &state) - { - return LifecycleBaseDriver::on_cleanup(state); - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - LifecycleProxyDriver::on_shutdown(const rclcpp_lifecycle::State &state) - { - return LifecycleBaseDriver::on_shutdown(state); - } - -} // namespace ros2_canopen - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleProxyDriver) diff --git a/canopen_proxy_driver/src/lifecycle_proxy_driver.cpp b/canopen_proxy_driver/src/lifecycle_proxy_driver.cpp new file mode 100644 index 00000000..991760bb --- /dev/null +++ b/canopen_proxy_driver/src/lifecycle_proxy_driver.cpp @@ -0,0 +1,29 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "canopen_proxy_driver/lifecycle_proxy_driver.hpp" + +using namespace ros2_canopen; + +LifecycleProxyDriver::LifecycleProxyDriver(rclcpp::NodeOptions node_options) +: LifecycleCanopenDriver(node_options) +{ + node_canopen_proxy_driver_ = + std::make_shared>( + this); + node_canopen_driver_ = std::static_pointer_cast( + node_canopen_proxy_driver_); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::LifecycleProxyDriver) diff --git a/canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp b/canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp new file mode 100644 index 00000000..07ef10ff --- /dev/null +++ b/canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp @@ -0,0 +1,8 @@ +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp" + +using namespace ros2_canopen::node_interfaces; + +template class ros2_canopen::node_interfaces::NodeCanopenProxyDriver; +template class ros2_canopen::node_interfaces::NodeCanopenProxyDriver< + rclcpp_lifecycle::LifecycleNode>; diff --git a/canopen_proxy_driver/src/proxy_driver.cpp b/canopen_proxy_driver/src/proxy_driver.cpp new file mode 100644 index 00000000..5cc3eca9 --- /dev/null +++ b/canopen_proxy_driver/src/proxy_driver.cpp @@ -0,0 +1,27 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "canopen_proxy_driver/proxy_driver.hpp" + +using namespace ros2_canopen; + +ProxyDriver::ProxyDriver(rclcpp::NodeOptions node_options) : CanopenDriver(node_options) +{ + node_canopen_proxy_driver_ = + std::make_shared>(this); + node_canopen_driver_ = std::static_pointer_cast( + node_canopen_proxy_driver_); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_canopen::ProxyDriver) diff --git a/canopen_proxy_driver/test/CMakeLists.txt b/canopen_proxy_driver/test/CMakeLists.txt new file mode 100644 index 00000000..d5c6e518 --- /dev/null +++ b/canopen_proxy_driver/test/CMakeLists.txt @@ -0,0 +1,28 @@ +ament_add_gtest(test_node_interface +test_node_interface.cpp +) +ament_target_dependencies(test_node_interface + ${dependencies} +) +target_include_directories(test_node_interface PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) +target_link_libraries(test_node_interface + node_canopen_proxy_driver +) + + + + +ament_add_gtest(test_driver_component +test_driver_component.cpp +) +ament_target_dependencies(test_driver_component + ${dependencies} +) +target_include_directories(test_driver_component PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../include/ + ) + + +FILE(COPY ${CMAKE_CURRENT_SOURCE_DIR}/master.dcf DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/canopen_proxy_driver/test/master.dcf b/canopen_proxy_driver/test/master.dcf new file mode 100644 index 00000000..4f6f975e --- /dev/null +++ b/canopen_proxy_driver/test/master.dcf @@ -0,0 +1,694 @@ +[DeviceComissioning] +NodeID=1 +NodeName= +NodeRefd= +Baudrate=1000 +NetNumber=1 +NetworkName= +NetRefd= +CANopenManager=1 +LSS_SerialNumber=0x00000000 + +[DeviceInfo] +VendorName= +VendorNumber=0x00000000 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=1 +SimpleBootUpSlave=0 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=2 +NrOfTxPDO=2 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=32 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1014 +6=0x1015 +7=0x1016 +8=0x1017 +9=0x1019 +10=0x1028 +11=0x1029 +12=0x102A +13=0x1400 +14=0x1401 +15=0x1600 +16=0x1601 +17=0x1800 +18=0x1801 +19=0x1A00 +20=0x1A01 +21=0x1F25 +22=0x1F55 +23=0x1F80 +24=0x1F81 +25=0x1F82 +26=0x1F84 +27=0x1F85 +28=0x1F86 +29=0x1F87 +30=0x1F88 +31=0x1F89 +32=0x1F8A + +[ManufacturerObjects] +SupportedObjects=12 +1=0x2000 +2=0x2001 +3=0x2200 +4=0x2201 +5=0x5800 +6=0x5801 +7=0x5A00 +8=0x5A01 +9=0x5C00 +10=0x5C01 +11=0x5E00 +12=0x5E01 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000080 + +[1006] +ParameterName=Communication cycle period +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1007] +ParameterName=Synchronous window length +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1016Value] +NrOfEntries=0 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1019] +ParameterName=Synchronous counter overflow value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1028] +ParameterName=Emergency consumer object +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +CompactSubObj=127 + +[1028Value] +NrOfEntries=2 +2=0x00000082 +3=0x00000083 + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=254 + +[1029Value] +NrOfEntries=1 +1=0x00 + +[102A] +ParameterName=NMT inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000182 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1401] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1401sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1401sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000183 + +[1401sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1401sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1401sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1401sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x20000120 + +[1601] +ParameterName=RPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1601Value] +NrOfEntries=1 +1=0x20010120 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000202 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1801] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1801sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1801sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000203 + +[1801sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1801sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1801sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1801sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw +DefaultValue=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x22000120 + +[1A01] +ParameterName=TPDO mapping parameter +ObjectType=0x09 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A01Value] +NrOfEntries=1 +1=0x22010120 + +[1F25] +ParameterName=Configuration request +ObjectType=0x08 +DataType=0x0005 +AccessType=wo +CompactSubObj=127 + +[1F55] +ParameterName=Expected software identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F80] +ParameterName=NMT startup +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 + +[1F81] +ParameterName=NMT slave assignment +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F81Value] +NrOfEntries=2 +2=0x00000005 +3=0x00000005 + +[1F82] +ParameterName=Request NMT +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F84] +ParameterName=Device type identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F84Value] +NrOfEntries=0 + +[1F85] +ParameterName=Vendor identification +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F85Value] +NrOfEntries=2 +2=0x00000360 +3=0x00000360 + +[1F86] +ParameterName=Product code +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F86Value] +NrOfEntries=0 + +[1F87] +ParameterName=Revision_number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F88] +ParameterName=Serial number +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=127 + +[1F89] +ParameterName=Boot time +DataType=0x0007 +AccessType=rw +DefaultValue=0 + +[1F8A] +ParameterName=Restore configuration +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=127 + +[1F8AValue] +NrOfEntries=0 + +[2000] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 1 +ObjectType=0x09 + +[2000sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2000sub1] +ParameterName=proxy_device_1: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2001] +SubNumber=2 +ParameterName=Mapped application objects for RPDO 2 +ObjectType=0x09 + +[2001sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2001sub1] +ParameterName=proxy_device_2: UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[2200] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 1 +ObjectType=0x09 + +[2200sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2200sub1] +ParameterName=proxy_device_1: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[2201] +SubNumber=2 +ParameterName=Mapped application objects for TPDO 2 +ObjectType=0x09 + +[2201sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=1 + +[2201sub1] +ParameterName=proxy_device_2: UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[5800] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5801] +ParameterName=Remote TPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5A00] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A00Value] +NrOfEntries=1 +1=0x40010020 + +[5A01] +ParameterName=Remote TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5A01Value] +NrOfEntries=1 +1=0x40010020 + +[5C00] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000102 + +[5C01] +ParameterName=Remote RPDO number and node-ID +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000103 + +[5E00] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E00Value] +NrOfEntries=1 +1=0x40000020 + +[5E01] +ParameterName=Remote RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[5E01Value] +NrOfEntries=1 +1=0x40000020 diff --git a/canopen_proxy_driver/test/test_driver_component.cpp b/canopen_proxy_driver/test/test_driver_component.cpp new file mode 100644 index 00000000..5d18ad7d --- /dev/null +++ b/canopen_proxy_driver/test/test_driver_component.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include +#include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" +#include "gtest/gtest.h" +using namespace rclcpp_components; + +TEST(ComponentLoad, test_load_component_1) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_proxy_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[0]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} + +TEST(ComponentLoad, test_load_component_2) +{ + rclcpp::init(0, nullptr); + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + std::vector resources = + manager->get_component_resources("canopen_proxy_driver"); + + EXPECT_EQ(2u, resources.size()); + + auto factory = manager->create_component_factory(resources[1]); + auto instance_wrapper = + factory->create_node_instance(rclcpp::NodeOptions().use_global_arguments(false)); + + rclcpp::shutdown(); +} diff --git a/canopen_proxy_driver/test/test_node_interface.cpp b/canopen_proxy_driver/test/test_node_interface.cpp new file mode 100644 index 00000000..4f285262 --- /dev/null +++ b/canopen_proxy_driver/test/test_node_interface.cpp @@ -0,0 +1,74 @@ +#include +#include +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" +#include "gtest/gtest.h" + +TEST(NodeCanopenProxyDriver, test_good_sequence_advanced) +{ + rclcpp::init(0, nullptr); + rclcpp::Node * node = new rclcpp::Node("Node"); + auto interface = new ros2_canopen::node_interfaces::NodeCanopenProxyDriver(node); + auto exec = std::make_shared(); + exec->add_node(node->get_node_base_interface()); + std::thread spinner = std::thread([exec] { exec->spin(); }); + + auto iface = static_cast(interface); + + EXPECT_NO_THROW(iface->init()); + + rclcpp::Parameter container_name("container_name", "none"); + rclcpp::Parameter node_id("node_id", 1); + rclcpp::Parameter timeout("non_transmit_timeout", 100); + rclcpp::Parameter config( + "config", + "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " + "\"simple.eds\"\ndcf_path: \"\"\n"); + node->set_parameter(container_name); + node->set_parameter(node_id); + node->set_parameter(timeout); + node->set_parameter(config); + EXPECT_NO_THROW(iface->configure()); + // Can't activate as master cannot be set. + EXPECT_ANY_THROW(iface->activate()); + rclcpp::shutdown(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (spinner.joinable()) + { + spinner.join(); + } +} + +TEST(NodeCanopenBasicLifecycleMaster, test_good_sequence_advanced) +{ + rclcpp::init(0, nullptr); + rclcpp_lifecycle::LifecycleNode * node = new rclcpp_lifecycle::LifecycleNode("Node"); + auto interface = new ros2_canopen::node_interfaces::NodeCanopenProxyDriver(node); + auto exec = std::make_shared(); + exec->add_node(node->get_node_base_interface()); + std::thread spinner = std::thread([exec] { exec->spin(); }); + + auto iface = static_cast(interface); + + EXPECT_NO_THROW(iface->init()); + + rclcpp::Parameter container_name("container_name", "none"); + rclcpp::Parameter node_id("node_id", 1); + rclcpp::Parameter timeout("non_transmit_timeout", 100); + rclcpp::Parameter config( + "config", + "node_id: 1\ndriver: \"ros2_canopen::CanopenDriver\"\npackage: \"canopen_core\"\ndcf: " + "\"simple.eds\"\ndcf_path: \"\"\n"); + node->set_parameter(container_name); + node->set_parameter(node_id); + node->set_parameter(timeout); + node->set_parameter(config); + EXPECT_NO_THROW(iface->configure()); + // Can't activate as master cannot be set. + EXPECT_ANY_THROW(iface->activate()); + rclcpp::shutdown(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (spinner.joinable()) + { + spinner.join(); + } +} diff --git a/canopen_ros2_control/CMakeLists.txt b/canopen_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..d0f0230e --- /dev/null +++ b/canopen_ros2_control/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.8) +project(canopen_ros2_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + canopen_402_driver + canopen_core + canopen_proxy_driver + hardware_interface + pluginlib + rclcpp + rclcpp_components + rclcpp_lifecycle +) + +find_package(ros2_control_test_assets) +find_package(ament_cmake_gmock REQUIRED) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library( + canopen_ros2_control + SHARED + src/canopen_system.cpp + src/cia402_system.cpp + src/robot_system.cpp +) + + + +target_compile_features(canopen_ros2_control PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_options(canopen_ros2_control PUBLIC -fPIC -Wl,--no-undefined) + +target_include_directories(canopen_ros2_control PUBLIC include) +ament_target_dependencies(canopen_ros2_control ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# prevent pluginlib from using boost +target_compile_definitions(canopen_ros2_control PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(hardware_interface canopen_ros2_control.xml) + +install( + TARGETS + canopen_ros2_control + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + DIRECTORY launch urdf config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + #find_package(ament_cmake_gmock REQUIRED) + #find_package(ros2_control_test_assets REQUIRED) + + #ament_add_gmock(test_canopen_system test/test_canopen_system.cpp) + #target_include_directories(test_canopen_system PRIVATE include) + #ament_target_dependencies( + #test_canopen_system ${THIS_PACKAGE_INCLUDE_DEPENDS} ros2_control_test_assets) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + canopen_ros2_control +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +ament_package() diff --git a/canopen_ros2_control/canopen_ros2_control.xml b/canopen_ros2_control/canopen_ros2_control.xml new file mode 100644 index 00000000..d6aed6d8 --- /dev/null +++ b/canopen_ros2_control/canopen_ros2_control.xml @@ -0,0 +1,23 @@ + + + + ros2_control hardware interface. + + + + + ros2_control hardware interface. + + + + + ros2_control hardware interface. + + + diff --git a/canopen_ros2_control/config/cia402_ros2_control.yaml b/canopen_ros2_control/config/cia402_ros2_control.yaml new file mode 100644 index 00000000..1b36e95f --- /dev/null +++ b/canopen_ros2_control/config/cia402_ros2_control.yaml @@ -0,0 +1,42 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + cia402_device_1_controller: + type: canopen_ros2_controllers/Cia402DeviceController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + +cia402_device_1_controller: + ros__parameters: + joint: node_1 + +forward_position_controller: + ros__parameters: + joints: + - node_1 + interface_name: position + +joint_trajectory_controller: + ros__parameters: + joints: + - node_1 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + node_1: { trajectory: 0.2, goal: 0.1 } diff --git a/canopen_ros2_control/config/ros2_control.yaml b/canopen_ros2_control/config/ros2_control.yaml new file mode 100644 index 00000000..771b5f9c --- /dev/null +++ b/canopen_ros2_control/config/ros2_control.yaml @@ -0,0 +1,13 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + node_1_controller: + type: canopen_ros2_controllers/CanopenProxyController + +node_1_controller: + ros__parameters: + joint: node_1 diff --git a/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp new file mode 100644 index 00000000..f2aa4fab --- /dev/null +++ b/canopen_ros2_control/include/canopen_ros2_control/canopen_system.hpp @@ -0,0 +1,208 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2022-06-29 + * + */ +//---------------------------------------------------------------------- + +#ifndef CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ +#define CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ + +#include +#include + +#include "canopen_core/device_container.hpp" +#include "canopen_proxy_driver/proxy_driver.hpp" +#include "canopen_ros2_control/visibility_control.h" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace canopen_ros2_control +{ +// needed auxiliary struct for ros2 control double registration +struct Ros2ControlCOData +{ + ros2_canopen::COData original_data; + + double index; // cast to uint16_t + double subindex; // cast to uint8_t + double data; // cast to uint32_t +}; + +struct RORos2ControlCOData : public Ros2ControlCOData +{ + void set_data(ros2_canopen::COData d) + { + original_data = d; + + index = static_cast(original_data.index_); + subindex = static_cast(original_data.subindex_); + data = static_cast(original_data.data_); + } +}; + +struct WORos2ControlCoData : public Ros2ControlCOData +{ + WORos2ControlCoData() : one_shot(std::numeric_limits::quiet_NaN()) {} + + // needed internally for write-only data + double one_shot; + + bool write_command() + { + bool ret_val; + // store ret value + ret_val = !std::isnan(one_shot); + // reset the existing active command if one exists + one_shot = std::numeric_limits::quiet_NaN(); + return ret_val; + } + + void prepare_data() + { + original_data.index_ = static_cast(index); + original_data.subindex_ = static_cast(subindex); + original_data.data_ = static_cast(data); + } +}; +struct Ros2ControlNmtState +{ + Ros2ControlNmtState() + : reset_ons(std::numeric_limits::quiet_NaN()), + reset_fbk(std::numeric_limits::quiet_NaN()), + start_ons(std::numeric_limits::quiet_NaN()), + start_fbk(std::numeric_limits::quiet_NaN()) + { + } + + void set_state(canopen::NmtState s) + { + original_state = s; + state = static_cast(s); + } + + bool reset_command() + { + bool ret_val; + // store ret value + ret_val = !std::isnan(reset_ons); + // reset the existing active command if one exists + reset_ons = std::numeric_limits::quiet_NaN(); + return ret_val; + } + + bool start_command() + { + bool ret_val; + // store ret value + ret_val = !std::isnan(start_ons); + // reset the existing active command if one exists + start_ons = std::numeric_limits::quiet_NaN(); + return ret_val; + } + canopen::NmtState original_state; + + double state; // read-only + + // basic commands + double reset_ons; // write-only + double reset_fbk; + double start_ons; // write-only + double start_fbk; +}; + +struct CanopenNodeData +{ + Ros2ControlNmtState nmt_state; // read-write + RORos2ControlCOData rpdo_data; // read-only + WORos2ControlCoData tpdo_data; // write-only + + WORos2ControlCoData rsdo; // write-only + WORos2ControlCoData wsdo; // write-only +}; + +class CanopenSystem : public hardware_interface::SystemInterface +{ +public: + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + CanopenSystem(); + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + ~CanopenSystem(); + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::shared_ptr device_container_; + std::shared_ptr executor_; + // can stuff + std::map canopen_data_; + // threads + std::unique_ptr spin_thread_; + std::unique_ptr init_thread_; + + void spin(); + void clean(); + +private: + void initDeviceContainer(); +}; + +} // namespace canopen_ros2_control + +#endif // CANOPEN_ROS2_CONTROL__CANOPEN_SYSTEM_HPP_ diff --git a/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp new file mode 100644 index 00000000..3615caf2 --- /dev/null +++ b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp @@ -0,0 +1,209 @@ +// Copyright (c) 2023, Fraunhofer IPA +// Copyright (c) 2022, StoglRobotics +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROL__CIA402_DATA_HPP_ +#define CANOPEN_ROS2_CONTROL__CIA402_DATA_HPP_ +#include +#include +#include +#include +#include "canopen_402_driver/base.hpp" +#include "canopen_402_driver/cia402_driver.hpp" +#include "canopen_ros2_control/helpers.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using namespace ros2_canopen; +namespace canopen_ros2_control +{ +struct Cia402Data +{ + uint8_t node_id; + std::string joint_name; + std::shared_ptr driver; + std::map command_interface_to_operation_mode; + std::vector interfaces; + std::vector interfaces_to_start; + std::vector interfaces_to_running; + std::vector interfaces_to_stop; + YAML::Node config; + + // FROM MOTOR + double actual_position = std::numeric_limits::quiet_NaN(); + double actual_velocity = std::numeric_limits::quiet_NaN(); + + // TO MOTOR + double target_position = std::numeric_limits::quiet_NaN(); + double target_velocity = std::numeric_limits::quiet_NaN(); + double target_torque = std::numeric_limits::quiet_NaN(); + + bool init_data(hardware_interface::ComponentInfo & joint, std::string device_dump) + { + joint_name = joint.name; + auto config = YAML::Load(device_dump); + + if (!config["node_id"]) + { + RCLCPP_ERROR(rclcpp::get_logger(joint_name), "No node id for '%s'", joint.name.c_str()); + return false; + } + + node_id = config["node_id"].as(); + RCLCPP_ERROR( + rclcpp::get_logger(joint_name), "Node id for '%s' is '%u'", joint.name.c_str(), node_id); + + if (config["position_mode"]) + { + auto position_mode = + (ros2_canopen::MotorBase::OperationMode)config["position_mode"].as(); + RCLCPP_INFO( + rclcpp::get_logger(joint_name), "Registered position_mode '%u' for '%s'", position_mode, + joint.name.c_str()); + command_interface_to_operation_mode.emplace( + std::pair(joint.name + "/" + "position", position_mode)); + } + if (config["velocity_mode"]) + { + auto velocity_mode = + (ros2_canopen::MotorBase::OperationMode)config["velocity_mode"].as(); + RCLCPP_INFO( + rclcpp::get_logger(joint_name), "Registered velocity_mode '%u' for '%s'", velocity_mode, + joint.name.c_str()); + command_interface_to_operation_mode.emplace( + std::pair(joint.name + "/" + "velocity", velocity_mode)); + } + if (config["effort_mode"]) + { + auto effort_mode = (ros2_canopen::MotorBase::OperationMode)config["effort_mode"].as(); + RCLCPP_INFO( + rclcpp::get_logger(joint_name), "Registered effort_mode '%u' for '%s'", effort_mode, + joint.name.c_str()); + command_interface_to_operation_mode.emplace( + std::pair(joint.name + "/" + "effort", effort_mode)); + } + return true; + } + + void export_state_interface(std::vector & state_interfaces) + { + // actual position + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_name, hardware_interface::HW_IF_POSITION, &actual_position)); + + // actual speed + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_name, hardware_interface::HW_IF_VELOCITY, &actual_velocity)); + } + + void export_command_interface( + std::vector & command_interfaces) + { + if ( + command_interface_to_operation_mode.find(joint_name + "/" + "position") != + command_interface_to_operation_mode.end()) + { + // target position + command_interfaces.emplace_back( + joint_name, hardware_interface::HW_IF_POSITION, &target_position); + interfaces.push_back(joint_name + "/" + "position"); + } + if ( + command_interface_to_operation_mode.find(joint_name + "/" + "velocity") != + command_interface_to_operation_mode.end()) + { + // target velocity + command_interfaces.emplace_back( + joint_name, hardware_interface::HW_IF_VELOCITY, &target_velocity); + interfaces.push_back(joint_name + "/" + "velocity"); + } + if ( + command_interface_to_operation_mode.find(joint_name + "/" + "effort") != + command_interface_to_operation_mode.end()) + { + // target effort + command_interfaces.emplace_back( + joint_name, hardware_interface::HW_IF_EFFORT, &target_position); + interfaces.push_back(joint_name + "/" + "effort"); + } + } + + void read_state() + { + actual_position = driver->get_position(); + actual_velocity = driver->get_speed(); + } + + void write_target() + { + const uint16_t & mode = driver->get_mode(); + switch (mode) + { + case MotorBase::No_Mode: + break; + case MotorBase::Profiled_Position: + case MotorBase::Cyclic_Synchronous_Position: + case MotorBase::Interpolated_Position: + driver->set_target(target_position); + break; + case MotorBase::Profiled_Velocity: + case MotorBase::Cyclic_Synchronous_Velocity: + driver->set_target(target_velocity); + break; + case MotorBase::Profiled_Torque: + driver->set_target(target_torque); + break; + default: + RCLCPP_INFO(rclcpp::get_logger("robot_system_interface"), "Mode not supported"); + } + } + + bool perform_switch() + { + if (interfaces_to_start.size() > 1) + { + RCLCPP_ERROR( + rclcpp::get_logger(joint_name), + "Trying to start multiple command interfaces at once for joint '%s'", joint_name.c_str()); + return false; + } + if (interfaces_to_start.size() == 0) + { + return true; + } + auto mode = command_interface_to_operation_mode[interfaces_to_start[0]]; + interfaces_to_running.clear(); + interfaces_to_running.push_back(interfaces_to_start[0]); + interfaces_to_stop.clear(); + interfaces_to_start.clear(); + RCLCPP_INFO( + rclcpp::get_logger(joint_name), + "Switching to '%s' command mode with CIA402 operation mode '%u'", + interfaces_to_running[0].c_str(), mode); + return driver->set_operation_mode(mode); + } + + bool check_id(uint8_t id) + { + if (node_id == id) + { + return true; + } + return false; + } +}; +} // namespace canopen_ros2_control +#endif diff --git a/canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp new file mode 100644 index 00000000..bcf7ebfa --- /dev/null +++ b/canopen_ros2_control/include/canopen_ros2_control/cia402_system.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2022, StoglRobotics +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2022-08-01 + * + */ +//---------------------------------------------------------------------- + +#ifndef CANOPEN_ROS2_CONTROL__CIA402_SYSTEM_HPP_ +#define CANOPEN_ROS2_CONTROL__CIA402_SYSTEM_HPP_ + +#include "canopen_402_driver/cia402_driver.hpp" +#include "canopen_ros2_control/canopen_system.hpp" + +constexpr double kResponseOk = 1.0; +constexpr double kResponseFail = 0.0; + +namespace canopen_ros2_control +{ + +struct MotorTriggerCommand +{ + double ons_cmd{std::numeric_limits::quiet_NaN()}; + double resp{std::numeric_limits::quiet_NaN()}; + + bool is_commanded() + { + bool tmp = !std::isnan(ons_cmd); + ons_cmd = std::numeric_limits::quiet_NaN(); + return tmp; + } + + void set_response(bool response) { resp = response ? kResponseOk : kResponseFail; } +}; + +struct MotorTarget : public MotorTriggerCommand +{ + double position_value; + double velocity_value; + double torque_value; +}; + +struct MotorNodeData +{ + // feedback + double actual_position; + double actual_speed; + + // basic control + MotorTriggerCommand init; + MotorTriggerCommand halt; + MotorTriggerCommand recover; + + // mode control + MotorTriggerCommand position_mode; + MotorTriggerCommand velocity_mode; + MotorTriggerCommand cyclic_velocity_mode; + MotorTriggerCommand cyclic_position_mode; + MotorTriggerCommand torque_mode; + MotorTriggerCommand interpolated_position_mode; + + // setpoint + MotorTarget target; +}; + +using namespace ros2_canopen; +class Cia402System : public CanopenSystem +{ +public: + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + Cia402System(); + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + ~Cia402System() = default; + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info); + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state); + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces(); + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces(); + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state); + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state); + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + +protected: + // can stuff + std::map motor_data_; + +private: + void switchModes(uint id, const std::shared_ptr & driver); + + void handleInit(uint id, const std::shared_ptr & driver); + + void handleRecover(uint id, const std::shared_ptr & driver); + + void handleHalt(uint id, const std::shared_ptr & driver); + + void initDeviceContainer(); +}; + +} // namespace canopen_ros2_control + +#endif // CANOPEN_ROS2_CONTROL__CIA402_SYSTEM_HPP_ diff --git a/canopen_ros2_control/include/canopen_ros2_control/helpers.hpp b/canopen_ros2_control/include/canopen_ros2_control/helpers.hpp new file mode 100644 index 00000000..cfd83a65 --- /dev/null +++ b/canopen_ros2_control/include/canopen_ros2_control/helpers.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2023, Fraunhofer IPA +// Copyright (c) 2022, StoglRobotics +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROL__HELPER_HPP_ +#define CANOPEN_ROS2_CONTROL__HELPER_HPP_ + +#include +#include +#include +#include + +namespace canopen_ros2_control +{ +/** + * @brief Struct for storing the data necessary for a triggering command. + * + * @details + * A trigger command from a controller will write to the command double. + * The result of the trigger operation will be written to the response double. + * A result of 1.0 means the operation was successful. + * A result of 0.0 means the operation failed. + * Only a trigger command of 1.0 will be accepted and trigger the operation. + * The command_available function can be used to check if a command is available, + * undefined commands (other than 1.0) will be ignored. + * The set_response function should be used to set the response value. + * It will then clear the command. + * + */ +struct TriggerCommand +{ + double command = std::numeric_limits::quiet_NaN(); + double response = std::numeric_limits::quiet_NaN(); + std::function trigger_function; + + bool try_trigger() + { + if (command == 1.0) + { + response = trigger_function() ? 1.0 : 0.0; + command = std::numeric_limits::quiet_NaN(); + return true; + } + command = std::numeric_limits::quiet_NaN(); + return false; + } +}; + +} // namespace canopen_ros2_control +#endif diff --git a/canopen_ros2_control/include/canopen_ros2_control/robot_system.hpp b/canopen_ros2_control/include/canopen_ros2_control/robot_system.hpp new file mode 100644 index 00000000..a0d0d21b --- /dev/null +++ b/canopen_ros2_control/include/canopen_ros2_control/robot_system.hpp @@ -0,0 +1,191 @@ +// Copyright (c) 2023, Fraunhofer IPA +// Copyright (c) 2022, StoglRobotics +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CANOPEN_ROS2_CONTROL__ROBOT_SYSTEM_HPP_ +#define CANOPEN_ROS2_CONTROL__ROBOT_SYSTEM_HPP_ + +#include "canopen_402_driver/cia402_driver.hpp" +#include "canopen_core/configuration_manager.hpp" +#include "canopen_core/device_container.hpp" +#include "canopen_ros2_control/cia402_data.hpp" +#include "canopen_ros2_control/visibility_control.h" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" +namespace canopen_ros2_control +{ +class RobotSystem : public hardware_interface::SystemInterface +{ +public: + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + RobotSystem() : hardware_interface::SystemInterface() {} + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + ~RobotSystem() = default; + + /** + * @brief Initialize the hardware interface + * + * Fetch the hardware info stored in the urdf. + * Specifically these values: + * - bus_config + * - master_config + * - can_interface_name + * - master_bin + * + * @param info + * @return hardware_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + /** + * @brief Configure the hardware interface + * + * Create the device container and the executor. + * Start the threads for running the canopen stack. + * + * @param info + * @return hardware_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Export the state interfaces for this system. + * + * The state interface of each cia402 device contains the following: + * - Position + * - Velocity + * + * @return std::vector + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() override; + + /** + * @brief Export the command interfaces for this system. + * + * The command interface of each cia402 device contains the following: + * - Position + * - Velocity + * - Effort + * - Operation Mode + * - Init Command + * - Halt Command + * - Recover Command + * + * @return std::vector + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() override; + + /** + * @brief Cleanup the hardware interface + * + * @param previous_state + * @return hardware_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Shutdown the hardware interface + * + * @param previous_state + * @return hardware_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Activate the hardware interface + * + * @param previous_state + * @return hardware_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Deactivate the hardware interface + * + * @param previous_state + * @return hardware_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Read the state from the hardware interface + * + * @return hardware_interface::return_type + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + /** + * @brief Write the command to the hardware interface + * + * @return hardware_interface::return_type + */ + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + +protected: + std::shared_ptr device_container_; + std::shared_ptr executor_; + std::vector robot_motor_data_; + std::string bus_config_; + std::string master_config_; + std::string master_bin_; + std::string can_interface_; + std::unique_ptr spin_thread_; + std::unique_ptr init_thread_; + rclcpp::Logger robot_system_logger = rclcpp::get_logger("robot_system"); + +private: + /** + * @brief Spins the ROS executor + * + */ + void spin(); + void clean(); + + /** + * @brief Initialize the device container + * + */ + void initDeviceContainer(); +}; +} // namespace canopen_ros2_control + +#endif diff --git a/canopen_ros2_control/include/canopen_ros2_control/visibility_control.h b/canopen_ros2_control/include/canopen_ros2_control/visibility_control.h new file mode 100644 index 00000000..28edd11d --- /dev/null +++ b/canopen_ros2_control/include/canopen_ros2_control/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROL__VISIBILITY_CONTROL_H_ +#define CANOPEN_ROS2_CONTROL__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef CANOPEN_ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC +#define CANOPEN_ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define CANOPEN_ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define CANOPEN_ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define CANOPEN_ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC +#define CANOPEN_ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define CANOPEN_ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // CANOPEN_ROS2_CONTROL__VISIBILITY_CONTROL_H_ diff --git a/canopen_ros2_control/launch/canopen_system.launch.py b/canopen_ros2_control/launch/canopen_system.launch.py new file mode 100644 index 00000000..7a0eabd7 --- /dev/null +++ b/canopen_ros2_control/launch/canopen_system.launch.py @@ -0,0 +1,277 @@ +# Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Lovro Ivanov lovro.ivanov@gmail.com + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def launch_setup(context, *args, **kwargs): + + name = LaunchConfiguration("name") + prefix = LaunchConfiguration("prefix") + + # bus configuration + bus_config_package = LaunchConfiguration("bus_config_package") + bus_config_directory = LaunchConfiguration("bus_config_directory") + bus_config_file = LaunchConfiguration("bus_config_file") + # bus configuration file full path + bus_config = PathJoinSubstitution( + [FindPackageShare(bus_config_package), bus_config_directory, bus_config_file] + ) + + # master configuration + master_config_package = LaunchConfiguration("master_config_package") + master_config_directory = LaunchConfiguration("master_config_directory") + master_config_file = LaunchConfiguration("master_config_file") + # master configuration file full path + master_config = PathJoinSubstitution( + [FindPackageShare(master_config_package), master_config_directory, master_config_file] + ) + + # can interface name + can_interface_name = LaunchConfiguration("can_interface_name") + + # robot description stuff + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "name:=", + name, + " ", + "prefix:=", + prefix, + " ", + "bus_config:=", + bus_config, + " ", + "master_config:=", + master_config, + " ", + "can_interface_name:=", + can_interface_name, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + # ros2 control configuration + ros2_control_config_package = LaunchConfiguration("ros2_control_config_package") + ros2_control_config_directory = LaunchConfiguration("ros2_control_config_directory") + ros2_control_config_file = LaunchConfiguration("ros2_control_config_file") + # ros2 control configuration file full path + ros2_control_config = PathJoinSubstitution( + [ + FindPackageShare(ros2_control_config_package), + ros2_control_config_directory, + ros2_control_config_file, + ] + ) + + # nodes to start are listed below + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_control_config], + output="screen", + ) + + # load one controller just to make sure it can connect to controller_manager + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + canopen_proxy_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["node_1_controller", "--controller-manager", "/controller_manager"], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # hardcoded slave configuration form test package + slave_config = PathJoinSubstitution( + [FindPackageShare(master_config_package), master_config_directory, "simple.eds"] + ) + + slave_launch = PathJoinSubstitution( + [FindPackageShare("canopen_fake_slaves"), "launch", "basic_slave.launch.py"] + ) + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slave_launch), + launch_arguments={ + "node_id": "2", + "node_name": "slave_node_2", + "slave_config": slave_config, + }.items(), + ) + + slave_node_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slave_launch), + launch_arguments={ + "node_id": "3", + "node_name": "slave_node_3", + "slave_config": slave_config, + }.items(), + ) + + nodes_to_start = [ + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + slave_node_1, + slave_node_2, + canopen_proxy_controller_spawner, + ] + + return nodes_to_start + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "name", description="robot name", default_value="canopen_test_system" + ) + ) + declared_arguments.append( + DeclareLaunchArgument("prefix", description="Prefix.", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Package where urdf file is stored.", + default_value="canopen_ros2_control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + description="Name of the urdf file.", + default_value="canopen_system.urdf.xacro", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_package", + default_value="canopen_ros2_control", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_directory", + default_value="config", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_file", + default_value="ros2_control.yaml", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_package", + default_value="canopen_tests", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_directory", + default_value="config/simple", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_file", + default_value="bus.yml", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_package", + default_value="canopen_tests", + description="Path to master configuration file (*.dcf)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_directory", + default_value="config/simple", + description="Path to master configuration file (*.dcf)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_file", + default_value="master.dcf", + description="Path to master configuration file (*.dcf)", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "can_interface_name", + default_value="vcan0", + description="Interface name for can", + ) + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/canopen_ros2_control/launch/cia402_system.launch.py b/canopen_ros2_control/launch/cia402_system.launch.py new file mode 100644 index 00000000..edb7d9c0 --- /dev/null +++ b/canopen_ros2_control/launch/cia402_system.launch.py @@ -0,0 +1,274 @@ +# Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Lovro Ivanov lovro.ivanov@gmail.com + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def launch_setup(context, *args, **kwargs): + + name = LaunchConfiguration("name") + prefix = LaunchConfiguration("prefix") + + # bus configuration + bus_config_package = LaunchConfiguration("bus_config_package") + bus_config_directory = LaunchConfiguration("bus_config_directory") + bus_config_file = LaunchConfiguration("bus_config_file") + # bus configuration file full path + bus_config = PathJoinSubstitution( + [FindPackageShare(bus_config_package), bus_config_directory, bus_config_file] + ) + + # master configuration + master_config_package = LaunchConfiguration("master_config_package") + master_config_directory = LaunchConfiguration("master_config_directory") + master_config_file = LaunchConfiguration("master_config_file") + # master configuration file full path + master_config = PathJoinSubstitution( + [FindPackageShare(master_config_package), master_config_directory, master_config_file] + ) + + # can interface name + can_interface = LaunchConfiguration("can_interface") + + # robot description stuff + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "name:=", + name, + " ", + "prefix:=", + prefix, + " ", + "bus_config:=", + bus_config, + " ", + "master_config:=", + master_config, + " ", + "can_interface:=", + can_interface, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + # ros2 control configuration + ros2_control_config_package = LaunchConfiguration("ros2_control_config_package") + ros2_control_config_directory = LaunchConfiguration("ros2_control_config_directory") + ros2_control_config_file = LaunchConfiguration("ros2_control_config_file") + # ros2 control configuration file full path + ros2_control_config = PathJoinSubstitution( + [ + FindPackageShare(ros2_control_config_package), + ros2_control_config_directory, + ros2_control_config_file, + ] + ) + + # nodes to start are listed below + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_control_config], + output="screen", + ) + + # load one controller just to make sure it can connect to controller_manager + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + cia402_device_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["cia402_device_1_controller", "--controller-manager", "/controller_manager"], + ) + + forward_position_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # hardcoded slave configuration form test package + slave_config = PathJoinSubstitution( + [FindPackageShare("canopen_tests"), "config/cia402", "cia402_slave.eds"] + ) + + slave_launch = PathJoinSubstitution( + [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"] + ) + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slave_launch), + launch_arguments={ + "node_id": "2", + "node_name": "cia402_node_1", + "slave_config": slave_config, + }.items(), + ) + + nodes_to_start = [ + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + slave_node_1, + cia402_device_controller_spawner, + forward_position_controller, + ] + + return nodes_to_start + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "name", description="robot name", default_value="canopen_test_system" + ) + ) + declared_arguments.append( + DeclareLaunchArgument("prefix", description="Prefix.", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Package where urdf file is stored.", + default_value="canopen_ros2_control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + description="Name of the urdf file.", + default_value="cia402_system.urdf.xacro", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_package", + default_value="canopen_ros2_control", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_directory", + default_value="config", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_control_config_file", + default_value="cia402_ros2_control.yaml", + description="Path to ros2_control configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_package", + default_value="canopen_tests", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_directory", + default_value="config/cia402", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "bus_config_file", + default_value="bus.yml", + description="Path to bus configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_package", + default_value="canopen_tests", + description="Path to master configuration file (*.dcf)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_directory", + default_value="config/cia402", + description="Path to master configuration file (*.dcf)", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "master_config_file", + default_value="master.dcf", + description="Path to master configuration file (*.dcf)", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "can_interface", + default_value="vcan0", + description="Interface name for can", + ) + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml new file mode 100644 index 00000000..fcb9f890 --- /dev/null +++ b/canopen_ros2_control/package.xml @@ -0,0 +1,41 @@ + + + + canopen_ros2_control + 0.0.1 + ros2_control wrapper for ros2_canopen functionalities + Lovro Ivanov + Denis Stogl + + Apache-2.0 + + ament_cmake + + canopen_402_driver + canopen_core + canopen_proxy_driver + hardware_interface + pluginlib + rclcpp + rclcpp_components + rclcpp_lifecycle + + xacro + controller_manager + robot_state_publisher + + joint_state_broadcaster + joint_trajectory_controller + forward_command_controller + + canopen_fake_slaves + canopen_tests + canopen_ros2_controllers + + ament_cmake_gmock + ros2_control_test_assets + + + ament_cmake + + diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp new file mode 100644 index 00000000..eed266a9 --- /dev/null +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -0,0 +1,290 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2022-06-29 + * + */ +//---------------------------------------------------------------------- + +#include "canopen_ros2_control/canopen_system.hpp" + +#include +#include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +auto const kLogger = rclcpp::get_logger("CanopenSystem"); +} + +namespace canopen_ros2_control +{ + +CanopenSystem::CanopenSystem() {} + +void CanopenSystem::clean() +{ + executor_->cancel(); + printf("Joining..."); + spin_thread_->join(); + printf("Joined!"); + + device_container_.reset(); + executor_.reset(); + + init_thread_->join(); + init_thread_.reset(); + + executor_.reset(); + spin_thread_.reset(); +} + +CanopenSystem::~CanopenSystem() { clean(); } + +hardware_interface::CallbackReturn CanopenSystem::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + RCLCPP_INFO(kLogger, "bus_config: '%s'", info_.hardware_parameters["bus_config"].c_str()); + RCLCPP_INFO(kLogger, "master_config: '%s'", info_.hardware_parameters["master_config"].c_str()); + RCLCPP_INFO( + kLogger, "can_interface_name: '%s'", info_.hardware_parameters["can_interface_name"].c_str()); + RCLCPP_INFO(kLogger, "master_bin: '%s'", info_.hardware_parameters["master_bin"].c_str()); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CanopenSystem::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + executor_ = std::make_shared(); + device_container_ = std::make_shared(executor_); + executor_->add_node(device_container_); + + // threads + spin_thread_ = std::make_unique(&CanopenSystem::spin, this); + init_thread_ = std::make_unique(&CanopenSystem::initDeviceContainer, this); + + // actually wait for init phase to end + if (init_thread_->joinable()) + { + init_thread_->join(); + } + else + { + RCLCPP_ERROR(kLogger, "Could not join init thread!"); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CanopenSystem::on_cleanup( + const rclcpp_lifecycle::State & previous_state) +{ + clean(); + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CanopenSystem::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + clean(); + return CallbackReturn::SUCCESS; +} + +void CanopenSystem::spin() +{ + executor_->spin(); + executor_->remove_node(device_container_); + + RCLCPP_INFO(kLogger, "Exiting spin thread..."); +} + +void CanopenSystem::initDeviceContainer() +{ + std::string tmp_master_bin = (info_.hardware_parameters["master_bin"] == "\"\"") + ? "" + : info_.hardware_parameters["master_bin"]; + + device_container_->init( + info_.hardware_parameters["can_interface_name"], info_.hardware_parameters["master_config"], + info_.hardware_parameters["bus_config"], tmp_master_bin); + auto drivers = device_container_->get_registered_drivers(); + RCLCPP_INFO(kLogger, "Number of registered drivers: '%zu'", device_container_->count_drivers()); + for (auto it = drivers.begin(); it != drivers.end(); it++) + { + auto proxy_driver = std::static_pointer_cast(it->second); + + auto nmt_state_cb = [&](canopen::NmtState nmt_state, uint8_t id) + { canopen_data_[id].nmt_state.set_state(nmt_state); }; + // register callback + proxy_driver->register_nmt_state_cb(nmt_state_cb); + + auto rpdo_cb = [&](ros2_canopen::COData data, uint8_t id) + { canopen_data_[id].rpdo_data.set_data(data); }; + // register callback + proxy_driver->register_rpdo_cb(rpdo_cb); + + RCLCPP_INFO( + kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%u'", + it->second->get_node_base_interface()->get_name(), it->first); + } + + RCLCPP_INFO(device_container_->get_logger(), "Initialisation successful."); +} + +std::vector CanopenSystem::export_state_interfaces() +{ + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) + { + // skip adding canopen interfaces + continue; + } + const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); + // RCLCPP_INFO(kLogger, "node id on export state interface for joint: '%s' is '%s'", + // info_.joints[i].name.c_str(), info_.joints[i].parameters["node_id"].c_str()); + + // rpdo index + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "rpdo/index", &canopen_data_[node_id].rpdo_data.index)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "rpdo/subindex", &canopen_data_[node_id].rpdo_data.subindex)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "rpdo/data", &canopen_data_[node_id].rpdo_data.data)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "nmt/state", &canopen_data_[node_id].nmt_state.state)); + } + + return state_interfaces; +} + +std::vector CanopenSystem::export_command_interfaces() +{ + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) + { + // skip adding canopen interfaces + continue; + } + + const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "tpdo/index", &canopen_data_[node_id].tpdo_data.index)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "tpdo/subindex", &canopen_data_[node_id].tpdo_data.subindex)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "tpdo/data", &canopen_data_[node_id].tpdo_data.data)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "tpdo/owns", &canopen_data_[node_id].tpdo_data.one_shot)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "nmt/reset", &canopen_data_[node_id].nmt_state.reset_ons)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "nmt/reset_fbk", &canopen_data_[node_id].nmt_state.reset_fbk)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "nmt/start", &canopen_data_[node_id].nmt_state.start_ons)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "nmt/start_fbk", &canopen_data_[node_id].nmt_state.start_fbk)); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn CanopenSystem::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to receive commands + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CanopenSystem::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to stop receiving commands + + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type CanopenSystem::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // TODO(anyone): read robot states + + // nmt state is set via Ros2ControlNmtState::set_state within nmt_state_cb + + // rpdo is set via RORos2ControlCOData::set_data within rpdo_cb + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type CanopenSystem::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // TODO(anyone): write robot's commands' + auto drivers = device_container_->get_registered_drivers(); + for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) + { + auto proxy_driver = std::static_pointer_cast(drivers[it->first]); + + // reset node nmt + if (it->second.nmt_state.reset_command()) + { + it->second.nmt_state.reset_fbk = static_cast(proxy_driver->reset_node_nmt_command()); + } + + // start nmt + if (it->second.nmt_state.start_command()) + { + it->second.nmt_state.start_fbk = static_cast(proxy_driver->start_node_nmt_command()); + } + + // tpdo data one shot mechanism + if (it->second.tpdo_data.write_command()) + { + it->second.tpdo_data.prepare_data(); + proxy_driver->tpdo_transmit(it->second.tpdo_data.original_data); + } + } + + return hardware_interface::return_type::OK; +} + +} // namespace canopen_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(canopen_ros2_control::CanopenSystem, hardware_interface::SystemInterface) diff --git a/canopen_ros2_control/src/cia402_system.cpp b/canopen_ros2_control/src/cia402_system.cpp new file mode 100644 index 00000000..de6f789e --- /dev/null +++ b/canopen_ros2_control/src/cia402_system.cpp @@ -0,0 +1,391 @@ +// Copyright (c) 2022, StoglRobotics +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2022-08-01 + * + */ +//---------------------------------------------------------------------- + +#include "canopen_ros2_control/cia402_system.hpp" +#include + +namespace +{ +auto const kLogger = rclcpp::get_logger("Cia402System"); +} + +namespace canopen_ros2_control +{ + +Cia402System::Cia402System() : CanopenSystem() {} + +hardware_interface::CallbackReturn Cia402System::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (CanopenSystem::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +void Cia402System::initDeviceContainer() +{ + std::string tmp_master_bin = (info_.hardware_parameters["master_bin"] == "\"\"") + ? "" + : info_.hardware_parameters["master_bin"]; + + device_container_->init( + info_.hardware_parameters["can_interface"], info_.hardware_parameters["master_config"], + info_.hardware_parameters["bus_config"], tmp_master_bin); + auto drivers = device_container_->get_registered_drivers(); + RCLCPP_INFO(kLogger, "Number of registered drivers: '%lu'", device_container_->count_drivers()); + for (auto it = drivers.begin(); it != drivers.end(); it++) + { + auto driver = std::static_pointer_cast(it->second); + + auto nmt_state_cb = [&](canopen::NmtState nmt_state, uint8_t id) + { canopen_data_[id].nmt_state.set_state(nmt_state); }; + // register callback + driver->register_nmt_state_cb(nmt_state_cb); + + auto rpdo_cb = [&](ros2_canopen::COData data, uint8_t id) + { canopen_data_[id].rpdo_data.set_data(data); }; + // register callback + driver->register_rpdo_cb(rpdo_cb); + + RCLCPP_INFO( + kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%u'", + it->second->get_node_base_interface()->get_name(), it->first); + } + + RCLCPP_INFO(device_container_->get_logger(), "Initialisation successful."); +} + +hardware_interface::CallbackReturn Cia402System::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + executor_ = std::make_shared(); + device_container_ = std::make_shared(executor_); + executor_->add_node(device_container_); + + // threads + spin_thread_ = std::make_unique(&Cia402System::spin, this); + init_thread_ = std::make_unique(&Cia402System::initDeviceContainer, this); + + // actually wait for init phase to end + if (init_thread_->joinable()) + { + init_thread_->join(); + + // TODO(livanov93): see how to handle configure once LifecycleCia402Driver is introduced + /* + auto drivers = device_container_->get_registered_drivers(); + for (auto it = drivers.begin(); it != drivers.end(); it++) { + auto d = std::static_pointer_cast(it->second); + d->configure(); + } + */ + } + else + { + RCLCPP_ERROR(kLogger, "Could not join init thread!"); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +std::vector Cia402System::export_state_interfaces() +{ + std::vector state_interfaces; + + // underlying base class export first + state_interfaces = CanopenSystem::export_state_interfaces(); + + for (uint i = 0; i < info_.joints.size(); i++) + { + if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) + { + // skip adding motor canopen interfaces + continue; + } + const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); + + // actual position + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &motor_data_[node_id].actual_position)); + // actual speed + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &motor_data_[node_id].actual_speed)); + } + + return state_interfaces; +} + +std::vector Cia402System::export_command_interfaces() +{ + std::vector command_interfaces; + + // underlying base class export first + command_interfaces = CanopenSystem::export_command_interfaces(); + + for (uint i = 0; i < info_.joints.size(); i++) + { + if (info_.joints[i].parameters.find("node_id") == info_.joints[i].parameters.end()) + { + // skip adding canopen interfaces + continue; + } + + const uint8_t node_id = static_cast(std::stoi(info_.joints[i].parameters["node_id"])); + + // target + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &motor_data_[node_id].target.position_value)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &motor_data_[node_id].target.velocity_value)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, + &motor_data_[node_id].target.torque_value)); + // init + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "init_cmd", &motor_data_[node_id].init.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "init_fbk", &motor_data_[node_id].init.resp)); + + // halt + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "halt_cmd", &motor_data_[node_id].halt.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "halt_fbk", &motor_data_[node_id].halt.resp)); + + // recover + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "recover_cmd", &motor_data_[node_id].recover.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "recover_fbk", &motor_data_[node_id].recover.resp)); + + // set position mode + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "position_mode_cmd", &motor_data_[node_id].position_mode.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "position_mode_fbk", &motor_data_[node_id].position_mode.resp)); + + // set velocity mode + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "velocity_mode_cmd", &motor_data_[node_id].velocity_mode.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "velocity_mode_fbk", &motor_data_[node_id].velocity_mode.resp)); + + // set cyclic velocity mode + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "cyclic_velocity_mode_cmd", + &motor_data_[node_id].cyclic_velocity_mode.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "cyclic_velocity_mode_fbk", + &motor_data_[node_id].cyclic_velocity_mode.resp)); + // set cyclic position mode + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "cyclic_position_mode_cmd", + &motor_data_[node_id].cyclic_position_mode.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "cyclic_position_mode_fbk", + &motor_data_[node_id].cyclic_position_mode.resp)); + // set interpolated position mode + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "interpolated_position_mode_cmd", + &motor_data_[node_id].interpolated_position_mode.ons_cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "interpolated_position_mode_fbk", + &motor_data_[node_id].interpolated_position_mode.resp)); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn Cia402System::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + return CanopenSystem::on_activate(previous_state); +} + +hardware_interface::CallbackReturn Cia402System::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + return CanopenSystem::on_deactivate(previous_state); +} + +hardware_interface::return_type Cia402System::read( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // TODO(anyone): read robot states + + auto ret_val = CanopenSystem::read(time, period); + + auto drivers = device_container_->get_registered_drivers(); + + for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) + { + auto motion_controller_driver = + std::static_pointer_cast(drivers[it->first]); + // get position + motor_data_[it->first].actual_position = motion_controller_driver->get_position(); + // get speed + motor_data_[it->first].actual_speed = motion_controller_driver->get_speed(); + } + + return ret_val; +} + +hardware_interface::return_type Cia402System::write( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + auto drivers = device_container_->get_registered_drivers(); + + for (auto it = canopen_data_.begin(); it != canopen_data_.end(); ++it) + { + // TODO(livanov93): check casting + auto motion_controller_driver = + std::static_pointer_cast(drivers[it->first]); + // do same as in proxy system first - handle nmt, tpdo, rpdo + // reset node nmt + if (it->second.nmt_state.reset_command()) + { + motion_controller_driver->reset_node_nmt_command(); + } + + // start nmt + if (it->second.nmt_state.start_command()) + { + motion_controller_driver->start_node_nmt_command(); + } + + // tpdo data one shot mechanism + if (it->second.tpdo_data.write_command()) + { + it->second.tpdo_data.prepare_data(); + motion_controller_driver->tpdo_transmit(it->second.tpdo_data.original_data); + } + + // initialisation + handleInit(it->first, motion_controller_driver); + + // halt + handleHalt(it->first, motion_controller_driver); + + // recover + handleRecover(it->first, motion_controller_driver); + + // mode switching + switchModes(it->first, motion_controller_driver); + + const uint16_t & mode = motion_controller_driver->get_mode(); + + switch (mode) + { + case MotorBase::No_Mode: + break; + case MotorBase::Profiled_Position: + case MotorBase::Cyclic_Synchronous_Position: + case MotorBase::Interpolated_Position: + motion_controller_driver->set_target(motor_data_[it->first].target.position_value); + break; + case MotorBase::Profiled_Velocity: + case MotorBase::Cyclic_Synchronous_Velocity: + motion_controller_driver->set_target(motor_data_[it->first].target.velocity_value); + break; + case MotorBase::Profiled_Torque: + motion_controller_driver->set_target(motor_data_[it->first].target.torque_value); + break; + default: + RCLCPP_INFO(kLogger, "Mode %u not supported", mode); + } + } + + return hardware_interface::return_type::OK; +} + +void Cia402System::switchModes(uint id, const std::shared_ptr & driver) +{ + if (motor_data_[id].position_mode.is_commanded()) + { + motor_data_[id].position_mode.set_response(driver->set_mode_position()); + } + + if (motor_data_[id].cyclic_position_mode.is_commanded()) + { + motor_data_[id].cyclic_position_mode.set_response(driver->set_mode_cyclic_position()); + } + + if (motor_data_[id].velocity_mode.is_commanded()) + { + motor_data_[id].velocity_mode.set_response(driver->set_mode_velocity()); + } + + if (motor_data_[id].cyclic_velocity_mode.is_commanded()) + { + motor_data_[id].cyclic_velocity_mode.set_response(driver->set_mode_cyclic_velocity()); + } + + if (motor_data_[id].torque_mode.is_commanded()) + { + motor_data_[id].torque_mode.set_response(driver->set_mode_torque()); + } + + if (motor_data_[id].interpolated_position_mode.is_commanded()) + { + motor_data_[id].interpolated_position_mode.set_response(driver->set_mode_torque()); + } +} + +void Cia402System::handleInit(uint id, const std::shared_ptr & driver) +{ + if (motor_data_[id].init.is_commanded()) + { + motor_data_[id].init.set_response(driver->init_motor()); + } +} + +void Cia402System::handleRecover( + uint id, const std::shared_ptr & driver) +{ + if (motor_data_[id].recover.is_commanded()) + { + motor_data_[id].recover.set_response(driver->recover_motor()); + } +} + +void Cia402System::handleHalt(uint id, const std::shared_ptr & driver) +{ + if (motor_data_[id].halt.is_commanded()) + { + motor_data_[id].halt.set_response(driver->halt_motor()); + } +} + +} // namespace canopen_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(canopen_ros2_control::Cia402System, hardware_interface::SystemInterface) diff --git a/canopen_ros2_control/src/robot_system.cpp b/canopen_ros2_control/src/robot_system.cpp new file mode 100644 index 00000000..bba54449 --- /dev/null +++ b/canopen_ros2_control/src/robot_system.cpp @@ -0,0 +1,338 @@ +// Copyright (c) 2023, Fraunhofer IPA +// Copyright (c) 2022, StoglRobotics +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "canopen_ros2_control/robot_system.hpp" +#include + +using namespace canopen_ros2_control; + +// auto robot_system_logger = rclcpp::get_logger("robot_system_interface"); + +hardware_interface::CallbackReturn RobotSystem::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + robot_system_logger = rclcpp::get_logger(info_.name + "_interface"); + RCLCPP_INFO(robot_system_logger, "Registering hardware interface '%s'", info_.name.c_str()); + + // Check bus config is specified. + if (info_.hardware_parameters.find("bus_config") == info_.hardware_parameters.end()) + { + RCLCPP_ERROR( + robot_system_logger, "No bus_config parameter provided for '%s' hardware interface.", + info_.name.c_str()); + return CallbackReturn::ERROR; + } + bus_config_ = info_.hardware_parameters["bus_config"]; + RCLCPP_INFO( + robot_system_logger, "'%s' has bus config: '%s'", info_.name.c_str(), bus_config_.c_str()); + + // Check master config is specified. + if (info_.hardware_parameters.find("master_config") == info_.hardware_parameters.end()) + { + RCLCPP_ERROR( + robot_system_logger, "No master_config parameter provided for '%s' hardware interface.", + info_.name.c_str()); + return CallbackReturn::ERROR; + } + master_config_ = info_.hardware_parameters["master_config"]; + RCLCPP_INFO( + robot_system_logger, "'%s' has master config: '%s'", info_.name.c_str(), + master_config_.c_str()); + + // Check master bin is specified. + if (info_.hardware_parameters.find("master_bin") != info_.hardware_parameters.end()) + { + master_bin_ = info_.hardware_parameters["master_bin"]; + if (master_bin_ == "\"\"") + { + master_bin_ = ""; + } + RCLCPP_INFO( + robot_system_logger, "'%s' has master bin: '%s'", info_.name.c_str(), master_bin_.c_str()); + } + else + { + master_bin_ = ""; + } + + // Check can_interface_name is specified. + if (info_.hardware_parameters.find("can_interface_name") == info_.hardware_parameters.end()) + { + RCLCPP_ERROR( + robot_system_logger, "No can_interface_name parameter provided for '%s' hardware interface.", + info_.name.c_str()); + return CallbackReturn::ERROR; + } + can_interface_ = info_.hardware_parameters["can_interface_name"]; + RCLCPP_INFO( + robot_system_logger, "'%s' has can interface: '%s'", info_.name.c_str(), + can_interface_.c_str()); + + ros2_canopen::ConfigurationManager config(bus_config_); + config.init_config(); + + // Load joint data + for (auto joint : info.joints) + { + auto driver_type = + config.get_entry(joint.parameters["device_name"], "driver").value(); + if (driver_type == "ros2_canopen::Cia402Driver") + { + auto data = Cia402Data(); + if (data.init_data(joint, config.dump_device(joint.parameters["device_name"]))) + { + robot_motor_data_.push_back(data); + } + { + robot_motor_data_.push_back(data); + } + } + else + { + RCLCPP_ERROR( + robot_system_logger, "Driver type '%s' not supported for joint '%s'", driver_type.c_str(), + joint.name.c_str()); + return CallbackReturn::ERROR; + } + } + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RobotSystem::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + device_container_ = std::make_shared(executor_); + executor_->add_node(device_container_); + + spin_thread_ = std::make_unique(&RobotSystem::spin, this); + init_thread_ = std::make_unique(&RobotSystem::initDeviceContainer, this); + + if (init_thread_->joinable()) + { + init_thread_->join(); + } + else + { + RCLCPP_ERROR(robot_system_logger, "Could not join init thread!"); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} +hardware_interface::CallbackReturn RobotSystem::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + for (auto & data : robot_motor_data_) + { + if (!data.driver->init_motor()) + { + RCLCPP_ERROR(robot_system_logger, "Failed to activate '%s'", data.joint_name.c_str()); + return CallbackReturn::FAILURE; + } + } + return CallbackReturn::SUCCESS; +} +hardware_interface::CallbackReturn RobotSystem::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + for (auto & data : robot_motor_data_) + { + if (!data.driver->halt_motor()) + { + RCLCPP_ERROR(robot_system_logger, "Failed to deactivate '%s'", data.joint_name.c_str()); + return CallbackReturn::FAILURE; + } + } + return CallbackReturn::SUCCESS; +} +hardware_interface::CallbackReturn RobotSystem::on_cleanup( + const rclcpp_lifecycle::State & previous_state) +{ + clean(); + return CallbackReturn::SUCCESS; +} +hardware_interface::CallbackReturn RobotSystem::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + clean(); + return CallbackReturn::SUCCESS; +} + +std::vector RobotSystem::export_state_interfaces() +{ + std::vector state_interfaces; + + // Iterate over joints in xacro + for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) + { + data.export_state_interface(state_interfaces); + } + return state_interfaces; +} + +std::vector RobotSystem::export_command_interfaces() +{ + std::vector command_interfaces; + + // Iterate over joints in xacro + for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) + { + data.export_command_interface(command_interfaces); + } + return command_interfaces; +} + +hardware_interface::return_type RobotSystem::read( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // Iterate over joints + for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) + { + data.read_state(); + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RobotSystem::write( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + for (canopen_ros2_control::Cia402Data & data : robot_motor_data_) + { + data.write_target(); + } + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RobotSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + // register interfaces to start per device + for (auto interface : start_interfaces) + { + auto it = std::find_if( + robot_motor_data_.begin(), robot_motor_data_.end(), + [interface](Cia402Data & data) + { + return std::find(data.interfaces.begin(), data.interfaces.end(), interface) != + data.interfaces.end(); + }); + if (it != robot_motor_data_.end()) + { + it->interfaces_to_start.push_back( + hardware_interface::CommandInterface(interface).get_interface_name()); + } + } + + // register interfaces to stop per device + for (auto interface : stop_interfaces) + { + auto it = std::find_if( + robot_motor_data_.begin(), robot_motor_data_.end(), + [interface](Cia402Data & data) + { + return std::find(data.interfaces.begin(), data.interfaces.end(), interface) != + data.interfaces.end(); + }); + if (it != robot_motor_data_.end()) + { + it->interfaces_to_stop.push_back( + hardware_interface::CommandInterface(interface).get_interface_name()); + } + } + + // perform switching + for (auto & data : robot_motor_data_) + { + if (!data.perform_switch()) + { + return hardware_interface::return_type::ERROR; + } + } + return hardware_interface::return_type::OK; +} + +void RobotSystem::initDeviceContainer() +{ + // Init device container + device_container_->init( + this->can_interface_, this->master_config_, this->bus_config_, this->master_bin_); + + // Get all registered drivers. + auto drivers = device_container_->get_registered_drivers(); + + // Iterate over all drivers and allocate them to the correct joint. + for (auto & data : robot_motor_data_) + { + // Find correct driver for joint via node id. + auto driver = std::find_if( + drivers.begin(), drivers.end(), + [&data](const std::pair> & driver) + { return driver.first == data.node_id; }); + + if (driver == drivers.end()) + { + RCLCPP_ERROR( + device_container_->get_logger(), "Could not find driver for joint '%s' with node id '%d'", + data.joint_name.c_str(), data.node_id); + continue; + } + + // Allocate driver to joint. + if ( + device_container_->get_driver_type(driver->first).compare("ros2_canopen::Cia402Driver") == 0) + { + data.driver = std::static_pointer_cast(driver->second); + } + } + RCLCPP_INFO(device_container_->get_logger(), "Initialisation successful."); +} + +void RobotSystem::spin() +{ + executor_->spin(); + executor_->remove_node(device_container_); + RCLCPP_INFO(device_container_->get_logger(), "Stopped spinning RobotSystem ROS2 executor"); +} + +void RobotSystem::clean() +{ + printf("Cancel exectutor..."); + executor_->cancel(); + printf("Join spin thread..."); + spin_thread_->join(); + + printf("Reset variables..."); + device_container_.reset(); + executor_.reset(); + + init_thread_->join(); + init_thread_.reset(); + + executor_.reset(); + spin_thread_.reset(); + robot_motor_data_.clear(); +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(canopen_ros2_control::RobotSystem, hardware_interface::SystemInterface) diff --git a/canopen_ros2_control/test/test_canopen_system.cpp b/canopen_ros2_control/test/test_canopen_system.cpp new file mode 100644 index 00000000..a1f6fb3d --- /dev/null +++ b/canopen_ros2_control/test/test_canopen_system.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2022, StoglRobotics +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2022-06-29 + * + */ +//---------------------------------------------------------------------- + +#include + +#include + +#include "hardware_interface/resource_manager.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +class TestCanopenSystem : public ::testing::Test +{ +protected: + void SetUp() override + { + // TODO(anyone): Extend this description to your robot + canopen_system_2dof_ = + R"( + + + canopen_ros2_control/CanopenSystem + + + + + 1.57 + + + + + 0.7854 + + + )"; + } + + std::string canopen_system_2dof_; +}; + +TEST_F(TestCanopenSystem, load_canopen_system_2dof) +{ + auto urdf = ros2_control_test_assets::urdf_head + canopen_system_2dof_ + + ros2_control_test_assets::urdf_tail; + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); +} diff --git a/canopen_ros2_control/urdf/canopen_system.ros2_control.xacro b/canopen_ros2_control/urdf/canopen_system.ros2_control.xacro new file mode 100644 index 00000000..2b2cf3fb --- /dev/null +++ b/canopen_ros2_control/urdf/canopen_system.ros2_control.xacro @@ -0,0 +1,30 @@ + + + + + + + + canopen_ros2_control/CanopenSystem + ${bus_config} + ${master_config} + ${can_interface_name} + "${master_bin}" + + + 2 + + + 3 + + + + + + diff --git a/canopen_ros2_control/urdf/canopen_system.urdf.xacro b/canopen_ros2_control/urdf/canopen_system.urdf.xacro new file mode 100644 index 00000000..1d3291f6 --- /dev/null +++ b/canopen_ros2_control/urdf/canopen_system.urdf.xacro @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/canopen_ros2_control/urdf/cia402_system.ros2_control.xacro b/canopen_ros2_control/urdf/cia402_system.ros2_control.xacro new file mode 100644 index 00000000..0cf2c15d --- /dev/null +++ b/canopen_ros2_control/urdf/cia402_system.ros2_control.xacro @@ -0,0 +1,30 @@ + + + + + + + + canopen_ros2_control/Cia402System + ${bus_config} + ${master_config} + ${can_interface} + "${master_bin}" + + + 2 + + + + + + + + + diff --git a/canopen_ros2_control/urdf/cia402_system.urdf.xacro b/canopen_ros2_control/urdf/cia402_system.urdf.xacro new file mode 100644 index 00000000..f1fabd14 --- /dev/null +++ b/canopen_ros2_control/urdf/cia402_system.urdf.xacro @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/canopen_ros2_controllers/CMakeLists.txt b/canopen_ros2_controllers/CMakeLists.txt new file mode 100644 index 00000000..9de45f2f --- /dev/null +++ b/canopen_ros2_controllers/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.8) +project(canopen_ros2_controllers) + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + canopen_402_driver + canopen_interfaces + canopen_proxy_driver + controller_interface + controller_manager + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + std_msgs + std_srvs +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + cia402_robot_controller_parameters + include/canopen_ros2_controllers/cia402_robot_controller.yaml +) + +add_library( + ${PROJECT_NAME} + SHARED + src/canopen_proxy_controller.cpp + src/cia402_device_controller.cpp + src/cia402_robot_controller.cpp +) + +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + cia402_robot_controller_parameters +) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +target_compile_definitions(${PROJECT_NAME} PRIVATE "CANOPEN_PROXY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") +pluginlib_export_plugin_description_file(controller_interface canopen_ros2_controllers.xml) + +install( + TARGETS + cia402_robot_controller_parameters + ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + ament_add_gmock(test_load_canopen_proxy_controller test/test_load_canopen_proxy_controller.cpp) + target_include_directories(test_load_canopen_proxy_controller PRIVATE include) + ament_target_dependencies( + test_load_canopen_proxy_controller + controller_manager + hardware_interface + ros2_control_test_assets + canopen_interfaces + ) + + ament_add_gmock(test_canopen_proxy_controller test/test_canopen_proxy_controller.cpp) + target_include_directories(test_canopen_proxy_controller PRIVATE include) + target_link_libraries(test_canopen_proxy_controller ${PROJECT_NAME}) + ament_target_dependencies( + test_canopen_proxy_controller + controller_interface + hardware_interface + canopen_interfaces + ) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +ament_package() diff --git a/canopen_ros2_controllers/canopen_ros2_controllers.xml b/canopen_ros2_controllers/canopen_ros2_controllers.xml new file mode 100644 index 00000000..1d10ad6b --- /dev/null +++ b/canopen_ros2_controllers/canopen_ros2_controllers.xml @@ -0,0 +1,20 @@ + + + + Generic controller for Canopen devices providing service interfaces through ros2_control stack to the can devices. + + + + + Generic controller for Canopen 402 devices providing service interfaces through ros2_control stack to the can devices. + + + + + Generic controller for Canopen 402 devices providing service interfaces through ros2_control stack to the can devices. + + + diff --git a/canopen_ros2_controllers/include/canopen_ros2_controllers/canopen_proxy_controller.hpp b/canopen_ros2_controllers/include/canopen_ros2_controllers/canopen_proxy_controller.hpp new file mode 100644 index 00000000..d5b599f8 --- /dev/null +++ b/canopen_ros2_controllers/include/canopen_ros2_controllers/canopen_proxy_controller.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROLLERS__CANOPEN_PROXY_CONTROLLER_HPP_ +#define CANOPEN_ROS2_CONTROLLERS__CANOPEN_PROXY_CONTROLLER_HPP_ + +#include +#include +#include + +#include "canopen_interfaces/msg/co_data.hpp" +#include "canopen_interfaces/srv/co_read.hpp" +#include "canopen_interfaces/srv/co_write.hpp" +#include "canopen_ros2_controllers/visibility_control.h" +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_msgs/msg/string.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace +{ +enum CommandInterfaces +{ + TPDO_INDEX, + TPDO_SUBINDEX, + TPDO_DATA, + TPDO_ONS, + NMT_RESET, + NMT_RESET_FBK, + NMT_START, + NMT_START_FBK, + LAST_COMMAND_AUX, +}; + +enum StateInterfaces +{ + RPDO_INDEX, + RPDO_SUBINDEX, + RPDO_DATA, + NMT_STATE, + LAST_STATE_AUX, +}; + +} // namespace + +namespace canopen_ros2_controllers +{ + +class CanopenProxyController : public controller_interface::ControllerInterface +{ +public: + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + CanopenProxyController(); + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerCommandMsg = canopen_interfaces::msg::COData; + using ControllerStartResetSrvType = std_srvs::srv::Trigger; + using ControllerSDOReadSrvType = canopen_interfaces::srv::CORead; + using ControllerSDOWriteSrvType = canopen_interfaces::srv::COWrite; + using ControllerNMTStateMsg = std_msgs::msg::String; + +protected: + std::string joint_name_; + + // Command subscribers + // TPDO subscription + rclcpp::Subscription::SharedPtr tpdo_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_cmd_; + + // NMT state publisher + using ControllerNmtStateRTPublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr nmt_state_pub_; + std::unique_ptr nmt_state_rt_publisher_; + std::string nmt_state_actual_ = "BOOTUP"; + + // RPDO publisher + using ControllerRPDOPRTublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr rpdo_pub_; + std::unique_ptr rpdo_rt_publisher_; + + // NMT reset service + rclcpp::Service::SharedPtr nmt_state_reset_service_; + // NMT start service + rclcpp::Service::SharedPtr nmt_state_start_service_; + // SDO read service + rclcpp::Service::SharedPtr sdo_read_service_; + // SDO write service + rclcpp::Service::SharedPtr sdo_write_service_; +}; + +} // namespace canopen_ros2_controllers + +#endif // CANOPEN_ROS2_CONTROLLERS__CANOPEN_PROXY_CONTROLLER_HPP_ diff --git a/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_device_controller.hpp b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_device_controller.hpp new file mode 100644 index 00000000..2c95ba04 --- /dev/null +++ b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_device_controller.hpp @@ -0,0 +1,133 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ +#define CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ + +#include "canopen_interfaces/srv/co_target_double.hpp" +#include "canopen_ros2_controllers/canopen_proxy_controller.hpp" + +static constexpr int kLoopPeriodMS = 100; +static constexpr double kCommandValue = 1.0; + +namespace +{ +enum Cia402CommandInterfaces +{ + INIT_CMD = CommandInterfaces::LAST_COMMAND_AUX, + INIT_FBK, + HALT_CMD, + HALT_FBK, + RECOVER_CMD, + RECOVER_FBK, + POSITION_MODE_CMD, + POSITION_MODE_FBK, + VELOCITY_MODE_CMD, + VELOCITY_MODE_FBK, + CYCLIC_VELOCITY_MODE_CMD, + CYCLIC_VELOCITY_MODE_FBK, + CYCLIC_POSITION_MODE_CMD, + CYCLIC_POSITION_MODE_FBK, + INTERPOLATED_POSITION_MODE_CMD, + INTERPOLATED_POSITION_MODE_FBK, +}; + +enum Cia402StateInterfaces +{ + FIRST_STATE = StateInterfaces::LAST_STATE_AUX, +}; + +} // namespace + +namespace canopen_ros2_controllers +{ + +class Cia402DeviceController : public canopen_ros2_controllers::CanopenProxyController +{ +public: + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + Cia402DeviceController(); + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init(); + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const; + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state); + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state); + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state); + + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period); + +protected: + inline rclcpp::Service::SharedPtr createTriggerSrv( + const std::string & service, Cia402CommandInterfaces cmd, Cia402CommandInterfaces fbk) + { + // define service profile + auto service_profile = rmw_qos_profile_services_default; + service_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + service_profile.depth = 1; + + rclcpp::Service::SharedPtr srv = + get_node()->create_service( + service, + [&, cmd, fbk]( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) + { + command_interfaces_[cmd].set_value(kCommandValue); + + while (std::isnan(command_interfaces_[fbk].get_value()) && rclcpp::ok()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(kLoopPeriodMS)); + } + + // report success + response->success = static_cast(command_interfaces_[fbk].get_value()); + // reset to nan + command_interfaces_[fbk].set_value(std::numeric_limits::quiet_NaN()); + command_interfaces_[cmd].set_value(std::numeric_limits::quiet_NaN()); + }, + service_profile); + + return srv; + } + + rclcpp::Service::SharedPtr handle_init_service_; + rclcpp::Service::SharedPtr handle_halt_service_; + rclcpp::Service::SharedPtr handle_recover_service_; + rclcpp::Service::SharedPtr handle_set_mode_position_service_; + rclcpp::Service::SharedPtr handle_set_mode_torque_service_; + rclcpp::Service::SharedPtr handle_set_mode_velocity_service_; + rclcpp::Service::SharedPtr handle_set_mode_cyclic_velocity_service_; + rclcpp::Service::SharedPtr handle_set_mode_cyclic_position_service_; + rclcpp::Service::SharedPtr handle_set_mode_interpolated_position_service_; + rclcpp::Service::SharedPtr handle_set_target_service_; +}; + +} // namespace canopen_ros2_controllers + +#endif // CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ diff --git a/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp new file mode 100644 index 00000000..8873acf0 --- /dev/null +++ b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp @@ -0,0 +1,153 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ +#define CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ + +#include "canopen_interfaces/srv/co_target_double.hpp" +#include "canopen_ros2_controllers/canopen_proxy_controller.hpp" +#include "cia402_robot_controller_parameters.hpp" + +namespace cia402_robot_controller +{ +/** + * @brief Controller for a robot with CiA402 drivers. + * + * This controller handles bringing up of the CiA402 drivers to operational + * state with the right operation mode set. + */ +class Cia402RobotController : public controller_interface::ControllerInterface +{ +public: + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + Cia402RobotController(); + + /** + * @brief Initialize controller + * + * @details + * This function initializes the controller. It declares the + * parameters of the controller. This is done using the + * generate_parameter_library. + * + * @return controller_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + /** + * @brief Get the command interface configuration object + * + * @details + * This function returns the command interface configuration. + * This controller has uses the following command interfaces per joint: + * - init + * - init_feedback + * - halt + * - halt_feedback + * - recover + * - recover_feedback + * - operation_mode + * - operation_mode_feedback + * + * @return controller_interface::InterfaceConfiguration + */ + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief Get the state interface configuration object + * + * @details + * This function returns the state interface configuration. + * This controller has no state interface. + * + * @return CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + */ + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + /** + * @brief Configure controller + * + * @details + * This function configures the controller. It reads the + * parameters of the controller and sets up the interfaces + * used by the controller. + * + * @param previous_state + * @return controller_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Activate controller + * + * @details + * This function activates the controller. It brings up the + * CiA402 drivers to operational state with the right operation + * mode set. + * + * @param previous_state + * @return controller_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Deactivate controller + * + * + * @param previous_state + * @return controller_interface::CallbackReturn + */ + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Update controller + * + * @details + * This function updates the controller. It does nothing. + * + * @param time + * @param period + * @return controller_interface::return_type + */ + CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + rclcpp::Service::SharedPtr handle_recover_service_; + + std::shared_ptr param_listener_; + Params params_; + controller_interface::InterfaceConfiguration command_interfaces_config_; + controller_interface::InterfaceConfiguration state_interfaces_config_; + + void declare_parameters(); + controller_interface::CallbackReturn read_parameters(); + void activate(); + + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace cia402_robot_controller + +#endif // CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ diff --git a/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml new file mode 100644 index 00000000..accace8c --- /dev/null +++ b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml @@ -0,0 +1,14 @@ +cia402_robot_controller: + joints: { + type: string_array, + default_value: [], + description: 'List of joints controlled by this controller', + } + operation_mode: { + type: int, + description: 'ID of operation mode to use', + } + command_poll_freq: { + type: int, + description: 'Frequency of polling command results', + } diff --git a/canopen_ros2_controllers/include/canopen_ros2_controllers/visibility_control.h b/canopen_ros2_controllers/include/canopen_ros2_controllers/visibility_control.h new file mode 100644 index 00000000..9e0631d6 --- /dev/null +++ b/canopen_ros2_controllers/include/canopen_ros2_controllers/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef CANOPEN_ROS2_CONTROLLERS__VISIBILITY_BUILDING_DLL +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT +#else +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT +#endif +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC_TYPE CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_LOCAL +#else +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_LOCAL +#endif +#define CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // CANOPEN_ROS2_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml new file mode 100644 index 00000000..8a328c6e --- /dev/null +++ b/canopen_ros2_controllers/package.xml @@ -0,0 +1,33 @@ + + + + canopen_ros2_controllers + 0.0.1 + ros2_control controllers for ros2_canopen functionalities + Denis Stogl + Lovro Ivanov + + Apache-2.0 + + ament_cmake + + canopen_402_driver + canopen_interfaces + canopen_proxy_driver + controller_interface + controller_manager + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + std_msgs + std_srvs + + ament_cmake_gmock + + + ament_cmake + + diff --git a/canopen_ros2_controllers/src/canopen_proxy_controller.cpp b/canopen_ros2_controllers/src/canopen_proxy_controller.cpp new file mode 100644 index 00000000..2ac95c2a --- /dev/null +++ b/canopen_ros2_controllers/src/canopen_proxy_controller.cpp @@ -0,0 +1,355 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "canopen_ros2_controllers/canopen_proxy_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" + +#include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" + +static constexpr int kLoopPeriodMS = 100; +static constexpr double kCommandValue = 1.0; + +namespace +{ // utility + +using ControllerCommandMsg = canopen_ros2_controllers::CanopenProxyController::ControllerCommandMsg; + +// called from RT control loop +void reset_controller_command_msg( + std::shared_ptr & msg, const std::string & joint_name) +{ + msg->index = 0u; + msg->subindex = 0u; + msg->data = 0u; +} +bool propagate_controller_command_msg(std::shared_ptr & msg) +{ + // TODO (livanov93): add better logic to decide if a message + // should be propagated to the bus + // check if it is 8 = uint8_t or 16 = uint16_t or 32 = uint32_t + return true; +} +} // namespace + +namespace canopen_ros2_controllers +{ +CanopenProxyController::CanopenProxyController() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn CanopenProxyController::on_init() +{ + try + { + // use auto declare + auto_declare("joint", joint_name_); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CanopenProxyController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto error_if_empty = [&](const auto & parameter, const char * parameter_name) + { + if (parameter.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was empty", parameter_name); + return true; + } + return false; + }; + + auto get_string_array_param_and_error_if_empty = + [&](std::vector & parameter, const char * parameter_name) + { + parameter = get_node()->get_parameter(parameter_name).as_string_array(); + return error_if_empty(parameter, parameter_name); + }; + + auto get_string_param_and_error_if_empty = + [&](std::string & parameter, const char * parameter_name) + { + parameter = get_node()->get_parameter(parameter_name).as_string(); + return error_if_empty(parameter, parameter_name); + }; + + if (get_string_param_and_error_if_empty(joint_name_, "joint")) + { + return controller_interface::CallbackReturn::ERROR; + } + + // Command Subscriber and callbacks + auto callback_cmd = [&](const std::shared_ptr msg) -> void + { input_cmd_.writeFromNonRT(msg); }; + tpdo_subscriber_ = get_node()->create_subscription( + "~/tpdo", rclcpp::SystemDefaultsQoS(), callback_cmd); + + std::shared_ptr msg = std::make_shared(); + reset_controller_command_msg(msg, joint_name_); + + input_cmd_.writeFromNonRT(msg); + + try + { + // nmt state publisher + nmt_state_pub_ = get_node()->create_publisher( + "~/nmt_state", rclcpp::SystemDefaultsQoS()); + nmt_state_rt_publisher_ = std::make_unique(nmt_state_pub_); + + // rpdo publisher + rpdo_pub_ = + get_node()->create_publisher("~/rpdo", rclcpp::SystemDefaultsQoS()); + rpdo_rt_publisher_ = std::make_unique(rpdo_pub_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + nmt_state_rt_publisher_->lock(); + nmt_state_rt_publisher_->msg_.data = std::string(); + nmt_state_rt_publisher_->unlock(); + + rpdo_rt_publisher_->lock(); + rpdo_rt_publisher_->msg_.index = 0u; + rpdo_rt_publisher_->msg_.subindex = 0u; + rpdo_rt_publisher_->msg_.data = 0u; + rpdo_rt_publisher_->unlock(); + + // init services + + // NMT reset + auto on_nmt_state_reset = [&]( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) + { + command_interfaces_[CommandInterfaces::NMT_RESET].set_value(kCommandValue); + + while (!std::isnan(command_interfaces_[CommandInterfaces::NMT_RESET].get_value())) + { + std::this_thread::sleep_for(std::chrono::milliseconds(kLoopPeriodMS)); + } + + // report success + response->success = + static_cast(command_interfaces_[CommandInterfaces::NMT_RESET_FBK].get_value()); + // reset to nan + command_interfaces_[CommandInterfaces::NMT_RESET_FBK].set_value( + std::numeric_limits::quiet_NaN()); + }; + + auto service_profile = rmw_qos_profile_services_default; + service_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + service_profile.depth = 1; + nmt_state_reset_service_ = get_node()->create_service( + "~/nmt_reset_node", on_nmt_state_reset, service_profile); + + // NMT start + auto on_nmt_state_start = [&]( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) + { + command_interfaces_[CommandInterfaces::NMT_START].set_value(kCommandValue); + + while (!std::isnan(command_interfaces_[CommandInterfaces::NMT_START].get_value())) + { + std::this_thread::sleep_for(std::chrono::milliseconds(kLoopPeriodMS)); + } + + // report success + response->success = + static_cast(command_interfaces_[CommandInterfaces::NMT_START_FBK].get_value()); + // reset to nan + command_interfaces_[CommandInterfaces::NMT_START_FBK].set_value( + std::numeric_limits::quiet_NaN()); + }; + nmt_state_start_service_ = get_node()->create_service( + "~/nmt_start_node", on_nmt_state_start, service_profile); + + // SDO read + auto on_sdo_read = [&]( + const canopen_interfaces::srv::CORead::Request::SharedPtr request, + canopen_interfaces::srv::CORead::Response::SharedPtr response) {}; + + sdo_read_service_ = get_node()->create_service( + "~/sdo_read", on_sdo_read, service_profile); + + // SDO write + auto on_sdo_write = [&]( + const canopen_interfaces::srv::COWrite::Request::SharedPtr request, + canopen_interfaces::srv::COWrite::Response::SharedPtr response) { + + }; + + sdo_write_service_ = get_node()->create_service( + "~/sdo_write", on_sdo_write, service_profile); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +CanopenProxyController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(9); + command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/index"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/subindex"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/type"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/data"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/owns"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/reset"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/reset_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/start"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/start_fbk"); + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration CanopenProxyController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(5); + state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/index"); + state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/subindex"); + state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/type"); + state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/data"); + state_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/state"); + + return state_interfaces_config; +} + +controller_interface::CallbackReturn CanopenProxyController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_command_msg(*(input_cmd_.readFromRT)(), joint_name_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CanopenProxyController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type CanopenProxyController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + // nmt state is retrieved in SystemInterface via cb and is exposed here + if (nmt_state_rt_publisher_) + { + auto message = std_msgs::msg::String(); + auto nmt_state = static_cast(state_interfaces_[StateInterfaces::NMT_STATE].get_value()); + + switch (static_cast(nmt_state)) + { + case canopen::NmtState::BOOTUP: + message.data = "BOOTUP"; + break; + case canopen::NmtState::PREOP: + message.data = "PREOP"; + break; + case canopen::NmtState::RESET_COMM: + message.data = "RESET_COMM"; + break; + case canopen::NmtState::RESET_NODE: + message.data = "RESET_NODE"; + break; + case canopen::NmtState::START: + message.data = "START"; + break; + case canopen::NmtState::STOP: + message.data = "STOP"; + break; + case canopen::NmtState::TOGGLE: + message.data = "TOGGLE"; + break; + default: + RCLCPP_ERROR(get_node()->get_logger(), "Unknown NMT State."); + message.data = "ERROR"; + break; + } + + if (nmt_state_actual_ != message.data && nmt_state_rt_publisher_->trylock()) + { + // publish on change only + nmt_state_actual_ = std::string(message.data); + nmt_state_rt_publisher_->msg_.data = nmt_state_actual_; + nmt_state_rt_publisher_->unlockAndPublish(); + } + } + + // exposing rpdo data via real-time publisher + if (rpdo_rt_publisher_ && rpdo_rt_publisher_->trylock()) + { + rpdo_rt_publisher_->msg_.index = + static_cast(state_interfaces_[StateInterfaces::RPDO_INDEX].get_value()); + rpdo_rt_publisher_->msg_.subindex = + static_cast(state_interfaces_[StateInterfaces::RPDO_SUBINDEX].get_value()); + rpdo_rt_publisher_->msg_.data = + static_cast(state_interfaces_[StateInterfaces::RPDO_DATA].get_value()); + + rpdo_rt_publisher_->unlockAndPublish(); + } + + // tpdo data is the main controller data retrieved via subscription + auto current_cmd = input_cmd_.readFromRT(); + if (!current_cmd || !(*current_cmd)) + { + return controller_interface::return_type::OK; + } + else if (propagate_controller_command_msg(*current_cmd)) + { + command_interfaces_[CommandInterfaces::TPDO_INDEX].set_value((*current_cmd)->index); + command_interfaces_[CommandInterfaces::TPDO_SUBINDEX].set_value((*current_cmd)->subindex); + command_interfaces_[CommandInterfaces::TPDO_DATA].set_value((*current_cmd)->data); + // tpdo data one shot mechanism + command_interfaces_[CommandInterfaces::TPDO_ONS].set_value(kCommandValue); + } + + return controller_interface::return_type::OK; +} + +} // namespace canopen_ros2_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + canopen_ros2_controllers::CanopenProxyController, controller_interface::ControllerInterface) diff --git a/canopen_ros2_controllers/src/cia402_device_controller.cpp b/canopen_ros2_controllers/src/cia402_device_controller.cpp new file mode 100644 index 00000000..0bfb8bc4 --- /dev/null +++ b/canopen_ros2_controllers/src/cia402_device_controller.cpp @@ -0,0 +1,165 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "canopen_ros2_controllers/cia402_device_controller.hpp" + +#include +#include +#include +#include + +#include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +using ControllerCommandMsg = canopen_ros2_controllers::CanopenProxyController::ControllerCommandMsg; + +// called from RT control loop +void reset_controller_command_msg( + std::shared_ptr & msg, const std::string & joint_name) +{ +} + +} // namespace + +namespace canopen_ros2_controllers +{ +Cia402DeviceController::Cia402DeviceController() +: canopen_ros2_controllers::CanopenProxyController() +{ +} + +controller_interface::CallbackReturn Cia402DeviceController::on_init() +{ + if (CanopenProxyController::on_init() != controller_interface::CallbackReturn::SUCCESS) + return controller_interface::CallbackReturn::ERROR; + + handle_init_service_ = createTriggerSrv( + "~/init", Cia402CommandInterfaces::INIT_CMD, Cia402CommandInterfaces::INIT_FBK); + + handle_halt_service_ = createTriggerSrv( + "~/halt", Cia402CommandInterfaces::HALT_CMD, Cia402CommandInterfaces::HALT_FBK); + + handle_recover_service_ = createTriggerSrv( + "~/recover", Cia402CommandInterfaces::RECOVER_CMD, Cia402CommandInterfaces::RECOVER_FBK); + + handle_set_mode_position_service_ = createTriggerSrv( + "~/position_mode", Cia402CommandInterfaces::POSITION_MODE_CMD, + Cia402CommandInterfaces::POSITION_MODE_FBK); + + handle_set_mode_velocity_service_ = createTriggerSrv( + "~/velocity_mode", Cia402CommandInterfaces::VELOCITY_MODE_CMD, + Cia402CommandInterfaces::VELOCITY_MODE_FBK); + + handle_set_mode_cyclic_velocity_service_ = createTriggerSrv( + "~/cyclic_velocity_mode", Cia402CommandInterfaces::CYCLIC_VELOCITY_MODE_CMD, + Cia402CommandInterfaces::CYCLIC_VELOCITY_MODE_FBK); + + handle_set_mode_cyclic_position_service_ = createTriggerSrv( + "~/cyclic_position_mode", Cia402CommandInterfaces::CYCLIC_POSITION_MODE_CMD, + Cia402CommandInterfaces::CYCLIC_POSITION_MODE_FBK); + + handle_set_mode_interpolated_position_service_ = createTriggerSrv( + "~/interpolated_position_mode", Cia402CommandInterfaces::INTERPOLATED_POSITION_MODE_CMD, + Cia402CommandInterfaces::INTERPOLATED_POSITION_MODE_FBK); + + /* + handle_set_mode_torque_service_ = createTriggerSrv("~/torque_mode", + Cia402CommandInterfaces::, + Cia402CommandInterfaces::); + + handle_set_target_service_ = createTriggerSrv("~/target", Cia402CommandInterfaces::, + Cia402CommandInterfaces::); + */ + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +Cia402DeviceController::command_interface_configuration() const +{ + auto command_interfaces_config = CanopenProxyController::command_interface_configuration(); + command_interfaces_config.names.push_back(joint_name_ + "/" + "init_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "init_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "halt_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "halt_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "recover_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "recover_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "position_mode_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "position_mode_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "velocity_mode_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "velocity_mode_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_velocity_mode_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_velocity_mode_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_position_mode_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "cyclic_position_mode_fbk"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "interpolated_position_mode_cmd"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "interpolated_position_mode_fbk"); + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration Cia402DeviceController::state_interface_configuration() + const +{ + auto state_interfaces_config = CanopenProxyController::state_interface_configuration(); + // no new state interfaces for this controller - additional state interfaces in cia402_system + // are position and velocity which are claimed by joint_state_broadcaster and + // feedback based controllers + return state_interfaces_config; +} + +controller_interface::CallbackReturn Cia402DeviceController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + if (CanopenProxyController::on_configure(previous_state) != CallbackReturn::SUCCESS) + return CallbackReturn::ERROR; + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn Cia402DeviceController::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + if (CanopenProxyController::on_activate(previous_state) != CallbackReturn::SUCCESS) + return CallbackReturn::ERROR; + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn Cia402DeviceController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + if (CanopenProxyController::on_deactivate(previous_state) != CallbackReturn::SUCCESS) + return CallbackReturn::ERROR; + + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type Cia402DeviceController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (CanopenProxyController::update(time, period) != controller_interface::return_type::OK) + return controller_interface::return_type::ERROR; + + return controller_interface::return_type::OK; +} + +} // namespace canopen_ros2_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + canopen_ros2_controllers::Cia402DeviceController, controller_interface::ControllerInterface) diff --git a/canopen_ros2_controllers/src/cia402_robot_controller.cpp b/canopen_ros2_controllers/src/cia402_robot_controller.cpp new file mode 100644 index 00000000..d399a585 --- /dev/null +++ b/canopen_ros2_controllers/src/cia402_robot_controller.cpp @@ -0,0 +1,237 @@ +#include "canopen_ros2_controllers/cia402_robot_controller.hpp" + +namespace cia402_robot_controller +{ + +enum CommandInterface +{ + INIT, + INIT_FEEDBACK, + HALT, + HALT_FEEDBACK, + RECOVER, + RECOVER_FEEDBACK, + OPERATION_MODE, + OPERATION_MODE_FEEDBACK, + ADD_OP +}; + +Cia402RobotController::Cia402RobotController() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn Cia402RobotController::on_init() +{ + try + { + declare_parameters(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void Cia402RobotController::declare_parameters() +{ + param_listener_ = std::make_shared(get_node()); +} + +controller_interface::CallbackReturn Cia402RobotController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = this->read_parameters(); + if (ret != controller_interface::CallbackReturn::SUCCESS) + { + return ret; + } + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn Cia402RobotController::read_parameters() +{ + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + + params_ = param_listener_->get_params(); + + if (params_.joints.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.operation_mode == 0) + { + RCLCPP_ERROR( + get_node()->get_logger(), "'operation_mode' parameter was not correctly specified"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.command_poll_freq <= 0) + { + RCLCPP_ERROR( + get_node()->get_logger(), "'command_poll_freq' parameter was not correctly specified"); + return controller_interface::CallbackReturn::ERROR; + } + + command_interfaces_config_.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (auto joint_name : params_.joints) + { + command_interfaces_config_.names.push_back(joint_name + "/" + "init"); + command_interfaces_config_.names.push_back(joint_name + "/" + "init_feedback"); + command_interfaces_config_.names.push_back(joint_name + "/" + "halt"); + command_interfaces_config_.names.push_back(joint_name + "/" + "halt_feedback"); + command_interfaces_config_.names.push_back(joint_name + "/" + "recover"); + command_interfaces_config_.names.push_back(joint_name + "/" + "recover_feedback"); + command_interfaces_config_.names.push_back(joint_name + "/" + "operation_mode"); + command_interfaces_config_.names.push_back(joint_name + "/" + "operation_mode_feedback"); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +Cia402RobotController::command_interface_configuration() const +{ + return command_interfaces_config_; +} + +controller_interface::InterfaceConfiguration Cia402RobotController::state_interface_configuration() + const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn Cia402RobotController::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + timer_ = this->get_node()->create_wall_timer( + std::chrono::milliseconds(params_.command_poll_freq), + std::bind(&Cia402RobotController::activate, this)); + return controller_interface::CallbackReturn::SUCCESS; +} + +void Cia402RobotController::activate() +{ + int jn = 0; + for (auto joint_name : params_.joints) + { + RCLCPP_INFO(get_node()->get_logger(), "Initialise '%s'", joint_name.c_str()); + // Initialise joint + //////////////////////////////////////////////////////////////////////////////////////// + RCLCPP_INFO( + get_node()->get_logger(), "Using %s to init", + command_interfaces_[jn + INIT].get_full_name().c_str()); + command_interfaces_[jn + INIT].set_value(1.0); + while (std::isnan(command_interfaces_[jn + INIT_FEEDBACK].get_value()) && rclcpp::ok()) + { + rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); + } + if (command_interfaces_[jn + INIT_FEEDBACK].get_value() != 1.0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Init of '%s' failed", joint_name.c_str()); + // return controller_interface::CallbackReturn::ERROR; + } + + command_interfaces_[jn + INIT].set_value(std::numeric_limits::quiet_NaN()); + command_interfaces_[jn + INIT_FEEDBACK].set_value(std::numeric_limits::quiet_NaN()); + // rclcpp::sleep_for(std::chrono::milliseconds(1000)); + RCLCPP_INFO( + get_node()->get_logger(), "Setting operation mode '%li' of '%s'", params_.operation_mode, + joint_name.c_str()); + // Set operation mode for joint + // Some devices may require setting operation mode before init. + //////////////////////////////////////////////////////////////////////////////////////// + + command_interfaces_[jn + OPERATION_MODE].set_value(params_.operation_mode); + while (std::isnan(command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value()) && + rclcpp::ok()) + { + rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); + } + if (command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value() != 1.0) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Setting operation mode '%li' of '%s' failed", + params_.operation_mode, joint_name.c_str()); + // return controller_interface::CallbackReturn::ERROR; + } + command_interfaces_[jn + OPERATION_MODE].set_value(std::numeric_limits::quiet_NaN()); + command_interfaces_[jn + OPERATION_MODE_FEEDBACK].set_value( + std::numeric_limits::quiet_NaN()); + jn += ADD_OP; + } + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + timer_->cancel(); +} + +controller_interface::CallbackReturn Cia402RobotController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + int jn = 0; + for (auto joint_name : params_.joints) + { + // Halt joint + //////////////////////////////////////////////////////////////////////////////////////// + command_interfaces_[jn + HALT].set_value(1.0); + while (std::isnan(command_interfaces_[jn + HALT_FEEDBACK].get_value()) && rclcpp::ok()) + { + rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); + } + if (command_interfaces_[jn + HALT_FEEDBACK].get_value() != 1.0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Halting of '%s' failed", joint_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + command_interfaces_[jn + HALT].set_value(std::numeric_limits::quiet_NaN()); + command_interfaces_[jn + HALT_FEEDBACK].set_value(std::numeric_limits::quiet_NaN()); + + // Set operation mode for joint + // Some devices may require setting operation mode before init. + //////////////////////////////////////////////////////////////////////////////////////// + + command_interfaces_[jn + OPERATION_MODE].set_value(0.0); + while (std::isnan(command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value()) && + rclcpp::ok()) + { + rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); + } + if (command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value() != 1.0) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Setting operation mode '%li' of '%s' failed", + params_.operation_mode, joint_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + command_interfaces_[jn + OPERATION_MODE].set_value(std::numeric_limits::quiet_NaN()); + command_interfaces_[jn + OPERATION_MODE_FEEDBACK].set_value( + std::numeric_limits::quiet_NaN()); + jn += ADD_OP; + } + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + RCLCPP_INFO(get_node()->get_logger(), "deactivate successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type Cia402RobotController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + return controller_interface::return_type::OK; +} +} // namespace cia402_robot_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cia402_robot_controller::Cia402RobotController, controller_interface::ControllerInterface) diff --git a/canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp b/canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp new file mode 100644 index 00000000..b30ee640 --- /dev/null +++ b/canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_canopen_proxy_controller.hpp" + +#include +#include +#include +#include +#include + +class CanopenProxyControllerTest +: public CanopenProxyControllerFixture +{ +}; + +// When there are many mandatory parameters, set all by default and remove one by one in a +// parameterized test +TEST_P(CanopenProxyControllerTestParameterizedParameters, one_parameter_is_missing) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, CanopenProxyControllerTestParameterizedParameters, + ::testing::Values( + std::make_tuple(std::string("joint"), rclcpp::ParameterValue(std::string({}))))); + +TEST_F(CanopenProxyControllerTest, joint_names_parameter_not_set) +{ + SetUpController(false); + + ASSERT_TRUE(controller_->joint_name_.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_TRUE(controller_->joint_name_.empty()); +} + +TEST_F(CanopenProxyControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->joint_name_.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->joint_name_, joint_name_); +} + +TEST_F(CanopenProxyControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], joint_name_ + "/" + command_interface_names_[i]); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], joint_name_ + "/" + state_interface_names_[i]); + } +} + +TEST_F(CanopenProxyControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_cmd_.readFromNonRT(); + EXPECT_EQ((*msg)->index, 0u); + EXPECT_EQ((*msg)->subindex, 0u); + EXPECT_EQ((*msg)->data, 0u); +} + +TEST_F(CanopenProxyControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(CanopenProxyControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(CanopenProxyControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[CommandInterfaces::TPDO_DATA].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE( + std::isnan(controller_->command_interfaces_[CommandInterfaces::TPDO_DATA].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE( + std::isnan(controller_->command_interfaces_[CommandInterfaces::TPDO_DATA].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(CanopenProxyControllerTest, test_sequence_configure_activate) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(CanopenProxyControllerTest, test_update_logic_fast) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // set command statically as value for good type + std::shared_ptr msg = std::make_shared(); + msg->index = 0u; + msg->subindex = 0u; + msg->data = 0u; + + controller_->input_cmd_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} diff --git a/canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp b/canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp new file mode 100644 index 00000000..b2fe921a --- /dev/null +++ b/canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp @@ -0,0 +1,276 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CANOPEN_ROS2_CONTROLLERS__CONTROLLER__TEST_CANOPEN_PROXY_CONTROLLER_HPP_ +#define CANOPEN_ROS2_CONTROLLERS__CONTROLLER__TEST_CANOPEN_PROXY_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "canopen_ros2_controllers/canopen_proxy_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = canopen_ros2_controllers::CanopenProxyController::ControllerNMTStateMsg; +using ControllerCommandMsg = canopen_ros2_controllers::CanopenProxyController::ControllerCommandMsg; +using ControllerModeSrvType = + canopen_ros2_controllers::CanopenProxyController::ControllerStartResetSrvType; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace + +// subclassing and friending so we can access member variables +class TestableCanopenProxyController : public canopen_ros2_controllers::CanopenProxyController +{ + FRIEND_TEST(CanopenProxyControllerTest, joint_names_parameter_not_set); + FRIEND_TEST(CanopenProxyControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(CanopenProxyControllerTest, activate_success); + FRIEND_TEST(CanopenProxyControllerTest, reactivate_success); + FRIEND_TEST(CanopenProxyControllerTest, test_setting_slow_mode_service); + FRIEND_TEST(CanopenProxyControllerTest, test_update_logic_fast); + FRIEND_TEST(CanopenProxyControllerTest, test_update_logic_slow); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = canopen_ros2_controllers::CanopenProxyController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + cmd_subscriber_wait_set_.add_subscription(tpdo_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, cmd_subscriber_wait_set_, timeout); + } + + // TODO(anyone): add implementation of any methods of your controller is needed + +private: + rclcpp::WaitSet cmd_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class CanopenProxyControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_canopen_ros2_controllers/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_canopen_ros2_controllers/set_slow_control_mode"); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController( + bool set_parameters = true, std::string controller_name = "test_canopen_ros2_controllers") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_name_, command_interface_names_[i], &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_name_, state_interface_names_[i], &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + if (set_parameters) + { + controller_->get_node()->set_parameter({"joint", joint_name_}); + } + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_canopen_ros2_controllers/state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + // TODO(anyone): add/remove arguments as it suites your command message type + void publish_commands( + const std::vector & displacements = {0.45}, + const std::vector & velocities = {0.0}, const double duration = 1.25) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.index = 0u; + msg.subindex = 0u; + msg.data = 0u; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + std::string joint_name_ = {"joint1"}; + std::vector command_interface_names_ = { + "tpdo/index", "tpdo/subindex", "tpdo/type", "tpdo/data", "tpdo/owns", + "nmt/reset", "nmt/reset_fbk", "nmt/start", "nmt/start_fbk"}; + + std::vector state_interface_names_ = { + "rpdo/index", "rpdo/subindex", "rpdo/type", "rpdo/data", "nmt/state"}; + + // see StateInterfaces in canopen_proxy_controller.hpp for correct order; + std::array joint_state_values_ = {0, 0, 0, 0, 0}; + // see CommandInterfaces in canopen_proxy_controller.hpp for correct order; + std::array joint_command_values_ = {101.101, 101.101, 101.101, 101.101, 101.101, + 101.101, 101.101, 101.101, 101.101}; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr slow_control_service_client_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class CanopenProxyControllerTestParameterizedParameters +: public CanopenProxyControllerFixture, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() { CanopenProxyControllerFixture::SetUp(); } + + static void TearDownTestCase() { CanopenProxyControllerFixture::TearDownTestCase(); } + +protected: + void SetUpController(bool set_parameters = true) + { + CanopenProxyControllerFixture::SetUpController(set_parameters); + controller_->get_node()->set_parameter({std::get<0>(GetParam()), std::get<1>(GetParam())}); + } +}; + +#endif // CANOPEN_ROS2_CONTROLLERS__CONTROLLER__TEST_CANOPEN_PROXY_CONTROLLER_HPP_ diff --git a/canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp b/canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp new file mode 100644 index 00000000..fde6f722 --- /dev/null +++ b/canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadCanopenProxyController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_canopen_ros2_controllers", "canopen_ros2_controllers/CanopenProxyController")); + + rclcpp::shutdown(); +} diff --git a/canopen_tests/CMakeLists.txt b/canopen_tests/CMakeLists.txt index 662aac1e..dd98262b 100644 --- a/canopen_tests/CMakeLists.txt +++ b/canopen_tests/CMakeLists.txt @@ -7,38 +7,28 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(canopen_core REQUIRED) -find_package(canopen_interfaces REQUIRED) -find_package(canopen_base_driver REQUIRED) -find_package(canopen_proxy_driver REQUIRED) find_package(lely_core_libraries REQUIRED) generate_dcf(simple) -generate_dcf(cia402) +cogen_dcf(cia402) generate_dcf(cia402_lifecycle) +generate_dcf(simple_lifecycle) +generate_dcf(robot_control) install(DIRECTORY - launch/ - DESTINATION share/${PROJECT_NAME}/launch/ + launch rviz urdf launch_tests + DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY - launch_tests/ - DESTINATION share/${PROJECT_NAME}/launch_tests/ -) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + if(CANOPEN_ENABLED) + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(launch_tests/test_proxy_driver.py) + add_launch_test(launch_tests/test_proxy_lifecycle_driver.py) + endif() endif() ament_package() diff --git a/canopen_tests/README.md b/canopen_tests/README.md new file mode 100644 index 00000000..12ed6416 --- /dev/null +++ b/canopen_tests/README.md @@ -0,0 +1,4 @@ +# CANopen Tests + +Enable launch tests with --cmake-args -DCANOPEN_ENABLED. +They can only be run on devices that have vcan0 enabled. diff --git a/canopen_tests/config/cia402/bus.yml b/canopen_tests/config/cia402/bus.yml index 946f44ca..b484a208 100644 --- a/canopen_tests/config/cia402/bus.yml +++ b/canopen_tests/config/cia402/bus.yml @@ -1,21 +1,23 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + master: node_id: 1 - driver: "ros2_canopen::MasterNode" - package: "canopen_core" - sync_period: 20000 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + sync_period: 10000 -cia402_device_1: - node_id: 2 +defaults: dcf: "cia402_slave.eds" - dcf_path: "install/canopen_tests/share/canopen_tests/config/cia402" - driver: "ros2_canopen::MotionControllerDriver" + driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" - period: 20 - enable_lazy_load: false + period: 10 revision_number: 0 sdo: - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s + - {index: 0x6081, sub_index: 0, value: 1000} + - {index: 0x6083, sub_index: 0, value: 2000} tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true @@ -23,7 +25,7 @@ cia402_device_1: transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - - {index: 0x6061, sub_index: 0} # mode of operaiton display + - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" @@ -47,4 +49,18 @@ cia402_device_1: cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position - - {index: 0x60FF, sub_index: 0} # target velocity \ No newline at end of file + - {index: 0x60FF, sub_index: 0} # target velocity + +nodes: + cia402_device_1: + node_id: 2 + cia402_device_2: + node_id: 3 + cia402_device_3: + node_id: 4 + cia402_device_4: + node_id: 5 + cia402_device_5: + node_id: 6 + cia402_device_6: + node_id: 7 diff --git a/canopen_tests/config/cia402/cia402_slave.eds b/canopen_tests/config/cia402/cia402_slave.eds index d8a2bddd..24be3815 100644 --- a/canopen_tests/config/cia402/cia402_slave.eds +++ b/canopen_tests/config/cia402/cia402_slave.eds @@ -1439,4 +1439,3 @@ DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 - diff --git a/canopen_tests/config/cia402_lifecycle/bus.yml b/canopen_tests/config/cia402_lifecycle/bus.yml index 994ad15f..4e92720c 100644 --- a/canopen_tests/config/cia402_lifecycle/bus.yml +++ b/canopen_tests/config/cia402_lifecycle/bus.yml @@ -1,14 +1,16 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + master: node_id: 1 - driver: "ros2_canopen::LifecycleMasterNode" - package: "canopen_core" + driver: "ros2_canopen::LifecycleMasterDriver" + package: "canopen_master_driver" sync_period: 20000 cia402_device_1: node_id: 2 dcf: "cia402_slave.eds" - dcf_path: "install/canopen_tests/share/canopen_tests/config/cia402_lifecycle" - driver: "ros2_canopen::LifecycleMotionControllerDriver" + driver: "ros2_canopen::LifecycleCia402Driver" package: "canopen_402_driver" period: 20 enable_lazy_load: false @@ -19,12 +21,14 @@ cia402_device_1: tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true + cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - - {index: 0x6061, sub_index: 0} # mode of operaiton display + - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true + cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value @@ -36,11 +40,13 @@ cia402_device_1: rpdo: # RPDO needed controlword, target position, target velocity, mode of operation 1: enabled: true + cob_id: "auto" mapping: - {index: 0x6040, sub_index: 0} # controlword - {index: 0x6060, sub_index: 0} # mode of operation 2: enabled: true + cob_id: "auto" mapping: - {index: 0x607A, sub_index: 0} # target position - - {index: 0x60FF, sub_index: 0} # target velocity \ No newline at end of file + - {index: 0x60FF, sub_index: 0} # target velocity diff --git a/canopen_tests/config/cia402_lifecycle/cia402_slave.eds b/canopen_tests/config/cia402_lifecycle/cia402_slave.eds index 444e66c8..d30fa434 100644 --- a/canopen_tests/config/cia402_lifecycle/cia402_slave.eds +++ b/canopen_tests/config/cia402_lifecycle/cia402_slave.eds @@ -1440,4 +1440,3 @@ DataType=0x0007 AccessType=ro DefaultValue=0x00040192 PDOMapping=0 - diff --git a/canopen_tests/config/robot_control/bus.yml b/canopen_tests/config/robot_control/bus.yml new file mode 100644 index 00000000..2f6ef611 --- /dev/null +++ b/canopen_tests/config/robot_control/bus.yml @@ -0,0 +1,95 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + +master: + node_id: 1 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + sync_period: 10000 + +joint_1: + node_id: 2 + dcf: "cia402_slave.eds" + driver: "ros2_canopen::Cia402Driver" + package: "canopen_402_driver" + period: 10 + position_mode: 1 + revision_number: 0 + sdo: + - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms + - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s + - {index: 0x6081, sub_index: 0, value: 1000} + - {index: 0x6083, sub_index: 0, value: 2000} + - {index: 0x6060, sub_index: 0, value: 7} + tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation + 1: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6041, sub_index: 0} # status word + - {index: 0x6061, sub_index: 0} # mode of operation display + 2: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6064, sub_index: 0} # position actual value + - {index: 0x606c, sub_index: 0} # velocity actual position + rpdo: # RPDO needed controlword, target position, target velocity, mode of operation + 1: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x6040, sub_index: 0} # controlword + - {index: 0x6060, sub_index: 0} # mode of operation + 2: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x607A, sub_index: 0} # target position + #- {index: 0x60FF, sub_index: 0} # target velocity + +joint_2: + node_id: 3 + dcf: "cia402_slave.eds" + driver: "ros2_canopen::Cia402Driver" + package: "canopen_402_driver" + period: 10 + position_mode: 1 + revision_number: 0 + switching_state: 2 + sdo: + - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms + - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s + - {index: 0x6081, sub_index: 0, value: 1000} + - {index: 0x6083, sub_index: 0, value: 2000} + - {index: 0x6060, sub_index: 0, value: 7} + tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation + 1: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6041, sub_index: 0} # status word + - {index: 0x6061, sub_index: 0} # mode of operation display + 2: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6064, sub_index: 0} # position actual value + - {index: 0x606c, sub_index: 0} # velocity actual position + rpdo: # RPDO needed controlword, target position, target velocity, mode of operation + 1: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x6040, sub_index: 0} # controlword + - {index: 0x6060, sub_index: 0} # mode of operation + 2: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x607A, sub_index: 0} # target position + #- {index: 0x60FF, sub_index: 0} # target velocity diff --git a/canopen_tests/config/robot_control/cia402_slave.eds b/canopen_tests/config/robot_control/cia402_slave.eds new file mode 100644 index 00000000..2918eda0 --- /dev/null +++ b/canopen_tests/config/robot_control/cia402_slave.eds @@ -0,0 +1,3368 @@ +[FileInfo] +CreatedBy=Eduard Prelipcean +ModifiedBy=Graure Iulian - Robert +Description=CANopen Technosoft +CreationTime=01:51PM +CreationDate=02-07-2022 +ModificationTime=01:51PM +ModificationDate=02-07-2022 +FileName=iPOS.D.v1.04.eds +FileVersion=0x01 +FileRevision=0x05 +EDSVersion=5 + +[DeviceInfo] +VendorName=Technosoft +VendorNumber=0x000001A3 +ProductName=iPOS +BaudRate_10=0 +BaudRate_20=0 +BaudRate_50=0 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=8 +DynamicChannelsSupported=0 +CompactPDO=0 +GroupMessaging=0 +NrOfRXPDO=4 +NrOfTXPDO=4 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[Comments] +Lines=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[1000] +ParameterName=Device type +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x60192 +PDOMapping=0 + + +[1001] +ParameterName=Error register +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + + +[1018] +ParameterName=Identity Object +ObjectType=0x9 +SubNumber=5 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=4 +PDOMapping=0 +LowLimit=1 +HighLimit=4 + +[1018sub1] +ParameterName=Vendor ID +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x000001A3 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[OptionalObjects] +SupportedObjects=102 +1=0x1002 +2=0x1003 +3=0x1005 +4=0x1006 +5=0x1008 +6=0x100A +7=0x100C +8=0x100D +9=0x1010 +10=0x1011 +11=0x1013 +12=0x1014 +13=0x1017 +14=0x1200 +15=0x1400 +16=0x1401 +17=0x1402 +18=0x1403 +19=0x1600 +20=0x1601 +21=0x1602 +22=0x1603 +23=0x1800 +24=0x1801 +25=0x1802 +26=0x1803 +27=0x1A00 +28=0x1A01 +29=0x1A02 +30=0x1A03 +31=0x6007 +32=0x6040 +33=0x6041 +34=0x605A +35=0x605B +36=0x605C +37=0x605D +38=0x605E +39=0x6060 +40=0x6061 +41=0x6062 +42=0x6063 +43=0x6064 +44=0x6065 +45=0x6066 +46=0x6067 +47=0x6068 +48=0x6069 +49=0x606B +50=0x606C +51=0x606F +52=0x6071 +53=0x6075 +54=0x6077 +55=0x607A +56=0x607B +57=0x607C +58=0x607D +59=0x607E +60=0x6081 +61=0x6083 +62=0x6085 +63=0x6086 +64=0x6087 +65=0x6089 +66=0x608A +67=0x608B +68=0x608C +69=0x608D +70=0x608E +71=0x608F +72=0x6091 +73=0x6092 +74=0x6093 +75=0x6094 +76=0x6096 +77=0x6097 +78=0x6098 +79=0x6099 +80=0x609A +81=0x60A2 +82=0x60A8 +83=0x60A9 +84=0x60AA +85=0x60AB +86=0x60B8 +87=0x60B9 +88=0x60BA +89=0x60BB +90=0x60BC +91=0x60BD +92=0x60C0 +93=0x60C1 +94=0x60C2 +95=0x60F2 +96=0x60F4 +97=0x60F8 +98=0x60FC +99=0x60FD +100=0x60FE +101=0x60FF +102=0x6502 + +[1002] +ParameterName=Manufacturer status register +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[1003] +ParameterName=Error Field +ObjectType=0x8 +SubNumber=6 + +[1003sub0] +ParameterName=Number of errors in history +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=0 +HighLimit=254 + +[1003sub1] +ParameterName=Standard error field +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[1003sub2] +ParameterName=Standard error field +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[1003sub3] +ParameterName=Standard error field +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[1003sub4] +ParameterName=Standard error field +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[1003sub5] +ParameterName=Standard error field +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC Message +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x80 +PDOMapping=0 + + +[1006] +ParameterName=Communication cycle period +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + + +[1008] +ParameterName=Manufacturer device name +ObjectType=0x7 +DataType=0x0009 +AccessType=const +DefaultValue=iPOS +PDOMapping=0 + + +[100A] +ParameterName=Manufacturer software version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 + + +[100C] +ParameterName=Guard time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + + +[100D] +ParameterName=Life time factor +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + + +[1010] +ParameterName=Store parameters +ObjectType=0x8 +SubNumber=2 + +[1010sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=1 +PDOMapping=0 +LowLimit=1 +HighLimit=1 + +[1010sub1] +ParameterName=Save all Parameters +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1011] +ParameterName=Restore default parameters +ObjectType=0x8 +SubNumber=2 + +[1011sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=1 +PDOMapping=0 +LowLimit=1 +HighLimit=1 + +[1011sub1] +ParameterName=Restore all default parameters +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1013] +ParameterName=High Resolution Time Stamp +ObjectType=0x7 +DataType=0x0007 +AccessType=rwr +DefaultValue=0x00000000 +PDOMapping=1 + + +[1014] +ParameterName=COB-ID EMCY Message +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 +PDOMapping=0 + + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + + +[1200] +ParameterName=Server SDO Parameter +ObjectType=0x9 +SubNumber=3 + +[1200sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=2 +HighLimit=2 + +[1200sub1] +ParameterName=COB-ID Client -> Server (rx) +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x600 +PDOMapping=0 + +[1200sub2] +ParameterName=COB-ID Server -> Client (tx) +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x580 +PDOMapping=0 + +[1400] +ParameterName=RPDO1 Communication Parameter +ObjectType=0x9 +SubNumber=3 + +[1400sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1400sub1] +ParameterName=COB-ID RPDO1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x200 +PDOMapping=0 + +[1400sub2] +ParameterName=Transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1401] +ParameterName=RPDO2 Communication Parameter +ObjectType=0x9 +SubNumber=3 + +[1401sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1401sub1] +ParameterName=COB-ID RPDO2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x300 +PDOMapping=0 + +[1401sub2] +ParameterName=Transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1402] +ParameterName=RPDO3 Communication Parameter +ObjectType=0x9 +SubNumber=3 + +[1402sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1402sub1] +ParameterName=COB-ID RPDO3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x400 +PDOMapping=0 + +[1402sub2] +ParameterName=Transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1403] +ParameterName=RPDO4 Communication Parameter +ObjectType=0x9 +SubNumber=3 + +[1403sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1403sub1] +ParameterName=COB-ID RPDO4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x500 +PDOMapping=0 + +[1403sub2] +ParameterName=Transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1600] +ParameterName=RPDO1 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1600sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1600sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1600sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601] +ParameterName=RPDO2 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1601sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1601sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1601sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60600008 +PDOMapping=0 + +[1601sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602] +ParameterName=RPDO3 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1602sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1602sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1602sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x607A0020 +PDOMapping=0 + +[1602sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603] +ParameterName=RPDO4 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1603sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1603sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1603sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60FF0020 +PDOMapping=0 + +[1603sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1800] +ParameterName=TPDO1 Communication Parameter +ObjectType=0x9 +SubNumber=5 + +[1800sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1800sub1] +ParameterName=COB-ID TPDO1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 +PDOMapping=0 + +[1800sub2] +ParameterName=Transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0x012C +PDOMapping=0 + +[1800sub5] +ParameterName=Event timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1801] +ParameterName=TPDO2 Communication Parameter +ObjectType=0x9 +SubNumber=5 + +[1801sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1801sub1] +ParameterName=COB-ID TPDO2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x280 +PDOMapping=0 + +[1801sub2] +ParameterName=Transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0x012C +PDOMapping=0 + +[1801sub5] +ParameterName=Event timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1802] +ParameterName=TPDO3 Communication Parameter +ObjectType=0x9 +SubNumber=5 + +[1802sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1802sub1] +ParameterName=COB-ID TPDO3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000380 +PDOMapping=0 + +[1802sub2] +ParameterName=Transmision type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1802sub3] +ParameterName=Inhibit Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0x012C +PDOMapping=0 + +[1802sub5] +ParameterName=Event timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1803] +ParameterName=TPDO4 Communication Parameter +ObjectType=0x9 +SubNumber=5 + +[1803sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +LowLimit=2 +HighLimit=5 + +[1803sub1] +ParameterName=COB-ID TPDO4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000480 +PDOMapping=0 + +[1803sub2] +ParameterName=Transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1803sub3] +ParameterName=Inhibit Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0x012C +PDOMapping=0 + +[1803sub5] +ParameterName=Event timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1A00] +ParameterName=TPDO1 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1A00sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1A00sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A00sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01] +ParameterName=TPDO2 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1A01sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1A01sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A01sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60610008 +PDOMapping=0 + +[1A01sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02] +ParameterName=TPDO3 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1A02sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1A02sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A02sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60640020 +PDOMapping=0 + +[1A02sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03] +ParameterName=TPDO4 Mapping Parameter +ObjectType=0x9 +SubNumber=9 + +[1A03sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1A03sub1] +ParameterName=Mapped object #1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A03sub2] +ParameterName=Mapped object #2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x606C0020 +PDOMapping=0 + +[1A03sub3] +ParameterName=Mapped object #3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub4] +ParameterName=Mapped object #4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub5] +ParameterName=Mapped object #5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub6] +ParameterName=Mapped object #6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub7] +ParameterName=Mapped object #7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub8] +ParameterName=Mapped object #8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[6007] +ParameterName=Abort connection option code +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +DefaultValue=0 +PDOMapping=0 +LowLimit=-32768 +HighLimit=32767 + + +[6040] +ParameterName=Control word +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[6041] +ParameterName=Status word +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[605A] +ParameterName=Quick stop option code +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=-32768 +HighLimit=32767 + +[605B] +ParameterName=Shutdown option code +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=-32768 +HighLimit=32767 + +[605C] +ParameterName=Disable operation option code +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +LowLimit=-32768 +HighLimit=32767 + +[605D] +ParameterName=Halt option code +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +LowLimit=-32768 +HighLimit=32767 + +[605E] +ParameterName=Fault reaction option code +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=-32768 +HighLimit=32767 + + +[6060] +ParameterName=Modes of Operation +ObjectType=0x7 +DataType=0x0002 +AccessType=rww +PDOMapping=1 +LowLimit=-128 +HighLimit=127 + + +[6061] +ParameterName=Modes of Operation Display +ObjectType=0x7 +DataType=0x0002 +AccessType=ro +PDOMapping=1 +LowLimit=-128 +HighLimit=127 + + +[6062] +ParameterName=Position demand value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + + +[6063] +ParameterName=Position actual internal value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + + +[6064] +ParameterName=Position actual value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + + +[6065] +ParameterName=Following error window +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + + +[6066] +ParameterName=Following error time out +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[6067] +ParameterName=Position window +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + + +[6068] +ParameterName=Position window time +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[6069] +ParameterName=Velocity sensor actual value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + + +[606B] +ParameterName=Velocity demand value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + + +[606C] +ParameterName=Velocity actual value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + + +[606F] +ParameterName=Velocity threshold +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + + +[6071] +ParameterName=Target torque +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +PDOMapping=1 + +[6075] +ParameterName=Motor rated current +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +HighLimit=4294967295 + +[6077] +ParameterName=Torque actual value +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[607A] +ParameterName=Target position +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + + +[607B] +ParameterName=Position range limit +ObjectType=0x8 +SubNumber=3 + +[607Bsub0] +ParameterName=Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[607Bsub1] +ParameterName=Min position range limit +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[607Bsub2] +ParameterName=Max position range limit +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[607C] +ParameterName=Home offset +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + + +[607D] +ParameterName=Software position limit +ObjectType=0x8 +SubNumber=3 + +[607Dsub0] +ParameterName=Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[607Dsub1] +ParameterName=Minimal position limit +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[607Dsub2] +ParameterName=Maximal position limit +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[607E] +ParameterName=Polarity +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + +[6081] +ParameterName=Profile velocity +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + + +[6083] +ParameterName=Profile acceleration +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=4294967295 + + +[6085] +ParameterName=Quick stop deceleration +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=4294967295 + + +[6086] +ParameterName=Motion profile type +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + + +[6087] +ParameterName=Torque slope +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +HighLimit=4294967295 + +[6089] +ParameterName=Position notation index +ObjectType=0x7 +DataType=0x0002 +AccessType=rww +DefaultValue=0 +PDOMapping=1 +LowLimit=-128 +HighLimit=127 + + +[608A] +ParameterName=Position domension index +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=255 + + +[608B] +ParameterName=Velocity notation index +ObjectType=0x7 +DataType=0x0002 +AccessType=rww +DefaultValue=0 +PDOMapping=1 +LowLimit=-128 +HighLimit=127 + + +[608C] +ParameterName=Velocity dimension index +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=255 + + +[608D] +ParameterName=Acceleration notation index +ObjectType=0x7 +DataType=0x0002 +AccessType=rww +PDOMapping=1 +LowLimit=-128 +HighLimit=127 + + +[608E] +ParameterName=Acceleration dimension index +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=255 + + +[608F] +ParameterName=Position encoder resolution +ObjectType=0x8 +SubNumber=3 + +[608Fsub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[608Fsub1] +ParameterName=Encoder increments +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[608Fsub2] +ParameterName=Motor revolutions +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6091] +ParameterName=Gear ratio +ObjectType=0x8 +SubNumber=3 + +[6091sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[6091sub1] +ParameterName=Motor shaft revolutions +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6091sub2] +ParameterName=Driving shaft revolutions +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6092] +ParameterName=Feed constant +ObjectType=0x8 +SubNumber=3 + +[6092sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[6092sub1] +ParameterName=Feed +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6092sub2] +ParameterName=Shaft revolutions +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6093] +ParameterName=Position factor +ObjectType=0x8 +SubNumber=3 + +[6093sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[6093sub1] +ParameterName=Position factor Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6093sub2] +ParameterName=Position factor Divisor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6094] +ParameterName=Velocity encoder factor +ObjectType=0x8 +SubNumber=3 + +[6094sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[6094sub1] +ParameterName=Velocity encoder factor Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6094sub2] +ParameterName=Velocity encoder factor Divisor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6096] +ParameterName=Velocity factor +ObjectType=0x8 +SubNumber=3 + +[6096sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[6096sub1] +ParameterName=Velocity factor Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6096sub2] +ParameterName=Velocity factor Divisor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6097] +ParameterName=Acceleration factor +ObjectType=0x8 +SubNumber=3 + +[6097sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[6097sub1] +ParameterName=Acceleration factor Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6097sub2] +ParameterName=Acceleration factor Divisor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[6098] +ParameterName=Homing method +ObjectType=0x7 +DataType=0x0002 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + + +[6099] +ParameterName=Homing speed +ObjectType=0x8 +SubNumber=3 + +[6099sub0] +ParameterName=number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[6099sub1] +ParameterName=Speed during search for switch +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + +[6099sub2] +ParameterName=Speed during search for zero +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + +[609A] +ParameterName=Homing acceleration +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + + +[60A2] +ParameterName=Jerk factor +ObjectType=0x8 +SubNumber=3 + +[60A2sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[60A2sub1] +ParameterName=Jerk factor Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[60A2sub2] +ParameterName=Jerk factor Divisor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[60A8] +ParameterName=SI unit position +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=4294967295 + +[60A9] +ParameterName=SI unit velocity +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=4294967295 + +[60AA] +ParameterName=SI unit acceleration +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=4294967295 + +[60AB] +ParameterName=SI Unit jerk +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=4294967295 + +[60B8] +ParameterName=Touch probe function +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[60B9] +ParameterName=Touch probe status +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 + +[60BA] +ParameterName=Touch probe 1 positive edge +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[60BB] +ParameterName=Touch probe 1 negative edge +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[60BC] +ParameterName=Touch probe 2 positive edge +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[60BD] +ParameterName=Touch probe 2 negative edge +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[60C0] +ParameterName=Interpolation sub mode select +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +DefaultValue=0 +PDOMapping=1 +LowLimit=-32768 +HighLimit=32767 + + +[60C1] +ParameterName=Interpolation data record +ObjectType=0x8 +SubNumber=3 + +[60C1sub0] +ParameterName=number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[60C1sub1] +ParameterName=X1: the first parameter of ip function +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[60C1sub2] +ParameterName=X2: the second parameter of ip function +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[60C2] +ParameterName=Interpolation time period +ObjectType=0x9 +SubNumber=3 + +[60C2sub0] +ParameterName=Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[60C2sub1] +ParameterName=Interpolation time period value +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 + +[60C2sub2] +ParameterName=Interpolation time index +ObjectType=0x7 +DataType=0x0002 +AccessType=rww +PDOMapping=1 + +[60F2] +ParameterName=Positioning option code +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[60F4] +ParameterName=Following error actual value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + + +[60F8] +ParameterName=Max slippage +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + + +[60FC] +ParameterName=Position demand internal value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + + +[60FD] +ParameterName=Digital inputs +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=1 + + +[60FE] +ParameterName=Digital outputs +ObjectType=0x8 +SubNumber=3 + +[60FEsub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=1 +HighLimit=2 + +[60FEsub1] +ParameterName=Physical outputs +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + +[60FEsub2] +ParameterName=Bit mask +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + +[60FF] +ParameterName=Target velocity +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + + +[6502] +ParameterName=Supported drive modes +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x001F0065 +PDOMapping=1 + + +[ManufacturerObjects] +SupportedObjects=84 +1=0x2000 +2=0x2002 +3=0x2003 +4=0x2004 +5=0x2005 +6=0x2006 +7=0x2009 +8=0x2010 +9=0x2012 +10=0x2013 +11=0x2017 +12=0x2018 +13=0x2019 +14=0x201A +15=0x201B +16=0x201C +17=0x201D +18=0x2022 +19=0x2023 +20=0x2025 +21=0x2026 +22=0x2027 +23=0x2045 +24=0x2046 +25=0x2047 +26=0x2050 +27=0x2051 +28=0x2052 +29=0x2053 +30=0x2054 +31=0x2055 +32=0x2058 +33=0x2060 +34=0x2064 +35=0x2065 +36=0x2066 +37=0x2067 +38=0x2069 +39=0x206A +40=0x206B +41=0x206C +42=0x206F +43=0x2070 +44=0x2071 +45=0x2072 +46=0x2073 +47=0x2074 +48=0x2075 +49=0x2076 +50=0x2077 +51=0x2079 +52=0x207A +53=0x207B +54=0x207C +55=0x207D +56=0x207F +57=0x2081 +58=0x2083 +59=0x2084 +60=0x2085 +61=0x2086 +62=0x2087 +63=0x2088 +64=0x208B +65=0x208C +66=0x208D +67=0x208E +68=0x208F +69=0x2090 +70=0x2091 +71=0x2092 +72=0x20A0 +73=0x2100 +74=0x2101 +75=0x2102 +76=0x2103 +77=0x2104 +78=0x2105 +79=0x2106 +80=0x2107 +81=0x2108 +82=0x210B +83=0x210F +84=0x2110 + +[2000] +ParameterName=Manufacturer Error Register +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +DefaultValue=0 +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2002] +ParameterName=Detailed Error Register +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +DefaultValue=0 +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + +[2003] +ParameterName=Communication Error Register +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +DefaultValue=0 +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + +[2004] +ParameterName=COB-ID High resolution time stamp +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x100 +PDOMapping=0 + + +[2005] +ParameterName=Max slippage time out +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=0 + + +[2006] +ParameterName=Call TML function +ObjectType=0x7 +DataType=0x0006 +AccessType=wo +PDOMapping=0 +LowLimit=1 +HighLimit=10 + + +[2009] +ParameterName=Detailed Error Register 2 +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +DefaultValue=0 +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + +[2010] +ParameterName=Master settings +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +DefaultValue=0 +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2012] +ParameterName=Master resolution +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + + +[2013] +ParameterName=EGEAR multiplication factor +ObjectType=0x9 +SubNumber=3 + +[2013sub0] +ParameterName=Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[2013sub1] +ParameterName=EGEAR ratio numerator (slave) +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +PDOMapping=1 + +[2013sub2] +ParameterName=EGEAR ratio denominator (master) +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[2017] +ParameterName=Master actual position +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + + +[2018] +ParameterName=Master actual speed +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + + +[2019] +ParameterName=CAM table load adress +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + + +[201A] +ParameterName=CAM table run adress +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + + +[201B] +ParameterName=CAM offset +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=0 +HighLimit=4294967295 + + +[201C] +ParameterName=External online reference +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + + +[201D] +ParameterName=External Reference Type +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + + +[2022] +ParameterName=Control effort +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + + +[2023] +ParameterName=Jerk time +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2025] +ParameterName=Stepper current in open-loop operation +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +PDOMapping=1 +LowLimit=-32768 +HighLimit=32767 + + +[2026] +ParameterName=Stand-by current for stepper in open-loop operation +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +PDOMapping=1 +LowLimit=-32768 +HighLimit=32767 + + +[2027] +ParameterName=Time out for stepper stand-by current +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2045] +ParameterName=Digital outputs status +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2046] +ParameterName=Analogue input: Reference +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2047] +ParameterName=Analogue input: Feedback +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2050] +ParameterName=Over-current protection level +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=32767 + + +[2051] +ParameterName=Over-current time out +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=0 +LowLimit=0 +HighLimit=65535 + + +[2052] +ParameterName=Motor nominal current +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 +LowLimit=0 +HighLimit=32767 + + +[2053] +ParameterName=I2t protection integrator limit +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +LowLimit=0 +HighLimit=2147483647 + + +[2054] +ParameterName=I2t protection scaling factor +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 +LowLimit=0 +HighLimit=65535 + + +[2055] +ParameterName=Analogue input: DC-link voltage +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + + +[2058] +ParameterName=Analogue input for drive temperature +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 +LowLimit=0 +HighLimit=65535 + + +[2060] +ParameterName=Software version of TML application +ObjectType=0x7 +DataType=0x0009 +AccessType=ro +PDOMapping=0 + + +[2064] +ParameterName=Read/Write configuration register +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=0x84 +PDOMapping=1 +LowLimit=0 +HighLimit=2147483647 + + +[2065] +ParameterName=Write data at address set in 0x2064 +ObjectType=0x7 +DataType=0x0007 +AccessType=wo +PDOMapping=1 +LowLimit=0 +HighLimit=2147483647 + + +[2066] +ParameterName=Read data from address set in 0x2064 +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=1 + + +[2067] +ParameterName=Write data at specified address +ObjectType=0x7 +DataType=0x0007 +AccessType=wo +PDOMapping=1 + + +[2069] +ParameterName=Checksum configuration register +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + + +[206A] +ParameterName=Checksum read register +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 + + +[206B] +ParameterName=CAM input scaling factor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=0x00010000 +PDOMapping=1 + + +[206C] +ParameterName=CAM output scaling factor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=0x00010000 +PDOMapping=1 + + +[206F] +ParameterName=Time notation index +ObjectType=0x7 +DataType=0x0002 +AccessType=rww +PDOMapping=1 +LowLimit=-128 +HighLimit=127 + + +[2070] +ParameterName=Time dimension index +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=255 + + +[2071] +ParameterName=Time factor +ObjectType=0x8 +SubNumber=3 + +[2071sub0] +ParameterName=number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[2071sub1] +ParameterName=Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 + +[2071sub2] +ParameterName=Feed constant +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 + +[2072] +ParameterName=Interpolated position mode status +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 + + +[2073] +ParameterName=Interpolated position buffer length +ObjectType=0x7 +DataType=0x0006 +AccessType=wo +DefaultValue=7 +PDOMapping=0 + + +[2074] +ParameterName=Interpolated position buffer configuration +ObjectType=0x7 +DataType=0x0006 +AccessType=wo +PDOMapping=0 + + +[2075] +ParameterName=Position triggers +ObjectType=0x8 +SubNumber=5 + +[2075sub0] +ParameterName=Number of sub-indexes +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=4 +PDOMapping=0 + +[2075sub1] +ParameterName=Position trigger 1 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2075sub2] +ParameterName=Position trigger 2 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2075sub3] +ParameterName=Position trigger 3 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2075sub4] +ParameterName=Position trigger 4 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2076] +ParameterName=Save current configuration +ObjectType=0x7 +DataType=0x0006 +AccessType=wo +PDOMapping=0 + + +[2077] +ParameterName=Execute TML program +ObjectType=0x7 +DataType=0x0006 +AccessType=wo +PDOMapping=0 + + +[2079] +ParameterName=Interpolated position initial position +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + + +[207A] +ParameterName=Interpolated Position 1st order time +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[207B] +ParameterName=Homing current threshold +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +PDOMapping=1 + +[207C] +ParameterName=Homing current threshold time +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[207D] +ParameterName=Dummy +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=255 + +[207F] +ParameterName=Current limit +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 +LowLimit=0 +HighLimit=65535 + +[2081] +ParameterName=Set/change the actual motor position +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=0 + +[2083] +ParameterName=Encoder resolution +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2084] +ParameterName=Stepper resolution +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2085] +ParameterName=Position triggered outputs +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=0 +LowLimit=0 +HighLimit=65535 + +[2086] +ParameterName=Speed Limitation for CSP +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +PDOMapping=0 + +[2087] +ParameterName=Motor Speed +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[2088] +ParameterName=Motor Position +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[208B] +ParameterName=Sin signal AD value +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[208C] +ParameterName=Cos signal AD value +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[208D] +ParameterName=Auxiliary encoder position +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[208E] +ParameterName=Auxiliary Settings Register +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=0 + +[208F] +ParameterName=Digital inputs 8b +ObjectType=0x9 +SubNumber=3 + +[208Fsub0] +ParameterName=Copy of Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[208Fsub1] +ParameterName=Copy of Digital inputs 8b Lo +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[208Fsub2] +ParameterName=Copy of Digital Inputs 8b Hi +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2090] +ParameterName=Digital outputs 8b +ObjectType=0x9 +SubNumber=3 + +[2090sub0] +ParameterName=Copy of Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[2090sub1] +ParameterName=Copy of Physical outputs 8b +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 + +[2090sub2] +ParameterName=Copy of Bit mask 8b +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 + +[2091] +ParameterName=Lock EEPROM +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=0 +LowLimit=0 +HighLimit=255 + +[2092] +ParameterName=User Variables +ObjectType=0x8 +SubNumber=5 + +[2092sub0] +ParameterName=Number of sub-indexes +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=4 +PDOMapping=0 + +[2092sub1] +ParameterName=UserVar1 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2092sub2] +ParameterName=UserVar2 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2092sub3] +ParameterName=UserVar3 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[2092sub4] +ParameterName=UserVar4 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[20A0] +ParameterName=Load Position and Speed monitoring +ObjectType=0x9 +SubNumber=4 + +[20A0sub0] +ParameterName=Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 + +[20A0sub1] +ParameterName=Reserved +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + +[20A0sub2] +ParameterName=Load Position Monitor +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + +[20A0sub3] +ParameterName=Load Speed Monitor +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +LowLimit=-2147483648 +HighLimit=2147483647 + +[2100] +ParameterName=Number of Steps per Revolution +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2101] +ParameterName=Number of microSteps per Step +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2102] +ParameterName=Brake status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 +LowLimit=0 +HighLimit=1 + +[2103] +ParameterName=Number of encoder Counts per Revolution +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[2104] +ParameterName=Auxiliary encoder function +ObjectType=0x7 +DataType=0x0005 +AccessType=rww +PDOMapping=1 + +[2105] +ParameterName=Auxiliary encoder status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2106] +ParameterName=Auxiliary encoder captured position positive edge +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[2107] +ParameterName=Auxiliary encoder captured position negative edge +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[2108] +ParameterName=Filter variable 16 bit +ObjectType=0x9 +SubNumber=4 + +[2108sub0] +ParameterName=Number of elements +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 + +[2108sub1] +ParameterName=16bit variable address +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[2108sub2] +ParameterName=Filter strength +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[2108sub3] +ParameterName=Filtered variable 16bit +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[210B] +ParameterName=Auxiliary Settings Register 2 +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=0 + +[210F] +ParameterName=Acceleration encoder factor +ObjectType=0x8 +SubNumber=3 + +[210Fsub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[210Fsub1] +ParameterName=Acceleration encoder factor Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[210Fsub2] +ParameterName=Acceleration encoder factor Divisor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[2110] +ParameterName=Jerk encoder factor +ObjectType=0x8 +SubNumber=3 + +[2110sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[2110sub1] +ParameterName=Jerk encoder factor Numerator +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 + +[2110sub2] +ParameterName=Jerk encoder factor Divisor +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +DefaultValue=1 +PDOMapping=1 +HighLimit=4294967295 diff --git a/canopen_tests/config/robot_control/prbt_0_1.dcf b/canopen_tests/config/robot_control/prbt_0_1.dcf new file mode 100644 index 00000000..090290b1 --- /dev/null +++ b/canopen_tests/config/robot_control/prbt_0_1.dcf @@ -0,0 +1,2052 @@ +[FileInfo] +CreatedBy=Dirk Osswald, SCHUNK GmbH & Co. KG +ModifiedBy=Pilz GmbH & Co. KG +Description=PRBT Electronic Data Sheet +CreationTime=01:33PM +CreationDate=05-23-2013 +ModificationTime=00:00PM +ModificationDate=09-07-2018 +FileName=prbt_0_1.dcf +FileVersion=0x03 +FileRevision=0x01 +EDSVersion=4.0 + +[DeviceInfo] +VendorName=Pilz GmbH & Co. KG +OrderCode=0 +BaudRate_10=0 +BaudRate_20=0 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=8 +DynamicChannelsSupported=0 +CompactPDO=0 +GroupMessaging=0 +NrOfRXPDO=2 +NrOfTXPDO=4 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=0 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 + +[Comments] +Lines=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[1000] +ParameterName=Device type +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +ObjFlags=0x0 + +[1001] +ParameterName=Error register +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=0 +ObjFlags=0x0 + +[1018] +ParameterName=Identity Object +ObjectType=0x9 +SubNumber=5 +ObjFlags=0x0 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=4 +PDOMapping=0 +LowLimit=1 +HighLimit=4 +ObjFlags=0 + +[1018sub1] +ParameterName=Vendor ID +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +ObjFlags=0 + +[1018sub2] +ParameterName=Product code +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +ObjFlags=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +ObjFlags=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +ObjFlags=0 + +[OptionalObjects] +SupportedObjects=62 +1=0x1002 +2=0x1003 +3=0x1005 +4=0x1008 +5=0x1009 +6=0x100A +7=0x100C +8=0x100D +9=0x1014 +10=0x1016 +11=0x1017 +12=0x1200 +13=0x1400 +14=0x1401 +15=0x1600 +16=0x1601 +17=0x1800 +18=0x1801 +19=0x1802 +20=0x1803 +21=0x1A00 +22=0x1A01 +23=0x1A02 +24=0x1A03 +25=0x20A0 +26=0x6040 +27=0x6041 +28=0x6042 +29=0x6043 +30=0x6044 +31=0x6046 +32=0x6048 +33=0x6049 +34=0x604C +35=0x6060 +36=0x6061 +37=0x6062 +38=0x6064 +39=0x6065 +40=0x606B +41=0x606C +42=0x6073 +43=0x6075 +44=0x6077 +45=0x607A +46=0x607C +47=0x607D +48=0x6081 +49=0x6083 +50=0x6086 +51=0x6098 +52=0x60B2 +53=0x60C0 +54=0x60C1 +55=0x60C2 +56=0x60C3 +57=0x60C4 +58=0x60F4 +59=0x60FD +60=0x60FE +61=0x6502 +62=0x2060 + +[1002] +ParameterName=Manufacturer Status Register +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1003] +ParameterName=Pre-defined Error Field +ObjectType=0x8 +SubNumber=3 +ObjFlags=0x0 + +[1003sub0] +ParameterName=Number of Errors +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0 + +[1003sub1] +ParameterName=Standard Error Field +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=2 + +[1003sub2] +ParameterName=Standard Error Field +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=2 + +[1005] +ParameterName=COB-ID SYNC message +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 +ObjFlags=0x0 + +[1008] +ParameterName=Manufacturer device name +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 +ObjFlags=0x0 + +[1009] +ParameterName=Manufacturer hardware version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 +ObjFlags=0x0 + +[100A] +ParameterName=Manufacturer software version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 +ObjFlags=0x0 + +[100C] +ParameterName=Guard time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 +ObjFlags=0x0 + +[100D] +ParameterName=Life time factor +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 +ObjFlags=0x0 + +[1014] +ParameterName=COB-ID EMCY message +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NodeID + 0x80 +PDOMapping=0 +ObjFlags=0x0 + +[1016] +ParameterName=Consumer Heartbeat Time +ObjectType=0x8 +SubNumber=4 +ObjFlags=0x0 + +[1016sub0] +ParameterName=Number of Entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 +LowLimit=1 +HighLimit=127 +ObjFlags=0 + +[1016sub1] +ParameterName=Consumer Heartbeat Time of Node-ID 1 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0 + +[1016sub2] +ParameterName=Consumer Heartbeat Time of Node-ID 2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0 + +[1016sub3] +ParameterName=Consumer Heartbeat Time of Node-ID 3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0 + +[1017] +ParameterName=Producer heartbeat time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 +LowLimit=0 +HighLimit=32767 +ObjFlags=0x0 +ParameterValue=100 + +[1200] +ParameterName=1. Server SDO parameter +ObjectType=0x9 +SubNumber=3 +ObjFlags=0x0 + +[1200sub0] +ParameterName=Number of supported Entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0 + +[1200sub1] +ParameterName=COB-ID Client -> Server (rx) +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NodeID + 0x600 +PDOMapping=0 +ObjFlags=0 + +[1200sub2] +ParameterName=COB-ID Server -> Client (tx) +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NodeID + 0x580 +PDOMapping=0 +ObjFlags=0 + +[1400] +ParameterName=1. Receive PDO parameter +ObjectType=0x9 +SubNumber=3 +ObjFlags=0x0 + +[1400sub0] +ParameterName=Largest Sub-Index supported +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0 + +[1400sub1] +ParameterName=COB-ID used by PDO +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NodeID + 0x200 +PDOMapping=0 +ObjFlags=0 +ParameterValue=$NODEID+0x200 + +[1400sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x1 + +[1401] +ParameterName=2. Receive PDO Parameter +ObjectType=0x9 +SubNumber=3 +ObjFlags=0x0 + +[1401sub0] +ParameterName=Largest Sub-Index supported +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0 + +[1401sub1] +ParameterName=COB-ID used by PDO +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NodeID + 0x300 +PDOMapping=0 +ObjFlags=0 +ParameterValue=$NODEID+0x300 + +[1401sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x1 + +[1600] +ParameterName=1. Receive PDO Mapping +ObjectType=0x9 +SubNumber=9 +ObjFlags=0x0 + +[1600sub0] +ParameterName=Number of mapped Application Objects +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 +ObjFlags=0 +ParameterValue=3 + +[1600sub1] +ParameterName=1. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x60400010 + +[1600sub2] +ParameterName=2. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60c10120 +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x60420010 + +[1600sub3] +ParameterName=3. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x60c10120 + +[1600sub4] +ParameterName=4. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1600sub5] +ParameterName=5. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1600sub6] +ParameterName=6. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1600sub7] +ParameterName=7. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1600sub8] +ParameterName=8. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1601] +ParameterName=2. Receive PDO mapping +ObjectType=0x9 +SubNumber=9 +ObjFlags=0x0 + +[1601sub0] +ParameterName=Number of mapped Application Objects +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=0 +HighLimit=8 +ObjFlags=0 +ParameterValue=2 + +[1601sub1] +ParameterName=1. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x607a0020 + +[1601sub2] +ParameterName=2. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x60810020 + +[1601sub3] +ParameterName=3. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1601sub4] +ParameterName=4. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1601sub5] +ParameterName=5. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1601sub6] +ParameterName=6. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1601sub7] +ParameterName=7. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1601sub8] +ParameterName=8. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1800] +ParameterName=1. Transmit PDO Parameter +ObjectType=0x9 +SubNumber=4 +ObjFlags=0x0 + +[1800sub0] +ParameterName=Largest Sub-Index supported +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +ObjFlags=0 + +[1800sub1] +ParameterName=COB-ID used by PDO +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NodeID + 0x180 +PDOMapping=0 +ObjFlags=0 +ParameterValue=$NODEID+0x180 + +[1800sub2] +ParameterName=transmission type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x1 + +[1800sub5] +ParameterName=event timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=0 +HighLimit=32767 +ObjFlags=0 + +[1801] +ParameterName=2. Transmit PDO Parameter +ObjectType=0x9 +SubNumber=4 +ObjFlags=0x0 + +[1801sub0] +ParameterName=Largest Sub-Index supported +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +ObjFlags=0 + +[1801sub1] +ParameterName=COB-ID used by PDO +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NodeID + 0x280 +PDOMapping=0 +ObjFlags=0 + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +ObjFlags=0 + +[1801sub5] +ParameterName=event timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0 + +[1802] +ParameterName=3. Transmit PDO Parameter +ObjectType=0x9 +SubNumber=4 + +[1802sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x06 + +[1802sub1] +ParameterName=COB-ID used by PDO +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NodeID + 0x380 +PDOMapping=0 +ObjFlags=0 +ParameterValue=$NODEID+0x380 + +[1802sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +ParameterValue=0x1 + +[1802sub5] +ParameterName=Event Timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1803] +ParameterName=4. Transmit PDO Parameter +ObjectType=0x9 +SubNumber=4 + +[1803sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x06 + +[1803sub1] +ParameterName=COB-ID used by PDO +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NodeID + 0x480 +PDOMapping=0 +ObjFlags=0 + +[1803sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=254 +PDOMapping=0 + +[1803sub5] +ParameterName=Event Timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1A00] +ParameterName=1. Transmit PDO Mapping +ObjectType=0x9 +SubNumber=9 +ObjFlags=0x0 + +[1A00sub0] +ParameterName=Number of mapped Application Objects +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=3 +PDOMapping=0 +LowLimit=0 +HighLimit=8 +ObjFlags=0 +ParameterValue=2 + +[1A00sub1] +ParameterName=1. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x60410010 + +[1A00sub2] +ParameterName=2. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60770010 +PDOMapping=0 +ObjFlags=0 +ParameterValue=0x60610008 + +[1A00sub3] +ParameterName=3. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60640020 +PDOMapping=0 +ObjFlags=0 + +[1A00sub4] +ParameterName=4. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A00sub5] +ParameterName=5. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A00sub6] +ParameterName=6. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A00sub7] +ParameterName=7. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A00sub8] +ParameterName=8. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A01] +ParameterName=2. Transmit PDO Mapping +ObjectType=0x9 +SubNumber=9 +ObjFlags=0x0 + +[1A01sub0] +ParameterName=Number of mapped Application Objects +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +LowLimit=0 +HighLimit=8 +ObjFlags=0 + +[1A01sub1] +ParameterName=1. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x606b0020 +PDOMapping=0 +ObjFlags=0 + +[1A01sub2] +ParameterName=2. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x606c0020 +PDOMapping=0 +ObjFlags=0 + +[1A01sub3] +ParameterName=3. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A01sub4] +ParameterName=4. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A01sub5] +ParameterName=5. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A01sub6] +ParameterName=6. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A01sub7] +ParameterName=7. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A01sub8] +ParameterName=8. mapped Object +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0 + +[1A02] +ParameterName=3. Transmit PDO Mapping +ObjectType=0x9 +SubNumber=9 + +[1A02sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=0 +HighLimit=8 +ParameterValue=2 + +[1A02sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ParameterValue=0x60640020 + +[1A02sub2] +ParameterName=PDO Mapping Entry_2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ParameterValue=0x606c0020 + +[1A02sub3] +ParameterName=PDO Mapping Entry_3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A02sub4] +ParameterName=PDO Mapping Entry_4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A02sub5] +ParameterName=PDO Mapping Entry_5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A02sub6] +ParameterName=PDO Mapping Entry_6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A02sub7] +ParameterName=PDO Mapping Entry_7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A02sub8] +ParameterName=PDO Mapping Entry_8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03] +ParameterName=4. Transmit PDO Mapping +ObjectType=0x9 +SubNumber=9 + +[1A03sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=0 +HighLimit=8 + +[1A03sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03sub2] +ParameterName=PDO Mapping Entry_2 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03sub3] +ParameterName=PDO Mapping Entry_3 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03sub4] +ParameterName=PDO Mapping Entry_4 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03sub5] +ParameterName=PDO Mapping Entry_5 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03sub6] +ParameterName=PDO Mapping Entry_6 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03sub7] +ParameterName=PDO Mapping Entry_7 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1A03sub8] +ParameterName=PDO Mapping Entry_8 +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6040] +ParameterName=controlword +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[6041] +ParameterName=statusword +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=1 + +[6042] +ParameterName=vl_target_velocity +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +DefaultValue=0 +PDOMapping=1 + +[6043] +ParameterName=vl_velocity_demand +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[6044] +ParameterName=vl_velocity_actual_value +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[6046] +ParameterName=vl_velocity_min_max_amount +ObjectType=0x8 +SubNumber=3 + +[6046sub0] +ParameterName=vl_velocity_min_max_amount_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[6046sub1] +ParameterName=vl_velocity_min_max_amount_vl_velocity_min_amount +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[6046sub2] +ParameterName=vl_velocity_min_max_amount_vl_velocity_max_amount +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[6048] +ParameterName=vl_velocity_acceleration +ObjectType=0x9 +SubNumber=3 + +[6048sub0] +ParameterName=vl_velocity_acceleration_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[6048sub1] +ParameterName=vl_velocity_acceleration_delta_speed +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[6048sub2] +ParameterName=vl_velocity_acceleration_delta_time +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[6049] +ParameterName=vl_velocity_deceleration +ObjectType=0x9 +SubNumber=3 + +[6049sub0] +ParameterName=vl_velocity_deceleration_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[6049sub1] +ParameterName=vl_velocity_deceleration_delta_speed +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[6049sub2] +ParameterName=vl_velocity_deceleration_delta_time +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[604C] +ParameterName=vl_dimension_factor +ObjectType=0x8 +SubNumber=3 + +[604Csub0] +ParameterName=vl_dimension_factor_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[604Csub1] +ParameterName=vl_dimension_factor_numerator +ObjectType=0x7 +DataType=0x0004 +AccessType=rw +DefaultValue=1 +PDOMapping=0 + +[604Csub2] +ParameterName=vl_dimension_factor_denominator +ObjectType=0x7 +DataType=0x0004 +AccessType=rw +DefaultValue=6000 +PDOMapping=0 + +[6060] +ParameterName=modes_of_operation +ObjectType=0x7 +DataType=0x0002 +AccessType=rw +PDOMapping=1 +ParameterValue=7 + +[6061] +ParameterName=modes_of_operation_display +ObjectType=0x7 +DataType=0x0002 +AccessType=ro +PDOMapping=1 +ObjFlags=0x1 + +[6062] +ParameterName=position_demand_value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6064] +ParameterName=position_actual_value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6065] +ParameterName=following_error_window +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[606B] +ParameterName=velocity_demand_value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[606C] +ParameterName=velocity_actual_value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6073] +ParameterName=max_current +ObjectType=0x7 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[6075] +ParameterName=motor_rated_current +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6077] +ParameterName=torque_actual_value +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[607A] +ParameterName=target_position +ObjectType=0x7 +DataType=0x0004 +AccessType=rw +PDOMapping=1 + +[607C] +ParameterName=home_offset +ObjectType=0x7 +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[607D] +ParameterName=software_position_limit +ObjectType=0x8 +SubNumber=3 + +[607Dsub0] +ParameterName=software_position_limit_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[607Dsub1] +ParameterName=software_position_limit_min_position_limit +ObjectType=0x7 +DataType=0x0004 +AccessType=rw + +[607Dsub2] +ParameterName=software_position_limit_max_position_limit +ObjectType=0x7 +DataType=0x0004 +AccessType=rw + +[6081] +ParameterName=profile_velocity +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=1 +DefaultValue=0x2710 + +[6083] +ParameterName=profile_acceleration +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=1 + +[6086] +ParameterName=motion_profile_type +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +PDOMapping=0 + +[6098] +ParameterName=homing_method +ObjectType=0x7 +DataType=0x0002 +AccessType=const +DefaultValue=0 +PDOMapping=0 +LowLimit=-128 +HighLimit=35 +ParameterValue=0 + +[60B2] +ParameterName=torque_offset +ObjectType=0x7 +DataType=0x0003 +AccessType=rww +PDOMapping=1 + +[60C0] +ParameterName=interpolation_sub_mode_select +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=-1 +HighLimit=0 +ParameterValue=0 + +[60C1] +ParameterName=interpolation_data_record +ObjectType=0x8 +SubNumber=2 + +[60C1sub0] +ParameterName=interpolation_data_record_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=1 +PDOMapping=0 + +[60C1sub1] +ParameterName=interpolation_data_record_setpoint_1 +ObjectType=0x7 +DataType=0x0004 +AccessType=rww +PDOMapping=1 + +[60C2] +ParameterName=interpolation_time_period +ObjectType=0x9 +SubNumber=3 + +[60C2sub0] +ParameterName=interpolation_time_period_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[60C2sub1] +ParameterName=interpolation_time_period_value +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=8 +PDOMapping=0 +ParameterValue=10 + +[60C2sub2] +ParameterName=interpolation_time_period_index +ObjectType=0x7 +DataType=0x0002 +AccessType=rw +DefaultValue=-3 +PDOMapping=0 + +[60C3] +ParameterName=interpolation_sync_definition +ObjectType=0x8 +SubNumber=3 + +[60C3sub0] +ParameterName=interpolation_sync_definition_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[60C3sub1] +ParameterName=syncronize_on_group +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[60C3sub2] +ParameterName=ip_sync_every_n_event +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +LowLimit=0 +HighLimit=255 + +[60C4] +ParameterName=interpolation_data_configuration +ObjectType=0x9 +SubNumber=7 + +[60C4sub0] +ParameterName=interpolation_data_configuration_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=6 +PDOMapping=0 + +[60C4sub1] +ParameterName=interpolation_data_configuration_maximum_buffer_size +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=1 +PDOMapping=0 + +[60C4sub2] +ParameterName=interpolation_data_configuration_actual_buffer_size +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[60C4sub3] +ParameterName=interpolation_data_configuration_buffer_organization +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +LowLimit=0 +HighLimit=1 + +[60C4sub4] +ParameterName=interpolation_data_configuration_buffer_position +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[60C4sub5] +ParameterName=interpolation_data_configuration_size_of_data_record +ObjectType=0x7 +DataType=0x0005 +AccessType=wo +DefaultValue=1 +PDOMapping=0 + +[60C4sub6] +ParameterName=interpolation_data_configuration_buffer_clear +ObjectType=0x7 +DataType=0x0005 +AccessType=wo +DefaultValue=0 +PDOMapping=0 + +[60F4] +ParameterName=following_error_actual_value +ObjectType=0x7 +DataType=0x0004 +AccessType=ro +PDOMapping=1 +ObjFlags=0x0 + +[60FD] +ParameterName=digital_inputs +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=1 +ObjFlags=0x2 + +[60FE] +ParameterName=digital_outputs +ObjectType=0x8 +SubNumber=3 + +[60FEsub0] +ParameterName=digital_outputs_highest_subindex +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 + +[60FEsub1] +ParameterName=digital_outputs_physical_outputs +ObjectType=0x7 +DataType=0x0007 +AccessType=rww +PDOMapping=1 +ObjFlags=2 + +[60FEsub2] +ParameterName=digital_outputs_bit_mask +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6502] +ParameterName=supported_drive_modes +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000043 +PDOMapping=0 + +[ManufacturerObjects] +SupportedObjects=29 +1=0x2000 +2=0x2001 +3=0x2002 +4=0x2003 +5=0x2004 +6=0x2007 +7=0x2008 +8=0x2009 +9=0x200A +10=0x200B +11=0x200C +12=0x200D +13=0x200E +14=0x2010 +15=0x2011 +16=0x2012 +17=0x2013 +18=0x2020 +19=0x2021 +20=0x2022 +21=0x2023 +22=0x2024 +23=0x2030 +24=0x2031 +25=0x2032 +26=0x2033 +27=0x2034 +28=0x2041 +29=0x2050 + +[2000] +ParameterName=manufacturer_debug_flags +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x0 + +[2001] +ParameterName=debug_values_int32 +ObjectType=0x8 +SubNumber=5 + +[2001sub0] +ParameterName=nb_debug_values_int32 +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=4 +PDOMapping=0 + +[2001sub1] +ParameterName=debug_value_int32_1 +ObjectType=0x7 +DataType=0x0004 +AccessType=rwr +PDOMapping=1 + +[2001sub2] +ParameterName=debug_value_int32_2 +ObjectType=0x7 +DataType=0x0004 +AccessType=rwr +PDOMapping=1 + +[2001sub3] +ParameterName=debug_value_int32_3 +ObjectType=0x7 +DataType=0x0004 +AccessType=rwr +PDOMapping=1 + +[2001sub4] +ParameterName=debug_value_int32_4 +ObjectType=0x7 +DataType=0x0004 +AccessType=rwr +PDOMapping=1 + +[2002] +ParameterName=debug_values_float +ObjectType=0x8 +SubNumber=5 + +[2002sub0] +ParameterName=nb_debug_values_float +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=4 +PDOMapping=0 + +[2002sub1] +ParameterName=debug_value_float_1 +ObjectType=0x7 +DataType=0x0008 +AccessType=rwr +PDOMapping=1 + +[2002sub2] +ParameterName=debug_value_float_2 +ObjectType=0x7 +DataType=0x0008 +AccessType=rwr +PDOMapping=1 + +[2002sub3] +ParameterName=debug_value_float_3 +ObjectType=0x7 +DataType=0x0008 +AccessType=rwr +PDOMapping=1 + +[2002sub4] +ParameterName=debug_value_float_4 +ObjectType=0x7 +DataType=0x0008 +AccessType=rwr +PDOMapping=1 + +[2003] +ParameterName=debug_message +ObjectType=0x7 +DataType=0x0009 +AccessType=ro +PDOMapping=1 + +[2004] +ParameterName=logging +ObjectType=0x9 +SubNumber=3 +ObjFlags=0x3 + +[2004sub0] +ParameterName=nb_logging +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=3 + +[2004sub1] +ParameterName=logging_state +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0x8001 +PDOMapping=0 +ObjFlags=3 + +[2004sub2] +ParameterName=logging_trigger_time +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 +ObjFlags=3 + +[2007] +ParameterName=EEPROM_Parameters +ObjectType=0x9 +SubNumber=2 + +[2007sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=1 +PDOMapping=0 + +[2007sub1] +ParameterName=SECT_CONFIG +ObjectType=0x7 +DataType=0x000a +AccessType=ro +PDOMapping=0 +ObjFlags=3 + +[2008] +ParameterName=Password +ObjectType=0x7 +DataType=0x0009 +AccessType=wo +PDOMapping=0 +ObjFlags=0x1 + +[2009] +ParameterName=User +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[200A] +ParameterName=Disconnect_Reset +ObjectType=0x7 +DataType=0x0005 +AccessType=wo +PDOMapping=0 +ObjFlags=0x3 + +[200B] +ParameterName=error_details +ObjectType=0x9 +SubNumber=4 +DataType=0x0008 +AccessType=ro + +[200Bsub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 + +[200Bsub1] +ParameterName=detail +ObjectType=0x7 +DataType=0x0008 +AccessType=ro +PDOMapping=0 + +[200Bsub2] +ParameterName=file +ObjectType=0x7 +DataType=0x0009 +AccessType=ro +PDOMapping=0 +ObjFlags=2 + +[200Bsub3] +ParameterName=line +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 + +[200C] +ParameterName=communication_mode +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[200D] +ParameterName=pcb_temperature +ObjectType=0x9 +SubNumber=3 + +[200Dsub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[200Dsub1] +ParameterName=actual_pcb_temperature +ObjectType=0x7 +DataType=0x0008 +AccessType=ro +PDOMapping=1 + +[200Dsub2] +ParameterName=pcb_temperature_update_period_ms +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=20000 +PDOMapping=0 +LowLimit=1000 +HighLimit=3600000 + +[200E] +ParameterName=sync_timeout_factor +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=6 +PDOMapping=0 + +[2010] +ParameterName=KR_Current +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2011] +ParameterName=TN_Current +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2012] +ParameterName=TD_Current +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2013] +ParameterName=KC_Current +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2020] +ParameterName=KR_Speed +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2021] +ParameterName=TN_Speed +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2022] +ParameterName=TD_Speed +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2023] +ParameterName=KC_Speed +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2024] +ParameterName=KS_FeedForward +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2030] +ParameterName=KR_Pos +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2031] +ParameterName=TN_Pos +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2032] +ParameterName=TD_Pos +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2033] +ParameterName=KC_Pos +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2034] +ParameterName=KP_FeedForward +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2041] +ParameterName=Delta_Position +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2050] +ParameterName=extended_status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0 +PDOMapping=1 +ObjFlags=0x1 + +[20A0] +ParameterName=vendor_parameters_and_status +ObjectType=0x9 +SubNumber=8 + +[20A0sub7] +ParameterName=run_permitted_status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0 +PDOMapping=1 +ObjFlags=0x1 + +[2060] +ParameterName=brake_test +ObjectType=0x9 +SubNumber=4 + +[2060sub1] +ParameterName=brake_test_duration +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +DefaultValue=2500 +PDOMapping=0 +ObjFlags=1 +ParameterValue=2500 + + +[2060sub2] +ParameterName=start_brake_test +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ParameterValue=0 + +[2060sub3] +ParameterName=brake_test_status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ParameterValue=0 diff --git a/canopen_tests/config/robot_control/ros2_controllers.yaml b/canopen_tests/config/robot_control/ros2_controllers.yaml new file mode 100644 index 00000000..f489d310 --- /dev/null +++ b/canopen_tests/config/robot_control/ros2_controllers.yaml @@ -0,0 +1,54 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + robot_controller: + type: canopen_ros2_controllers/Cia402RobotController + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + +robot_controller: + ros__parameters: + joints: + - joint1 + - joint2 + operation_mode: 1 + command_poll_freq: 5 + +forward_position_controller: + ros__parameters: + joints: + - joint1 + - joint2 + interface_name: position + +# joint_trajectory_controller: +# ros__parameters: +# joints: +# - prbt_joint_1 +# - prbt_joint_2 +# - prbt_joint_3 +# - prbt_joint_4 +# - prbt_joint_5 +# - prbt_joint_6 +# command_interfaces: +# - position +# state_interfaces: +# - position +# - velocity +# state_publish_rate: 100.0 +# action_monitor_rate: 20.0 +# allow_partial_joints_goal: false +# constraints: +# stopped_velocity_tolerance: 0.2 +# goal_time: 0.6 +# stopped_velocity_tolerance: 0.05 +# prbt_joint_1: {trajectory: 0.157, goal: 0.01} +# prbt_joint_2: {trajectory: 0.157, goal: 0.01} +# prbt_joint_3: {trajectory: 0.157, goal: 0.01} +# prbt_joint_4: {trajectory: 0.157, goal: 0.01} +# prbt_joint_5: {trajectory: 0.157, goal: 0.01} +# prbt_joint_6: {trajectory: 0.157, goal: 0.01} diff --git a/canopen_tests/config/simple/bus.yml b/canopen_tests/config/simple/bus.yml index e9a31be7..4b9d35cb 100644 --- a/canopen_tests/config/simple/bus.yml +++ b/canopen_tests/config/simple/bus.yml @@ -1,20 +1,19 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + master: node_id: 1 - driver: "ros2_canopen::LifecycleMasterNode" - package: "canopen_core" + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" proxy_device_1: node_id: 2 dcf: "simple.eds" - dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" - driver: "ros2_canopen::LifecycleProxyDriver" + driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" - enable_lazy_load: false proxy_device_2: node_id: 3 dcf: "simple.eds" - dcf_path: "install/canopen_tests/share/canopen_tests/config/simple" - driver: "ros2_canopen::LifecycleProxyDriver" + driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" - enable_lazy_load: true \ No newline at end of file diff --git a/canopen_tests/config/simple/simple.eds b/canopen_tests/config/simple/simple.eds index 80c20bac..68d9bb2a 100644 --- a/canopen_tests/config/simple/simple.eds +++ b/canopen_tests/config/simple/simple.eds @@ -63,9 +63,11 @@ SupportedObjects=11 11=0x1A00 [ManufacturerObjects] -SupportedObjects=2 +SupportedObjects=4 1=0x4000 2=0x4001 +3=0x4002 +4=0x4003 [1000] ParameterName=Device type @@ -267,4 +269,14 @@ PDOMapping=1 ParameterName=UNSIGNED32 sent from slave DataType=0x0007 AccessType=rwr -PDOMapping=1 \ No newline at end of file +PDOMapping=1 + +[4002] +ParameterName=INTEGER32 test +DataType=0x0004 +AccessType=rw + +[4003] +ParameterName=INTEGER16 test +DataType=0x0003 +AccessType=rw diff --git a/canopen_tests/config/simple_lifecycle/bus.yml b/canopen_tests/config/simple_lifecycle/bus.yml new file mode 100644 index 00000000..48b52868 --- /dev/null +++ b/canopen_tests/config/simple_lifecycle/bus.yml @@ -0,0 +1,19 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + +master: + node_id: 1 + driver: "ros2_canopen::LifecycleMasterDriver" + package: "canopen_master_driver" + +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + driver: "ros2_canopen::LifecycleProxyDriver" + package: "canopen_proxy_driver" + +proxy_device_2: + node_id: 3 + dcf: "simple.eds" + driver: "ros2_canopen::LifecycleProxyDriver" + package: "canopen_proxy_driver" diff --git a/canopen_tests/config/simple_lifecycle/simple.eds b/canopen_tests/config/simple_lifecycle/simple.eds new file mode 100644 index 00000000..77126767 --- /dev/null +++ b/canopen_tests/config/simple_lifecycle/simple.eds @@ -0,0 +1,270 @@ +[DeviceInfo] +VendorName=Lely Industries N.V. +VendorNumber=0x00000360 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=1 +NrOfTxPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0011=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=11 +1=0x1003 +2=0x1005 +3=0x1014 +4=0x1015 +5=0x1016 +6=0x1017 +7=0x1029 +8=0x1400 +9=0x1600 +10=0x1800 +11=0x1A00 + +[ManufacturerObjects] +SupportedObjects=2 +1=0x4000 +2=0x4001 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw + +[1018] +SubNumber=5 +ParameterName=Identity object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000360 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=1 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x200 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x40000020 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x40010020 + +[4000] +ParameterName=UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[4001] +ParameterName=UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 diff --git a/canopen_tests/launch/cia402_lifecycle_setup.launch.py b/canopen_tests/launch/cia402_lifecycle_setup.launch.py index a532a574..164b7a96 100644 --- a/canopen_tests/launch/cia402_lifecycle_setup.launch.py +++ b/canopen_tests/launch/cia402_lifecycle_setup.launch.py @@ -8,30 +8,28 @@ def generate_launch_description(): slave_eds_path = os.path.join( - get_package_share_directory("canopen_tests"), "config", "cia402", "cia402_slave.eds" - ) + get_package_share_directory("canopen_tests"), "config", "cia402", "cia402_slave.eds" + ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ - os.path.join( - get_package_share_directory("canopen_mock_slave"), "launch" - ), + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ - "node_id": "2", + "node_id": "2", "node_name": "cia402_node_1", "slave_config": slave_eds_path, - }.items(), + }.items(), ) master_bin_path = os.path.join( - get_package_share_directory("canopen_tests"), - "config", - "cia402_lifecycle", - "master.bin", - ) + get_package_share_directory("canopen_tests"), + "config", + "cia402_lifecycle", + "master.bin", + ) if not os.path.exists(master_bin_path): master_bin_path = "" @@ -39,7 +37,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), - "/canopen_lifecycle.launch.py", + "/canopen.launch.py", ] ), launch_arguments={ @@ -60,6 +58,4 @@ def generate_launch_description(): }.items(), ) - return LaunchDescription( - [slave_node_1, device_container] - ) + return LaunchDescription([slave_node_1, device_container]) diff --git a/canopen_tests/launch/cia402_setup.launch.py b/canopen_tests/launch/cia402_setup.launch.py index d12d3ca9..b98222af 100644 --- a/canopen_tests/launch/cia402_setup.launch.py +++ b/canopen_tests/launch/cia402_setup.launch.py @@ -8,32 +8,30 @@ def generate_launch_description(): slave_eds_path = os.path.join( - get_package_share_directory("canopen_tests"), "config", "cia402", "cia402_slave.eds" - ) + get_package_share_directory("canopen_tests"), "config", "cia402", "cia402_slave.eds" + ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ - os.path.join( - get_package_share_directory("canopen_mock_slave"), "launch" - ), + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/cia402_slave.launch.py", ] ), launch_arguments={ - "node_id": "2", + "node_id": "2", "node_name": "cia402_node_1", "slave_config": slave_eds_path, - }.items(), + }.items(), ) master_bin_path = os.path.join( - get_package_share_directory("canopen_tests"), - "config", - "cia402_lifecycle", - "master.bin", - ) + get_package_share_directory("canopen_tests"), + "config", + "cia402_lifecycle", + "master.bin", + ) if not os.path.exists(master_bin_path): - master_bin_path = "" + master_bin_path = "" device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -59,6 +57,4 @@ def generate_launch_description(): }.items(), ) - return LaunchDescription( - [slave_node_1, device_container] - ) + return LaunchDescription([slave_node_1, device_container]) diff --git a/canopen_tests/launch/proxy_lifecycle_setup.launch.py b/canopen_tests/launch/proxy_lifecycle_setup.launch.py index b71303dc..03e01cac 100644 --- a/canopen_tests/launch/proxy_lifecycle_setup.launch.py +++ b/canopen_tests/launch/proxy_lifecycle_setup.launch.py @@ -8,72 +8,68 @@ def generate_launch_description(): slave_eds_path = os.path.join( - get_package_share_directory("canopen_tests"), "config", "simple", "simple.eds" - ) + get_package_share_directory("canopen_tests"), "config", "simple_lifecycle", "simple.eds" + ) slave_node_1 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ - os.path.join( - get_package_share_directory("canopen_mock_slave"), "launch" - ), + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ - "node_id": "2", + "node_id": "2", "node_name": "slave_node_1", "slave_config": slave_eds_path, - }.items(), + }.items(), ) slave_node_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ - os.path.join( - get_package_share_directory("canopen_mock_slave"), "launch" - ), + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), "/basic_slave.launch.py", ] ), launch_arguments={ - "node_id": "3", + "node_id": "3", "node_name": "slave_node_2", "slave_config": slave_eds_path, - }.items(), + }.items(), + ) + + print( + os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "proxy_write_sdo", + "master.dcf", + ) ) - print(os.path.join( - get_package_share_directory("canopen_tests"), - "config", - "proxy_write_sdo", - "master.dcf", - )) - device_container = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_core"), "launch"), - "/canopen_lifecycle.launch.py", + "/canopen.launch.py", ] ), launch_arguments={ "master_config": os.path.join( get_package_share_directory("canopen_tests"), "config", - "simple", + "simple_lifecycle", "master.dcf", ), "master_bin": "", "bus_config": os.path.join( get_package_share_directory("canopen_tests"), "config", - "simple", + "simple_lifecycle", "bus.yml", ), "can_interface_name": "vcan0", }.items(), ) - return LaunchDescription( - [slave_node_1, slave_node_2, device_container] - ) + return LaunchDescription([slave_node_1, slave_node_2, device_container]) diff --git a/canopen_tests/launch/proxy_setup.launch.py b/canopen_tests/launch/proxy_setup.launch.py new file mode 100644 index 00000000..6afe382c --- /dev/null +++ b/canopen_tests/launch/proxy_setup.launch.py @@ -0,0 +1,75 @@ +import os +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + slave_eds_path = os.path.join( + get_package_share_directory("canopen_tests"), "config", "simple", "simple.eds" + ) + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/basic_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "2", + "node_name": "slave_node_1", + "slave_config": slave_eds_path, + }.items(), + ) + + slave_node_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/basic_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "3", + "node_name": "slave_node_2", + "slave_config": slave_eds_path, + }.items(), + ) + + print( + os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "proxy_write_sdo", + "master.dcf", + ) + ) + + device_container = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_core"), "launch"), + "/canopen.launch.py", + ] + ), + launch_arguments={ + "master_config": os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "simple", + "master.dcf", + ), + "master_bin": "", + "bus_config": os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "simple", + "bus.yml", + ), + "can_interface_name": "vcan0", + }.items(), + ) + + return LaunchDescription([slave_node_1, slave_node_2, device_container]) diff --git a/canopen_tests/launch/robot_control_setup.launch.py b/canopen_tests/launch/robot_control_setup.launch.py new file mode 100644 index 00000000..9e197f1f --- /dev/null +++ b/canopen_tests/launch/robot_control_setup.launch.py @@ -0,0 +1,111 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("canopen_tests"), + "urdf/robot_controller", + "robot_controller.urdf.xacro", + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + robot_control_config = PathJoinSubstitution( + [FindPackageShare("canopen_tests"), "config/robot_control", "ros2_controllers.yaml"] + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("canopen_tests"), "rviz", "robot_controller.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_control_config], + output="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["robot_controller", "--controller-manager", "/controller_manager"], + ) + + forward_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + slave_config = PathJoinSubstitution( + [FindPackageShare("canopen_tests"), "config/robot_control", "cia402_slave.eds"] + ) + + slave_launch = PathJoinSubstitution( + [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"] + ) + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slave_launch), + launch_arguments={ + "node_id": "2", + "node_name": "slave_node_1", + "slave_config": slave_config, + }.items(), + ) + + slave_node_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slave_launch), + launch_arguments={ + "node_id": "3", + "node_name": "slave_node_2", + "slave_config": slave_config, + }.items(), + ) + + nodes_to_start = [ + # slave_node_2, + # slave_node_1, + control_node, + joint_state_broadcaster_spawner, + # robot_controller_spawner, + forward_position_controller_spawner, + robot_state_publisher_node, + slave_node_1, + slave_node_2, + # rviz_node + ] + + return LaunchDescription(nodes_to_start) diff --git a/canopen_tests/launch/view_urdf.launch.py b/canopen_tests/launch/view_urdf.launch.py new file mode 100644 index 00000000..de2ceea6 --- /dev/null +++ b/canopen_tests/launch/view_urdf.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("canopen_tests"), + "urdf/robot_controller", + "robot_controller.urdf.xacro", + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("canopen_tests"), "rviz", "robot_controller.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + nodes_to_start = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(nodes_to_start) diff --git a/canopen_tests/launch_tests/test_proxy_driver.py b/canopen_tests/launch_tests/test_proxy_driver.py index e2518227..12ab5eba 100644 --- a/canopen_tests/launch_tests/test_proxy_driver.py +++ b/canopen_tests/launch_tests/test_proxy_driver.py @@ -1,7 +1,6 @@ import os from time import sleep import pytest -from sympy import true from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch @@ -26,7 +25,7 @@ def generate_test_description(): PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("canopen_tests"), "launch"), - "/proxy_lifecycle_setup.launch.py", + "/proxy_setup.launch.py", ] ) ) @@ -43,7 +42,6 @@ class TestLifecycle(unittest.TestCase): @classmethod def setUpClass(cls): print("SetupClass") - @classmethod def tearDownClass(cls): @@ -63,25 +61,28 @@ def tearDown(self): self.node.destroy_node() self.x.join() - - def test_01_to_active(self): - assert self.node.checkTransition("lifecycle_device_manager_node", State.PRIMARY_STATE_UNCONFIGURED, Transition.TRANSITION_CONFIGURE), "Could not configure device manager" - assert self.node.checkTransition("lifecycle_device_manager_node", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_ACTIVATE), "Could not configure device manager" - def test_02_nmt(self): assert self.node.checkTrigger("proxy_device_1", "nmt_reset_node") assert self.node.checkTrigger("proxy_device_2", "nmt_reset_node") sleep(1.0) - + def test_03_sdo_read(self): assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, type=32, data=0) assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, type=32, data=0) def test_04_sdo_write(self): - assert self.node.checkSDOWrite("proxy_device_1", index=0x4000, subindex=0, type=32, data=100) - assert self.node.checkSDOWrite("proxy_device_2", index=0x4000, subindex=0, type=32, data=100) - assert self.node.checkSDORead("proxy_device_1", index=0x4000, subindex=0, type=32, data=100) - assert self.node.checkSDORead("proxy_device_2", index=0x4000, subindex=0, type=32, data=100) + assert self.node.checkSDOWrite( + "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 + ) + assert self.node.checkSDOWrite( + "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 + ) + assert self.node.checkSDORead( + "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 + ) + assert self.node.checkSDORead( + "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 + ) def test_05_sdo_read_id(self): assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=100) @@ -89,11 +90,14 @@ def test_05_sdo_read_id(self): def test_06_sdo_write_id(self): assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=32, data=999) - assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=32, data=999) + assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=32, data=999) assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=999) assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=32, data=999) - def test_07_to_unconfigured(self): - assert self.node.checkTransition("lifecycle_device_manager_node", State.PRIMARY_STATE_ACTIVE, Transition.TRANSITION_DEACTIVATE), "Could not configure device manager" - assert self.node.checkTransition("lifecycle_device_manager_node", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_CLEANUP), "Could not configure device manager" - \ No newline at end of file + def test_07_rpdo_tpdo(self): + assert self.node.checkRpdoTpdo( + "proxy_device_1", index=0x4000, subindex=0, type=32, data=101 + ) + assert self.node.checkRpdoTpdo( + "proxy_device_2", index=0x4000, subindex=0, type=32, data=202 + ) diff --git a/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py new file mode 100644 index 00000000..09eb42ef --- /dev/null +++ b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py @@ -0,0 +1,119 @@ +import os +from time import sleep +import pytest +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_testing +import threading +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from canopen_utils.test_node import TestNode +from lifecycle_msgs.msg import State, Transition +from std_srvs.srv import Trigger +import unittest + + +@pytest.mark.rostest +def generate_test_description(): + + launch_desc = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_tests"), "launch"), + "/proxy_lifecycle_setup.launch.py", + ] + ) + ) + + ready_to_test = launch.actions.TimerAction( + period=5.0, + actions=[launch_testing.actions.ReadyToTest()], + ) + + return (LaunchDescription([launch_desc, ready_to_test]), {}) + + +class TestLifecycle(unittest.TestCase): + @classmethod + def setUpClass(cls): + print("SetupClass") + + @classmethod + def tearDownClass(cls): + print("TearDownClass") + + @classmethod + def setUp(self): + print("Setup") + rclpy.init() + self.node = TestNode() + self.x = threading.Thread(target=rclpy.spin, args=[self.node]) + self.x.start() + + def tearDown(self): + print("TearDown") + rclpy.shutdown() + self.node.destroy_node() + self.x.join() + + def test_01_to_active(self): + assert self.node.checkTransition( + "lifecycle_manager", State.PRIMARY_STATE_UNCONFIGURED, Transition.TRANSITION_CONFIGURE + ), "Could not configure device manager" + assert self.node.checkTransition( + "lifecycle_manager", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_ACTIVATE + ), "Could not configure device manager" + + def test_02_nmt(self): + assert self.node.checkTrigger("proxy_device_1", "nmt_reset_node") + assert self.node.checkTrigger("proxy_device_2", "nmt_reset_node") + sleep(1.0) + + def test_03_sdo_read(self): + assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, type=32, data=0) + assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, type=32, data=0) + + def test_04_sdo_write(self): + assert self.node.checkSDOWrite( + "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 + ) + assert self.node.checkSDOWrite( + "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 + ) + assert self.node.checkSDORead( + "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 + ) + assert self.node.checkSDORead( + "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 + ) + + def test_05_sdo_read_id(self): + assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=100) + assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=32, data=100) + + def test_06_sdo_write_id(self): + assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=32, data=999) + assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=32, data=999) + assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=999) + assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=32, data=999) + + def test_07_rpdo_tpdo(self): + assert self.node.checkRpdoTpdo( + "proxy_device_1", index=0x4000, subindex=0, type=32, data=101 + ) + assert self.node.checkRpdoTpdo( + "proxy_device_2", index=0x4000, subindex=0, type=32, data=202 + ) + + def test_08_to_unconfigured(self): + assert self.node.checkTransition( + "lifecycle_manager", State.PRIMARY_STATE_ACTIVE, Transition.TRANSITION_DEACTIVATE + ), "Could not configure device manager" + assert self.node.checkTransition( + "lifecycle_manager", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_CLEANUP + ), "Could not configure device manager" diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 2f7902ea..f19b8247 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -8,12 +8,12 @@ Apache-2.0 ament_cmake + lely_core_libraries - canopen_core - canopen_interfaces - canopen_base_driver - canopen_proxy_driver - canopen_402_driver + + canopen_402_driver + canopen_core + canopen_proxy_driver ament_lint_auto diff --git a/canopen_tests/rviz/robot_controller.rviz b/canopen_tests/rviz/robot_controller.rviz new file mode 100644 index 00000000..730acd5c --- /dev/null +++ b/canopen_tests/rviz/robot_controller.rviz @@ -0,0 +1,203 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 752 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: /home/ws/test.urdf + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + link1: + Value: true + link2: + Value: true + link3: + Value: true + link4: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + link1: + link2: + link3: + link4: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: link1 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d00650100000000000007360000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004c50000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1846 + X: 1994 + Y: 0 diff --git a/canopen_tests/urdf/robot_controller/robot_controller.macro.xacro b/canopen_tests/urdf/robot_controller/robot_controller.macro.xacro new file mode 100644 index 00000000..86c0a4f2 --- /dev/null +++ b/canopen_tests/urdf/robot_controller/robot_controller.macro.xacro @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/canopen_tests/urdf/robot_controller/robot_controller.ros2_control.xacro b/canopen_tests/urdf/robot_controller/robot_controller.ros2_control.xacro new file mode 100644 index 00000000..22793d76 --- /dev/null +++ b/canopen_tests/urdf/robot_controller/robot_controller.ros2_control.xacro @@ -0,0 +1,29 @@ + + + + + + + canopen_ros2_control/RobotSystem + ${bus_config} + ${master_config} + ${can_interface_name} + "${master_bin}" + + + joint_1 + + + + joint_2 + + + + + diff --git a/canopen_tests/urdf/robot_controller/robot_controller.urdf.xacro b/canopen_tests/urdf/robot_controller/robot_controller.urdf.xacro new file mode 100644 index 00000000..36378761 --- /dev/null +++ b/canopen_tests/urdf/robot_controller/robot_controller.urdf.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + diff --git a/canopen_utils/canopen_utils/cyclic_tester.py b/canopen_utils/canopen_utils/cyclic_tester.py index 525fe14c..2f7afc2b 100644 --- a/canopen_utils/canopen_utils/cyclic_tester.py +++ b/canopen_utils/canopen_utils/cyclic_tester.py @@ -9,12 +9,12 @@ class DoubleTalker(Node): """Publish messages to a topic using two publishers at different rates.""" def __init__(self): - super().__init__('double_talker') + super().__init__("double_talker") self.i = 0 self.cli = self.create_client(COTargetDouble, "trinamic_pd42/target") while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + self.get_logger().info("service not available, waiting again...") self.increment = 1000.0 self.period = 0.1 self.value = 0.0 @@ -22,7 +22,6 @@ def __init__(self): self.timer = self.create_timer(self.period, self.timer_callback, callback_group=self.group) self.req = COTargetDouble.Request() - def timer_callback(self): self.value = self.value + self.increment self.req.target = self.value @@ -55,6 +54,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() - diff --git a/canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py b/canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py new file mode 100644 index 00000000..00f1a906 --- /dev/null +++ b/canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py @@ -0,0 +1,64 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from lifecycle_msgs.srv import GetState, ChangeState +from lifecycle_msgs.msg import State, Transition +from std_srvs.srv import Trigger +from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID +from canopen_interfaces.msg import COData + + +class SimpleTestNode(Node): + def __init__(self, name="test_node"): + super().__init__(name) + + def checkRpdoTpdo(self, node_name, index: int, subindex: int, type: int, data: int) -> bool: + publisher = self.create_publisher(COData, "/" + node_name + "/tpdo", 10) + subscriber = self.create_subscription( + COData, "/" + node_name + "/rpdo", self.rpdo_callback, 10 + ) + target = COData() + target.index = index + target.subindex = subindex + target.type = type + target.data = data + publisher.publish(target) + print("Published tpdo") + # Wait for topic to be published + self.rpdo_data = None + while not (self.rpdo_data): + print("Waiting for subscriber to connect. ") + rclpy.spin_once(self, timeout_sec=0.1) + + if self.rpdo_data.data == data: + result = True + else: + result = False + + self.destroy_publisher(publisher) + self.destroy_subscription(subscriber) + + return result + + def rpdo_callback(self, msg): + print("RPDO Callback") + print(msg) + self.rpdo_data = msg + + +def main(args=None): + rclpy.init(args=args) + node = SimpleTestNode() + + result = node.checkRpdoTpdo("proxy_device_1", index=0x4000, subindex=0, type=32, data=999) + print("RESULT: " + str(result)) + + result = node.checkRpdoTpdo("proxy_device_2", index=0x4000, subindex=0, type=32, data=999) + print("RESULT: " + str(result)) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/canopen_utils/canopen_utils/test_node.py b/canopen_utils/canopen_utils/test_node.py index 86690b5d..2b686712 100644 --- a/canopen_utils/canopen_utils/test_node.py +++ b/canopen_utils/canopen_utils/test_node.py @@ -1,22 +1,19 @@ +import rclpy from rclpy.node import Node from lifecycle_msgs.srv import GetState, ChangeState from lifecycle_msgs.msg import State, Transition from std_srvs.srv import Trigger from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID +from canopen_interfaces.msg import COData -class TestNode(Node): +class TestNode(Node): def __init__(self, name="test_node"): super().__init__(name) - def checkTransition(self, node_name: str, state: int, tranisition: int) -> bool: - get_state_client = self.create_client( - GetState, node_name + "/get_state" - ) - change_state_client = self.create_client( - ChangeState, node_name + "/change_state" - ) + get_state_client = self.create_client(GetState, node_name + "/get_state") + change_state_client = self.create_client(ChangeState, node_name + "/change_state") if not get_state_client.wait_for_service(timeout_sec=3.0): get_state_client.destroy() change_state_client.destroy() @@ -81,7 +78,9 @@ def checkSDOWrite(self, node_name, index: int, subindex: int, type: int, data: i return True return False - def checkSDOReadID(self, node_id: int, index: int, subindex: int, type: int, data: int) -> bool: + def checkSDOReadID( + self, node_id: int, index: int, subindex: int, type: int, data: int + ) -> bool: client = self.create_client(COReadID, "/master/sdo_read") if not client.wait_for_service(timeout_sec=3.0): return False @@ -96,7 +95,9 @@ def checkSDOReadID(self, node_id: int, index: int, subindex: int, type: int, dat return True return False - def checkSDOWriteID(self, node_id: int, index: int, subindex: int, type: int, data: int) -> bool: + def checkSDOWriteID( + self, node_id: int, index: int, subindex: int, type: int, data: int + ) -> bool: client = self.create_client(COWriteID, "/master/sdo_write") if not client.wait_for_service(timeout_sec=3.0): return False @@ -111,4 +112,31 @@ def checkSDOWriteID(self, node_id: int, index: int, subindex: int, type: int, da if result.success: return True return False - \ No newline at end of file + + def checkRpdoTpdo(self, node_name, index: int, subindex: int, type: int, data: int) -> bool: + publisher = self.create_publisher(COData, "/" + node_name + "/tpdo", 10) + subscriber = self.create_subscription( + COData, "/" + node_name + "/rpdo", self.rpdo_callback, 10 + ) + target = COData() + target.index = index + target.subindex = subindex + target.type = type + target.data = data + publisher.publish(target) + print("Published tpdo to topic: " + "/" + node_name + "/tpdo") + self.rpdo_data = None + # Wait for topic to be published + while not self.rpdo_data: + rclpy.spin_once(self, timeout_sec=0.5) + + self.destroy_publisher(publisher) + self.destroy_subscription(subscriber) + + if self.rpdo_data.data == data: + return True + return False + + def rpdo_callback(self, msg): + print(msg) + self.rpdo_data = msg diff --git a/canopen_utils/no_tests/_test_copyright.py b/canopen_utils/no_tests/_test_copyright.py index cc8ff03f..f46f861d 100644 --- a/canopen_utils/no_tests/_test_copyright.py +++ b/canopen_utils/no_tests/_test_copyright.py @@ -19,5 +19,5 @@ @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/canopen_utils/no_tests/_test_flake8.py b/canopen_utils/no_tests/_test_flake8.py index 27ee1078..49c1644f 100644 --- a/canopen_utils/no_tests/_test_flake8.py +++ b/canopen_utils/no_tests/_test_flake8.py @@ -20,6 +20,4 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/canopen_utils/no_tests/_test_pep257.py b/canopen_utils/no_tests/_test_pep257.py index b234a384..a2c3deb8 100644 --- a/canopen_utils/no_tests/_test_pep257.py +++ b/canopen_utils/no_tests/_test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index 4c2bb874..faff5556 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -1,26 +1,26 @@ from setuptools import setup -package_name = 'canopen_utils' +package_name = "canopen_utils" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='christoph', - maintainer_email='christoph.hellmann.santos@ipa.fraunhofer.de', - description='TODO: Package description', - license='Apache-2.0', - tests_require=['pytest'], + maintainer="christoph", + maintainer_email="christoph.hellmann.santos@ipa.fraunhofer.de", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'cyclic_tester = canopen_utils.cyclic_tester:main' + "console_scripts": [ + "cyclic_tester = canopen_utils.cyclic_tester:main", + "simple_tester = canopen_utils.simple_rpdo_tpdo_tester:main", ], }, ) diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index 5df35bc2..a67d176b 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -1,16 +1,17 @@ cmake_minimum_required(VERSION 3.8) project(lely_core_libraries) find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) include(ExternalProject) ExternalProject_Add(upstr_lely_core_libraries # Name for custom target #--Download step-------------- GIT_REPOSITORY https://gitlab.com/lely_industries/lely-core.git - GIT_TAG f13629b4fcc042744ff497c8bf7428cf2775aba8 - TIMEOUT 60 + GIT_TAG ac31cd5cdf2ded4b2a5ba4a94520f388450614ff + TIMEOUT 60 #--Update/Patch step---------- - UPDATE_COMMAND touch /config.h - COMMAND mkdir -p /include /lib + UPDATE_COMMAND touch /config.h + COMMAND mkdir -p /include /lib COMMAND touch /python/dcf-tools/README.md #--Configure step------------- CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static "CPPFLAGS=-DNDEBUG" "CFLAGS=-DNDEBUG" "CXXFLAGS=-DNDEBUG" @@ -37,6 +38,14 @@ install( DESTINATION share/${PROJECT_NAME} ) +ament_python_install_package(cogen SCRIPTS_DESTINATION lib/cogen) + +# install entry-point script(s) in bin as well +install( + DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_python/cogen/scripts/ + DESTINATION bin/ + USE_SOURCE_PERMISSIONS) + ament_export_include_directories(include) ament_export_libraries(lely-can lely-co lely-coapp lely-ev lely-io2 lely-io lely-libc lely-tap lely-util) ament_package( diff --git a/lely_core_libraries/cmake/lely_core_libraries-extras.cmake b/lely_core_libraries/cmake/lely_core_libraries-extras.cmake index 7e1496b6..6aab6f02 100644 --- a/lely_core_libraries/cmake/lely_core_libraries-extras.cmake +++ b/lely_core_libraries/cmake/lely_core_libraries-extras.cmake @@ -3,10 +3,10 @@ # in your package. Inside that folder there needs to be the bus.yml # file to use for generation. macro( - generate_dcf + generate_dcf TARGET) add_custom_target( - ${TARGET}_prepare ALL + ${TARGET}_prepare ALL COMMAND rm -rf ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/* COMMAND rm -rf ${CMAKE_BINARY_DIR}/config/${TARGET}/* COMMAND mkdir -p ${CMAKE_BINARY_DIR}/config/${TARGET} @@ -14,21 +14,62 @@ macro( ) add_custom_target( - ${TARGET} ALL + ${TARGET} ALL DEPENDS ${TARGET}_prepare ) add_custom_command( TARGET ${TARGET} POST_BUILD - COMMAND dcfgen -d ${CMAKE_BINARY_DIR}/config/${TARGET}/ -rS bus.yml + COMMAND sed 's|@BUS_CONFIG_PATH@|${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}|g' + ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/bus.yml > ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml + COMMAND dcfgen -v -d ${CMAKE_BINARY_DIR}/config/${TARGET}/ -rS ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ ) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ + PATTERN "bus.yml" EXCLUDE ) - + + install(DIRECTORY + ${CMAKE_BINARY_DIR}/config/${TARGET}/ + DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ + ) + +endmacro() + +macro( + cogen_dcf + TARGET) + add_custom_target( + ${TARGET}_prepare ALL + COMMAND rm -rf ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/* + COMMAND rm -rf ${CMAKE_BINARY_DIR}/config/${TARGET}/* + COMMAND mkdir -p ${CMAKE_BINARY_DIR}/config/${TARGET} + COMMAND mkdir -p ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/ + ) + + add_custom_target( + ${TARGET} ALL + DEPENDS ${TARGET}_prepare + ) + + add_custom_command( + TARGET ${TARGET} POST_BUILD + COMMAND cogen --input-file ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/bus.yml --output-file ${CMAKE_BINARY_DIR}/config/${TARGET}/preprocessed_bus.yml + COMMAND sed 's|@BUS_CONFIG_PATH@|${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}|g' + ${CMAKE_BINARY_DIR}/config/${TARGET}/preprocessed_bus.yml > ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml + COMMAND dcfgen -v -d ${CMAKE_BINARY_DIR}/config/${TARGET}/ -rS ${CMAKE_BINARY_DIR}/config/${TARGET}/bus.yml + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ + ) + + install(DIRECTORY + ${CMAKE_CURRENT_SOURCE_DIR}/config/${TARGET}/ + DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ + PATTERN "bus.yml" EXCLUDE + ) + install(DIRECTORY ${CMAKE_BINARY_DIR}/config/${TARGET}/ DESTINATION share/${PROJECT_NAME}/config/${TARGET}/ @@ -44,4 +85,4 @@ macro(dcfgen INPUT_DIR FILE OUTPUT_DIR) COMMAND "dcfgen" "-d" ${OUTPUT_DIR} "-rS" ${INPUT_DIR}${FILE} WORKING_DIRECTORY ${INPUT_DIR} ) -endmacro() \ No newline at end of file +endmacro() diff --git a/lely_core_libraries/cogen/__init__.py b/lely_core_libraries/cogen/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lely_core_libraries/cogen/cogen.py b/lely_core_libraries/cogen/cogen.py new file mode 100644 index 00000000..0f170954 --- /dev/null +++ b/lely_core_libraries/cogen/cogen.py @@ -0,0 +1,61 @@ +import yaml +import argparse +from dataclasses import dataclass +from dcfgen.cli import Master, Slave + + +def main(): + parser = argparse.ArgumentParser( + description="Expands the CANopen bus.yml for further processing." + ) + parser.add_argument("--input-file", type=str, help="The name of the input file", required=True) + parser.add_argument( + "--output-file", type=str, help="The name of the output file", required=True + ) + args = parser.parse_args() + + with open(args.input_file) as input: + cfg = yaml.load(input, yaml.FullLoader) + + master = {} + if "master" not in cfg: + print("Found no master.") + return + else: + master = cfg["master"] + + nodes = {} + if "nodes" not in cfg: + print("Found no nodes entry.") + return + else: + nodes = cfg["nodes"] + + options = {} + if "options" in cfg: + options = cfg["options"] + + defaults = {} + if "defaults" in cfg: + defaults = cfg["defaults"] + + # add defaults to each node + for node_name in nodes: + for entry_name in defaults: + nodes[node_name][entry_name] = defaults[entry_name] + + modified_file = {} + modified_file["options"] = options + # modified_file["defaults"] = defaults + modified_file["master"] = master + for node_name in nodes: + modified_file[node_name] = nodes[node_name] + + with open(args.output_file, mode="wt") as output: + yaml.dump(modified_file, output) + + return + + +if __name__ == "__main__": + main() diff --git a/lely_core_libraries/setup.cfg b/lely_core_libraries/setup.cfg new file mode 100644 index 00000000..de771051 --- /dev/null +++ b/lely_core_libraries/setup.cfg @@ -0,0 +1,3 @@ +[options.entry_points] +console_scripts = + cogen = cogen.cogen:main From cd84b646b383d7f288353b16de2dfcb269d25dda Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Tue, 6 Jun 2023 22:12:15 +0200 Subject: [PATCH 02/59] Beta release preparations (#120) * Improve lely compilation time Signed-off-by: Christoph Hellmann Santos * Bump lely_core_librries to version 2.3.2 Signed-off-by: Christoph Hellmann Santos * Add license files Signed-off-by: Christoph Hellmann Santos * Adapt package xml Signed-off-by: Christoph Hellmann Santos * Add changelogs - forthcoming for now. Signed-off-by: Christoph Hellmann Santos * Update readme Signed-off-by: Christoph Hellmann Santos * Add apacje-2.0 license notifications to files Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos --- README.md | 50 +++- canopen/CHANGELOG.rst | 154 ++++++++++++ canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 208 ++++++++++++++++ canopen_base_driver/CHANGELOG.rst | 163 +++++++++++++ .../node_canopen_base_driver.hpp | 14 ++ .../node_canopen_base_driver_impl.hpp | 14 ++ canopen_base_driver/src/base_driver.cpp | 1 + .../node_canopen_base_driver.cpp | 14 ++ .../test/test_base_driver_component.cpp | 14 ++ .../test_node_canopen_base_driver_ros.cpp | 14 ++ canopen_core/CHANGELOG.rst | 227 ++++++++++++++++++ .../canopen_core/device_container_error.hpp | 14 ++ .../include/canopen_core/driver_error.hpp | 14 ++ .../include/canopen_core/driver_node.hpp | 14 ++ .../include/canopen_core/master_error.hpp | 14 ++ .../include/canopen_core/master_node.hpp | 13 + .../node_interfaces/node_canopen_driver.hpp | 14 ++ .../node_canopen_driver_interface.hpp | 14 ++ .../node_interfaces/node_canopen_master.hpp | 14 ++ .../node_canopen_master_interface.hpp | 14 ++ canopen_core/launch/canopen.launch.py | 14 ++ canopen_core/src/configuration_manager.cpp | 14 ++ canopen_core/src/device_container_error.cpp | 14 ++ canopen_core/src/driver_error.cpp | 14 ++ canopen_core/src/driver_node.cpp | 14 ++ canopen_core/src/lifecycle_manager.cpp | 14 ++ canopen_core/src/master_error.cpp | 14 ++ canopen_core/src/master_node.cpp | 14 ++ .../node_interfaces/node_canopen_driver.cpp | 13 + .../node_interfaces/node_canopen_master.cpp | 14 ++ canopen_core/test/test_canopen_driver.cpp | 14 ++ canopen_core/test/test_canopen_master.cpp | 14 ++ canopen_core/test/test_device_container.cpp | 14 ++ canopen_core/test/test_errors.cpp | 14 ++ .../test/test_lifecycle_canopen_driver.cpp | 14 ++ .../test/test_lifecycle_canopen_master.cpp | 14 ++ canopen_core/test/test_lifecycle_manager.cpp | 14 ++ .../test/test_node_canopen_driver.cpp | 14 ++ .../test/test_node_canopen_master.cpp | 14 ++ canopen_fake_slaves/CHANGELOG.rst | 54 +++++ .../canopen_fake_slaves/base_slave.hpp | 13 + .../canopen_fake_slaves/basic_slave.hpp | 14 ++ .../canopen_fake_slaves/cia402_slave.hpp | 14 ++ .../canopen_fake_slaves/motion_generator.hpp | 20 ++ .../launch/basic_slave.launch.py | 14 ++ .../launch/cia402_slave.launch.py | 14 ++ canopen_fake_slaves/src/basic_slave.cpp | 14 ++ canopen_fake_slaves/src/cia402_slave.cpp | 14 ++ canopen_fake_slaves/src/motion_generator.cpp | 14 ++ canopen_interfaces/CHANGELOG.rst | 56 +++++ canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 35 +++ .../node_canopen_basic_master.hpp | 15 ++ .../node_canopen_basic_master_impl.hpp | 15 ++ .../src/lifecycle_master_driver.cpp | 15 ++ canopen_master_driver/src/master_driver.cpp | 15 ++ .../node_canopen_basic_master.cpp | 15 ++ .../test/test_master_driver_component.cpp | 14 ++ .../test/test_node_canopen_basic_master.cpp | 14 ++ .../test_node_canopen_basic_master_ros.cpp | 14 ++ canopen_proxy_driver/CHANGELOG.rst | 128 ++++++++++ .../node_canopen_proxy_driver.hpp | 14 ++ .../node_canopen_proxy_driver_impl.hpp | 14 ++ .../node_canopen_proxy_driver.cpp | 14 ++ .../test/test_driver_component.cpp | 14 ++ .../test/test_node_interface.cpp | 14 ++ canopen_ros2_control/CHANGELOG.rst | 158 ++++++++++++ canopen_ros2_control/LICENSE | 202 ++++++++++++++++ canopen_ros2_controllers/CHANGELOG.rst | 99 ++++++++ canopen_ros2_controllers/LICENSE | 202 ++++++++++++++++ canopen_tests/CHANGELOG.rst | 169 +++++++++++++ .../launch/cia402_lifecycle_setup.launch.py | 14 ++ canopen_tests/launch/cia402_setup.launch.py | 14 ++ .../launch/proxy_lifecycle_setup.launch.py | 14 ++ canopen_tests/launch/proxy_setup.launch.py | 14 ++ .../launch/robot_control_setup.launch.py | 14 ++ canopen_tests/launch/view_urdf.launch.py | 14 ++ .../launch_tests/test_proxy_driver.py | 14 ++ .../test_proxy_lifecycle_driver.py | 14 ++ canopen_utils/CHANGELOG.rst | 51 ++++ canopen_utils/LICENSE | 202 ++++++++++++++++ canopen_utils/canopen_utils/cyclic_tester.py | 14 ++ .../canopen_utils/simple_rpdo_tpdo_tester.py | 14 ++ canopen_utils/canopen_utils/test_node.py | 14 ++ lely_core_libraries/CHANGELOG.rst | 84 +++++++ lely_core_libraries/CMakeLists.txt | 8 +- lely_core_libraries/cogen/cogen.py | 14 ++ lely_core_libraries/package.xml | 2 +- 89 files changed, 3183 insertions(+), 20 deletions(-) create mode 100644 canopen/CHANGELOG.rst create mode 100644 canopen_402_driver/CHANGELOG.rst create mode 100644 canopen_base_driver/CHANGELOG.rst create mode 100644 canopen_core/CHANGELOG.rst create mode 100644 canopen_fake_slaves/CHANGELOG.rst create mode 100644 canopen_interfaces/CHANGELOG.rst create mode 100644 canopen_master_driver/CHANGELOG.rst create mode 100644 canopen_proxy_driver/CHANGELOG.rst create mode 100644 canopen_ros2_control/CHANGELOG.rst create mode 100644 canopen_ros2_control/LICENSE create mode 100644 canopen_ros2_controllers/CHANGELOG.rst create mode 100644 canopen_ros2_controllers/LICENSE create mode 100644 canopen_tests/CHANGELOG.rst create mode 100644 canopen_utils/CHANGELOG.rst create mode 100644 canopen_utils/LICENSE create mode 100644 lely_core_libraries/CHANGELOG.rst diff --git a/README.md b/README.md index d2c08abf..063dad87 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,11 @@ # ROS2 CANopen +## Status + [![Build Status](https://github.com/ros-industrial/ros2_canopen/workflows/rolling/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) [![Documentation Status](https://github.com/ros-industrial/ros2_canopen/workflows/Documentation/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) +The stack is currently under development and not yet ready for production use. ## Documentation The documentation consists of two parts: a manual and an api reference. @@ -21,18 +24,30 @@ Older ROS 2 releases are EOL and are not supported anymore. * Manual: https://ros-industrial.github.io/ros2_canopen/manual/humble/ * API reference: https://ros-industrial.github.io/ros2_canopen/api/humble/ -## Status -Currently under development. Not for production use. +## Features +These are some of the features this stack implements. For further information please refer to the documentation. -**Available Features:** -* Device Manager (using rclcpp::components) -* MasterDriver (Service Interface) -* ProxyDriver (Service Interface) -* Cia402Driver (Service Interface) -* Generic ros2_control Interface (implementing `hardware_interface::SystemInterface`) - check https://control.ros.org for more details +* **YAML-Bus configuration** + This canopen stack enables you to configure the bus using a YAML file. In this file you define the nodes that are connected to the bus by specifying their node id, the corresponding EDS file and the driver to run for the node. You can also specify further parameters that overwrite EDS parameters or are inputs to the driver. +* **Service based operation** + The stack can be operated using standard ROS2 nodes. In this case the device container will load the drivers for master and slave nodes. Each driver will be visible as a + node and expose a ROS 2 interface. All drivers are brought up when the device manager is launched. +* **Managed service based operation** + The stack can be opeprated using managed ROS2 nodes. In + this case the device container will load the drivers for master and slave nodes based on the bus configuration. Each driver will be a lifecycle node and expose a ROS 2 interface. The lifecycle manager can be used to bring all + device up and down in the correct sequence. +* **ROS2 control based operation** + Currently, multiple ros2_control interfaces are available. These can be used for controlling CANopen devices. The interfaces are: + * canopen_ros2_control/CANopenSystem + * canopen_ros2_control/CIA402System + * canopen_ros2_control/RobotSystem +* **CANopen drivers** + Currently, the following drivers are available: + * ProxyDriver + * Cia402Driver -**Post build testing** +## Post testing To test stack after it was built from source you should first setup a virtual can network. ```bash sudo modprobe vcan @@ -40,10 +55,21 @@ sudo ip link add dev vcan0 type vcan sudo ip link set vcan0 txqueuelen 1000 sudo ip link set up vcan0 ``` -Then you can run the integration tests contained in canopen_tests package. +Then you can launch a managed example +```bash +ros2 launch canopen_tests cia402_lifecycle_setup.launch.py +ros2 lifecycle set /lifecycle_manager configure +ros2 lifecycle set /lifecycle_manager activate +``` + +Or you can launch a standard example +```bash +ros2 launch canopen_tests cia402_setup.launch.py +``` + +Or you can launch a ros2_control example ```bash -launch_test src/ros2_canopen/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py -launch_test src/ros2_canopen/canopen_tests/launch_tests/test_proxy_driver.py +ros2 launch canopen_tests robot_control_setup.launch.py ``` ## Contributing diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst new file mode 100644 index 00000000..77a7ff22 --- /dev/null +++ b/canopen/CHANGELOG.rst @@ -0,0 +1,154 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Adapt package xml +* Reduce processor load (`#111 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Correct repo link (`#94 `_) +* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) + * Add Interpolated Position Mode (linear only, no PT or PVT) + * add interpolated position mode to system interface + * Add interpolated position mode to controllers. + * Add to interpolated position mode to documentation + --------- +* Merge branch 'destogl-patch-2' +* Format updates +* Merge branch 'master' into patch-2 +* Merge branch 'bjsowa-master' +* add short documentation +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Documentation for streamlined design (`#67 `_) + * Add canopen_core tests (90% coverage) + * Restructure and add plantuml + * Changes to quickstart/configuration + * Revert "Add canopen_core tests (90% coverage)" as it is not needed. + This reverts commit 771c498347f190777fb28edfd5044b96cbfd7bf0. + * Create custom driver documentation + * Remove breathe api reference and use doxygen + * Update interface and naming information for drivers + * Update test documentation +* Update deployment +* undo renaming can_interface_name -> can_interface +* Integration with ros2_control +* Restructure documentation (`#37 `_) + * Document device container node + * Document lely_master_bridge + * Document Lifecycle Device Container + * Document Lifecycle Device Manager + * Document LifecyleMasterNode + * Document Master Node + * Fix error + * Document lifecycle base driver + * Document lely bridge + * Document canopen_proxy_driver + * Document canopen_402_driver + * Update sphinx documentation +* make documentation on test with ros2_control more detailed + Make the test documentation a small example with explanations of the functionality. +* Merge branch 'canopen-system-interface' into canopen-controller +* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface + Add generic system interface for ros2_control +* Add documentation about testing ros2_control generic interface. +* Update dcfgen cmake integration (`#41 `_) +* Update configuration-package.rst +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Update running-configuration-package.rst (`#29 `_) + Add setup steps for candleLight USB-CAN adapter +* Add information about root rights (`#28 `_) + * add information about root rights + * Add information about running a configuration pkg +* Add configuration (`#17 `_) +* Further additions to documentation (`#16 `_) + * change section titles in configuration package.rst + * add system-interface graphic and other change to configure-package.rst + * add description to Proxy Driver + * Add ros2_canopen logo +* Configuration manager integration (`#14 `_) + * Add longer startup delay and test documentation + * Add speed and position publisher + * Create Configuration Manager + * make MasterNode a component and add configuration manager functionalities + * add configuration manager functionalities + * add configuration manger functionalities + * Add documentation for Configuration Manager + * add info messages and documentation + * update launch files and configuration fiels + * add can_utils package + * add info text + * simplify dependencies + * remove tests from can_utils + * avoid tests for canopen_utils + * changes info logging and adds nmt and sdo tests + * add tests + * remove launch_tests from cmake +* Update configuration package tutorial (`#5 `_) + * Update configuration package tutorial + * improve bus configuration, development objectives and other documentation and add css for tables + * Reviewed Configuration Package Guide + * Add alpha test description + * Add test decriptions + * Update device manager description with new graphics +* Update documentation +* Merge branch 'licenses' into 'master' + add licenses to each package + See merge request ipa326/ros-industrial/ros2_canopen!22 +* rename ros2_canopen package to canopen +* add licenses to each package +* Merge branch 'renaming' into 'master' + Update package names to fit ROS2 naming rules better + See merge request ipa326/ros-industrial/ros2_canopen!21 +* rename packages to fit ROS2 conventions better +* Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu diff --git a/canopen/package.xml b/canopen/package.xml index 45ffbd0d..ed835de3 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -5,7 +5,7 @@ 0.0.1 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos - Apache License 2.0 + Apache-2.0 ament_cmake canopen_interfaces diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst new file mode 100644 index 00000000..e95a4d67 --- /dev/null +++ b/canopen_402_driver/CHANGELOG.rst @@ -0,0 +1,208 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_402_driver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Reduce processor load (`#111 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Add driver dictionaries (`#110 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + --------- +* Motor Profile Updates (`#101 `_) + * Extend and fix info statement. + * Fix service handler overwriting. + * Consider enum 3 as profiled velocity. Remove some code duplication by reusing private setters in service cbs. Create setter for interpolated position mode. + * Fix cyclic position mode. + * Simplify write method cases defined by mode of op. +* Merge pull request `#100 `_ from ipa-vsp/bugfix/fix-vel-pos-scaling + Proper scaling from the device +* proper vel and pos scaling from device +* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) + * Boost lock free queue implemetation + * include boost libraries in CMakelists + * Testing rpdo/tpdo ping pond + * pre-commit changes + * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking + * Fixed typo + * pre-commit update + * FIxed: properly export Boost libraries + * Update code documentation +* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) + * Add Interpolated Position Mode (linear only, no PT or PVT) + * add interpolated position mode to system interface + * Add interpolated position mode to controllers. + * Add to interpolated position mode to documentation + --------- +* Simplify 402 driver (`#89 `_) + * Split motor.hpp and motor.cpp into different files + * Fix missing symbol error + --------- +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Remove false license statements (`#76 `_) + * Remove false license statements +* Scaling factors for position and velocity (`#74 `_) + * Introduce scaling factors +* Merge branch 'livanov93-motor-profile' +* Set target based on condition. +* Expose 402 main functionalities to ros2_control system interface. +* Extend 402 functions via public methods - same as callback based ones. +* Adapt 402 hardware interface to device container getter. +* Add position and speed getter. +* Add unit tests for canopen_core (`#64 `_) + * Testing changes to canopen_core + * Testing changes to canopen_base_driver and canopen_402_driver + * Add canopen_core tests (90% coverage) + * Fix DriverException error in canopen_402_driver + * Catch errors in nmt and rpdo listeners + * Fix naming issues + * Fix deactivate transition + * Fix unclean shutdown +* Publish to joint_states, not joint_state (`#63 `_) + Co-authored-by: G.A. vd. Hoorn + Co-authored-by: Christoph Hellmann Santos +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* Fix 402 issues from testing +* Streamline logging +* Fix node_canopen_402_drivers add_to_master and handlers +* Fix get speed and get position +* Feature parity for lifecycle nodes +* Add 402 driver functions for ros2_control +* Add master dcfs and remove from gitignore +* Add device container and general changes to make things work. +* canopen_402_driver adaption to new framework +* Add in code documentation for canopen_core (`#53 `_) + * Document device container node + * Document lely_master_bridge + * Document Lifecycle Device Container + * Document Lifecycle Device Manager + * Document LifecyleMasterNode + * Document Master Node + * Fix error + * Document lifecycle base driver + * Document lely bridge + * Document canopen_proxy_driver + * Document canopen_402_driver +* Add configuration parameter passthrough (`#52 `_) +* Publish joint state instead of velocity topics (`#47 `_) + * disable loader service + * add custom target/command and install to macro + * publish jointstate + * correct variable name squiggle + * Minor changes to driver and slave + * Update lely core library + * Add sensor_msgs to dependencies + * Remove artifacts + * Remove some artifacts +* Solve Boot Error (`#49 `_) +* Remove some unecessary changes. +* Remove pedantic cmake flags. +* fix ament_export_libraries (`#45 `_) +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Fix remote object bug in canopen_402_driver. (`#40 `_) +* Merge pull request `#1 `_ from livanov93/canopen-system-interface + [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities +* Remove pedantic cmake flags. +* Configuration manager integration (`#14 `_) + * Add longer startup delay and test documentation + * Add speed and position publisher + * Create Configuration Manager + * make MasterNode a component and add configuration manager functionalities + * add configuration manager functionalities + * add configuration manger functionalities + * Add documentation for Configuration Manager + * add info messages and documentation + * update launch files and configuration fiels + * add can_utils package + * add info text + * simplify dependencies + * remove tests from can_utils + * avoid tests for canopen_utils + * changes info logging and adds nmt and sdo tests + * add tests + * remove launch_tests from cmake +* Merge branch 'licenses' into 'master' + add licenses to each package + See merge request ipa326/ros-industrial/ros2_canopen!22 +* update package descriptions +* add licenses to each package +* Merge branch 'renaming' into 'master' + Update package names to fit ROS2 naming rules better + See merge request ipa326/ros-industrial/ros2_canopen!21 +* add missing variable for cyclic position mode +* store tests of proxy driver in canopen_proxy_driver +* rename packages to fit ROS2 conventions better +* Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, G.A. vd. Hoorn, Lovro, Vishnuprasad Prachandabhanu, livanov93 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst new file mode 100644 index 00000000..718540a0 --- /dev/null +++ b/canopen_base_driver/CHANGELOG.rst @@ -0,0 +1,163 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_base_driver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Reduce processor load (`#111 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Add driver dictionaries (`#110 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + --------- +* Fix stack smashing (`#103 `_) +* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) + * Boost lock free queue implemetation + * include boost libraries in CMakelists + * Testing rpdo/tpdo ping pond + * pre-commit changes + * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking + * Fixed typo + * pre-commit update + * FIxed: properly export Boost libraries + * Update code documentation +* Add EMCY callback to base driver (`#91 `_) +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Add unit tests for canopen_core (`#64 `_) + * Testing changes to canopen_core + * Testing changes to canopen_base_driver and canopen_402_driver + * Add canopen_core tests (90% coverage) + * Fix DriverException error in canopen_402_driver + * Catch errors in nmt and rpdo listeners + * Fix naming issues + * Fix deactivate transition + * Fix unclean shutdown +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* Streamline logging +* Fix tests base driver +* Feature parity for lifecycle nodes +* Add master dcfs and remove from gitignore +* Integration with ros2_control +* Add device container and general changes to make things work. +* Change canopen_base_driver for templating problems +* Make changes to canopen_base_driver for new structure +* Add in code documentation for canopen_core (`#53 `_) + * Document device container node + * Document lely_master_bridge + * Document Lifecycle Device Container + * Document Lifecycle Device Manager + * Document LifecyleMasterNode + * Document Master Node + * Fix error + * Document lifecycle base driver + * Document lely bridge + * Document canopen_proxy_driver + * Document canopen_402_driver +* Add configuration parameter passthrough (`#52 `_) +* Solve Boot Error (`#49 `_) +* Remove some unecessary changes. +* Remove pedantic cmake flags. +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Merge pull request `#1 `_ from livanov93/canopen-system-interface + [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities +* Remove pedantic cmake flags. +* Configuration manager integration (`#14 `_) + * Add longer startup delay and test documentation + * Add speed and position publisher + * Create Configuration Manager + * make MasterNode a component and add configuration manager functionalities + * add configuration manager functionalities + * add configuration manger functionalities + * Add documentation for Configuration Manager + * add info messages and documentation + * update launch files and configuration fiels + * add can_utils package + * add info text + * simplify dependencies + * remove tests from can_utils + * avoid tests for canopen_utils + * changes info logging and adds nmt and sdo tests + * add tests + * remove launch_tests from cmake +* Merge branch 'licenses' into 'master' + add licenses to each package + See merge request ipa326/ros-industrial/ros2_canopen!22 +* update package descriptions +* add licenses to each package +* Merge branch 'renaming' into 'master' + Update package names to fit ROS2 naming rules better + See merge request ipa326/ros-industrial/ros2_canopen!21 +* rename packages to fit ROS2 conventions better +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp index 31ee7041..d914b17c 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_BASE_DRIVER #define NODE_CANOPEN_BASE_DRIVER diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index 21444ddf..8189d23b 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_BASE_DRIVER_IMPL #define NODE_CANOPEN_BASE_DRIVER_IMPL #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" diff --git a/canopen_base_driver/src/base_driver.cpp b/canopen_base_driver/src/base_driver.cpp index a43ef069..d4b4a8cc 100644 --- a/canopen_base_driver/src/base_driver.cpp +++ b/canopen_base_driver/src/base_driver.cpp @@ -11,6 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. + #include "canopen_base_driver/base_driver.hpp" using namespace ros2_canopen; diff --git a/canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp b/canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp index cdfcbc07..25edb6ab 100644 --- a/canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp +++ b/canopen_base_driver/src/node_interfaces/node_canopen_base_driver.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" #include "canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp" #include "canopen_core/driver_error.hpp" diff --git a/canopen_base_driver/test/test_base_driver_component.cpp b/canopen_base_driver/test/test_base_driver_component.cpp index 577e12c2..c3b0d380 100644 --- a/canopen_base_driver/test/test_base_driver_component.cpp +++ b/canopen_base_driver/test/test_base_driver_component.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include diff --git a/canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp b/canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp index 56daa1c8..7916c2a7 100644 --- a/canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp +++ b/canopen_base_driver/test/test_node_canopen_base_driver_ros.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include "canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp" diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst new file mode 100644 index 00000000..e9174b5e --- /dev/null +++ b/canopen_core/CHANGELOG.rst @@ -0,0 +1,227 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_core +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Reduce processor load (`#111 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Add driver dictionaries (`#110 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + --------- +* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) + * Boost lock free queue implemetation + * include boost libraries in CMakelists + * Testing rpdo/tpdo ping pond + * pre-commit changes + * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking + * Fixed typo + * pre-commit update + * FIxed: properly export Boost libraries + * Update code documentation +* Add EMCY callback to base driver (`#91 `_) +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge branch 'bjsowa-master' +* Don't treat options as driver +* Merge branch 'master' of github.com:bjsowa/ros2_canopen into bjsowa-master +* Don't treat options section as another device +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Disable device container tests (`#77 `_) +* Handle demand set master failure (`#70 `_) + * adapt fake cia402 slave + * Add retries for demand_set_master in case of failure +* Doxygen documentation for canopen_core (`#78 `_) + * canopen_core in code documentation + * Some more documentation +* intra_process_comms +* Doxygen documentation for canopen_core (`#78 `_) + * canopen_core in code documentation + * Some more documentation +* intra_process_comms +* Add unit tests for canopen_core (`#64 `_) + * Testing changes to canopen_core + * Testing changes to canopen_base_driver and canopen_402_driver + * Add canopen_core tests (90% coverage) + * Fix DriverException error in canopen_402_driver + * Catch errors in nmt and rpdo listeners + * Fix naming issues + * Fix deactivate transition + * Fix unclean shutdown +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* undo renaming can_interface_name -> can_interface +* Undo formatting in ros2_control +* Add lifecycle manager to device_container library +* Remove canopen_lifecycle.launch.py as it i no longer needed. +* Streamline logging +* Integrate lifecycle manager +* Fix tests canopen_core +* Fix canopen_master_driver for explicit instantiation +* Add CanopenDriverInterface Documentation +* Add master dcfs and remove from gitignore +* add node interface accessor and lifecycle information to drivers +* Add type accessor functions to device_container +* Integration with ros2_control +* Add function to device container +* Add device container and general changes to make things work. +* Update CmakeFile of canopen core +* Add tests to canopen core +* Remove device and do some renaming +* Add node base classes +* Add errors +* Add core node interfaces +* Add in code documentation for canopen_core (`#53 `_) + * Document device container node + * Document lely_master_bridge + * Document Lifecycle Device Container + * Document Lifecycle Device Manager + * Document LifecyleMasterNode + * Document Master Node + * Fix error + * Document lifecycle base driver + * Document lely bridge + * Document canopen_proxy_driver + * Document canopen_402_driver +* Add configuration parameter passthrough (`#52 `_) +* Publish joint state instead of velocity topics (`#47 `_) + * disable loader service + * add custom target/command and install to macro + * publish jointstate + * correct variable name squiggle + * Minor changes to driver and slave + * Update lely core library + * Add sensor_msgs to dependencies + * Remove artifacts + * Remove some artifacts +* Disable dynamic loading for containers (`#50 `_) + * disable loader service + * Remove artifacts +* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface + Add generic system interface for ros2_control +* Update canopen_core/CMakeLists.txt +* Fix merging issues. +* Merge remote-tracking branch 'livanov/fix-dependencies' into canopen-system-interface +* Remove some unecessary changes. +* Apply suggestions from code review +* Add nmt and rpdo callbacks. +* Start device manager in system interface. +* fix-ifndef-directive (`#43 `_) +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Merge pull request `#1 `_ from livanov93/canopen-system-interface + [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities +* Apply suggestions from code review +* Remove pedantic cmake flags. +* Add nmt and rpdo callbacks. +* Start device manager in system interface. +* Add yaml_cpp_vendor as dependency for master_node (`#18 `_) + * commented out the yaml_cpp vendor since it doesnt let it build on the 20.04 machine + * Update CMakeLists.txt + * Update CMakeLists.txt + * Update CMakeLists.txt + Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> +* Configuration manager integration (`#14 `_) + * Add longer startup delay and test documentation + * Add speed and position publisher + * Create Configuration Manager + * make MasterNode a component and add configuration manager functionalities + * add configuration manager functionalities + * add configuration manger functionalities + * Add documentation for Configuration Manager + * add info messages and documentation + * update launch files and configuration fiels + * add can_utils package + * add info text + * simplify dependencies + * remove tests from can_utils + * avoid tests for canopen_utils + * changes info logging and adds nmt and sdo tests + * add tests + * remove launch_tests from cmake +* Add Industrial CI for foxy and galactic (`#10 `_) + * Add ci + * remove rclcpp_lifecycle stuff from CMAKElists.txt + * add rclcpp_lifecycle and msgs + * add galactic to ci and limit ci to prs and pushes to master +* Merge branch 'licenses' into 'master' + add licenses to each package + See merge request ipa326/ros-industrial/ros2_canopen!22 +* add header guards +* update package descriptions +* add license in files +* Merge branch 'renaming' into 'master' + Update package names to fit ROS2 naming rules better + See merge request ipa326/ros-industrial/ros2_canopen!21 +* store tests of proxy driver in canopen_proxy_driver +* rename packages to fit ROS2 conventions better +* Contributors: Aulon Bajrami, Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_core/include/canopen_core/device_container_error.hpp b/canopen_core/include/canopen_core/device_container_error.hpp index d43a66ba..03b6a7cf 100644 --- a/canopen_core/include/canopen_core/device_container_error.hpp +++ b/canopen_core/include/canopen_core/device_container_error.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef DEVICE_CONTAINER_ERROR_HPP_ #define DEVICE_CONTAINER_ERROR_HPP_ diff --git a/canopen_core/include/canopen_core/driver_error.hpp b/canopen_core/include/canopen_core/driver_error.hpp index 657b6c0a..993dd315 100644 --- a/canopen_core/include/canopen_core/driver_error.hpp +++ b/canopen_core/include/canopen_core/driver_error.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef DRIVER_ERROR_HPP_ #define DRIVER_ERROR_HPP_ diff --git a/canopen_core/include/canopen_core/driver_node.hpp b/canopen_core/include/canopen_core/driver_node.hpp index c7f6b4ab..83c306d8 100644 --- a/canopen_core/include/canopen_core/driver_node.hpp +++ b/canopen_core/include/canopen_core/driver_node.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef DRIVER_NODE_HPP_ #define DRIVER_NODE_HPP_ diff --git a/canopen_core/include/canopen_core/master_error.hpp b/canopen_core/include/canopen_core/master_error.hpp index 947302c5..778ea8e5 100644 --- a/canopen_core/include/canopen_core/master_error.hpp +++ b/canopen_core/include/canopen_core/master_error.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef MASTER_ERROR_HPP_ #define MASTER_ERROR_HPP_ diff --git a/canopen_core/include/canopen_core/master_node.hpp b/canopen_core/include/canopen_core/master_node.hpp index b779b497..12d7d6b7 100644 --- a/canopen_core/include/canopen_core/master_node.hpp +++ b/canopen_core/include/canopen_core/master_node.hpp @@ -1,3 +1,16 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MASTER_NODE_HPP_ #define MASTER_NODE_HPP_ diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp index fcae632a..a76ee758 100644 --- a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_DRIVER_HPP_ #define NODE_CANOPEN_DRIVER_HPP_ diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp index d4b918b8..8f80c808 100644 --- a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver_interface.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_DRIVER_INTERFACE_HPP_ #define NODE_CANOPEN_DRIVER_INTERFACE_HPP_ diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp index 73a96c2e..373fe502 100644 --- a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_MASTER_HPP_ #define NODE_CANOPEN_MASTER_HPP_ diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp index 6357e0c0..d7087307 100644 --- a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master_interface.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_MASTER_INTERFACE_HPP_ #define NODE_CANOPEN_MASTER_INTERFACE_HPP_ diff --git a/canopen_core/launch/canopen.launch.py b/canopen_core/launch/canopen.launch.py index 38728024..d7655cd8 100644 --- a/canopen_core/launch/canopen.launch.py +++ b/canopen_core/launch/canopen.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os import sys diff --git a/canopen_core/src/configuration_manager.cpp b/canopen_core/src/configuration_manager.cpp index 490d7f13..9b42802d 100644 --- a/canopen_core/src/configuration_manager.cpp +++ b/canopen_core/src/configuration_manager.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/configuration_manager.hpp" #include diff --git a/canopen_core/src/device_container_error.cpp b/canopen_core/src/device_container_error.cpp index 33e13de7..a8bc73c5 100644 --- a/canopen_core/src/device_container_error.cpp +++ b/canopen_core/src/device_container_error.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/device_container_error.hpp" #include namespace ros2_canopen diff --git a/canopen_core/src/driver_error.cpp b/canopen_core/src/driver_error.cpp index 8fae1cb8..626295a5 100644 --- a/canopen_core/src/driver_error.cpp +++ b/canopen_core/src/driver_error.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/driver_error.hpp" #include namespace ros2_canopen diff --git a/canopen_core/src/driver_node.cpp b/canopen_core/src/driver_node.cpp index 3fdb9071..f8fc786a 100644 --- a/canopen_core/src/driver_node.cpp +++ b/canopen_core/src/driver_node.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/driver_node.hpp" using namespace ros2_canopen; diff --git a/canopen_core/src/lifecycle_manager.cpp b/canopen_core/src/lifecycle_manager.cpp index cd9b9b43..63bf8bf7 100644 --- a/canopen_core/src/lifecycle_manager.cpp +++ b/canopen_core/src/lifecycle_manager.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/lifecycle_manager.hpp" namespace ros2_canopen diff --git a/canopen_core/src/master_error.cpp b/canopen_core/src/master_error.cpp index 41c72bde..ba117fb2 100644 --- a/canopen_core/src/master_error.cpp +++ b/canopen_core/src/master_error.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/master_error.hpp" #include namespace ros2_canopen diff --git a/canopen_core/src/master_node.cpp b/canopen_core/src/master_node.cpp index 0954380f..0c070f3a 100644 --- a/canopen_core/src/master_node.cpp +++ b/canopen_core/src/master_node.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/master_node.hpp" using namespace ros2_canopen; diff --git a/canopen_core/src/node_interfaces/node_canopen_driver.cpp b/canopen_core/src/node_interfaces/node_canopen_driver.cpp index f0c2c205..6668a0df 100644 --- a/canopen_core/src/node_interfaces/node_canopen_driver.cpp +++ b/canopen_core/src/node_interfaces/node_canopen_driver.cpp @@ -1,3 +1,16 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include "canopen_core/node_interfaces/node_canopen_driver.hpp" using namespace std::chrono_literals; diff --git a/canopen_core/src/node_interfaces/node_canopen_master.cpp b/canopen_core/src/node_interfaces/node_canopen_master.cpp index 1dc08cf9..683288e0 100644 --- a/canopen_core/src/node_interfaces/node_canopen_master.cpp +++ b/canopen_core/src/node_interfaces/node_canopen_master.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/node_interfaces/node_canopen_master.hpp" template class ros2_canopen::node_interfaces::NodeCanopenMaster; diff --git a/canopen_core/test/test_canopen_driver.cpp b/canopen_core/test/test_canopen_driver.cpp index ef47fb59..33b0e5ab 100644 --- a/canopen_core/test/test_canopen_driver.cpp +++ b/canopen_core/test/test_canopen_driver.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "canopen_core/driver_node.hpp" #include "gmock/gmock.h" diff --git a/canopen_core/test/test_canopen_master.cpp b/canopen_core/test/test_canopen_master.cpp index 2eb16c84..626c3e58 100644 --- a/canopen_core/test/test_canopen_master.cpp +++ b/canopen_core/test/test_canopen_master.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "canopen_core/master_node.hpp" #include "gmock/gmock.h" diff --git a/canopen_core/test/test_device_container.cpp b/canopen_core/test/test_device_container.cpp index 6cbe0afe..1004fd26 100644 --- a/canopen_core/test/test_device_container.cpp +++ b/canopen_core/test/test_device_container.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "canopen_core/device_container.hpp" #include "canopen_core/device_container_error.hpp" diff --git a/canopen_core/test/test_errors.cpp b/canopen_core/test/test_errors.cpp index 09ae656c..246b1ce5 100644 --- a/canopen_core/test/test_errors.cpp +++ b/canopen_core/test/test_errors.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "canopen_core/device_container_error.hpp" #include "canopen_core/driver_error.hpp" diff --git a/canopen_core/test/test_lifecycle_canopen_driver.cpp b/canopen_core/test/test_lifecycle_canopen_driver.cpp index 7526a1f2..38065aca 100644 --- a/canopen_core/test/test_lifecycle_canopen_driver.cpp +++ b/canopen_core/test/test_lifecycle_canopen_driver.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "canopen_core/driver_node.hpp" #include "gmock/gmock.h" diff --git a/canopen_core/test/test_lifecycle_canopen_master.cpp b/canopen_core/test/test_lifecycle_canopen_master.cpp index 68c53d77..659bce6c 100644 --- a/canopen_core/test/test_lifecycle_canopen_master.cpp +++ b/canopen_core/test/test_lifecycle_canopen_master.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "canopen_core/master_node.hpp" #include "gmock/gmock.h" diff --git a/canopen_core/test/test_lifecycle_manager.cpp b/canopen_core/test/test_lifecycle_manager.cpp index fdf7fa26..04b6faf9 100644 --- a/canopen_core/test/test_lifecycle_manager.cpp +++ b/canopen_core/test/test_lifecycle_manager.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "canopen_core/lifecycle_manager.hpp" #include "gmock/gmock.h" diff --git a/canopen_core/test/test_node_canopen_driver.cpp b/canopen_core/test/test_node_canopen_driver.cpp index 557fe273..fee5bee1 100644 --- a/canopen_core/test/test_node_canopen_driver.cpp +++ b/canopen_core/test/test_node_canopen_driver.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/driver_node.hpp" #include "canopen_core/node_interfaces/node_canopen_driver.hpp" #include "gmock/gmock.h" diff --git a/canopen_core/test/test_node_canopen_master.cpp b/canopen_core/test/test_node_canopen_master.cpp index a0f4a705..c43f1f94 100644 --- a/canopen_core/test/test_node_canopen_master.cpp +++ b/canopen_core/test/test_node_canopen_master.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_core/master_node.hpp" #include "canopen_core/node_interfaces/node_canopen_master.hpp" #include "gmock/gmock.h" diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst new file mode 100644 index 00000000..5c0f975b --- /dev/null +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -0,0 +1,54 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_fake_slaves +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add driver dictionaries (`#110 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + --------- +* Include rpdo/tpdo test in launch_test. (`#98 `_) + * Implement rpdo/tpdo test + * Removed unnecessary files +* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) + * Boost lock free queue implemetation + * include boost libraries in CMakelists + * Testing rpdo/tpdo ping pond + * pre-commit changes + * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking + * Fixed typo + * pre-commit update + * FIxed: properly export Boost libraries + * Update code documentation +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Remove references to sympy.true (`#84 `_) + Co-authored-by: James Ward +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Clean cia402 fake shutdown (`#72 `_) + * adapt fake cia402 slave +* intra_process_comms +* intra_process_comms +* Rename canopen_mock_slave package to canopen_fake_slaves (`#66 `_) + * Testing changes to canopen_core + * Testing changes to canopen_base_driver and canopen_402_driver + * Add canopen_core tests (90% coverage) + * Fix DriverException error in canopen_402_driver + * Catch errors in nmt and rpdo listeners + * Fix naming issues + * Fix deactivate transition + * Fix unclean shutdown + * Rename canopen_mock_slave to canopen_fake_slaves + * Build flage CANOPEN_ENABLED for disabling tests on CI. +* Contributors: Błażej Sowa, Christoph Hellmann Santos, James Ward, Vishnuprasad Prachandabhanu diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp index 2282c0e2..68013056 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp @@ -1,3 +1,16 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef SLAVE_HPP #define SLAVE_HPP diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index 89cef4ef..21b9a3ba 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef BASIC_SLAVE_HPP #define BASIC_SLAVE_HPP #include diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp index 3496e768..61f1b5f7 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef CIA402_SLAVE_HPP #define CIA402_SLAVE_HPP #include diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp index 831026c9..ac000bb7 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp @@ -1,6 +1,26 @@ #ifndef __MOTIONGENERATOR_H__ #define __MOTIONGENERATOR_H__ +// Copyright (c) 2016 AerDronix, https://aerdronix.wordpress.com/ +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + #include /** * Generates the analytical solution for the trapezoidal motion. diff --git a/canopen_fake_slaves/launch/basic_slave.launch.py b/canopen_fake_slaves/launch/basic_slave.launch.py index a5797738..b999e037 100644 --- a/canopen_fake_slaves/launch/basic_slave.launch.py +++ b/canopen_fake_slaves/launch/basic_slave.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os import sys diff --git a/canopen_fake_slaves/launch/cia402_slave.launch.py b/canopen_fake_slaves/launch/cia402_slave.launch.py index eae159b6..90c250a5 100644 --- a/canopen_fake_slaves/launch/cia402_slave.launch.py +++ b/canopen_fake_slaves/launch/cia402_slave.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os import sys diff --git a/canopen_fake_slaves/src/basic_slave.cpp b/canopen_fake_slaves/src/basic_slave.cpp index f609fbb0..2861a655 100644 --- a/canopen_fake_slaves/src/basic_slave.cpp +++ b/canopen_fake_slaves/src/basic_slave.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_fake_slaves/basic_slave.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/canopen_fake_slaves/src/cia402_slave.cpp b/canopen_fake_slaves/src/cia402_slave.cpp index 093b54b9..725a2a2f 100644 --- a/canopen_fake_slaves/src/cia402_slave.cpp +++ b/canopen_fake_slaves/src/cia402_slave.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_fake_slaves/cia402_slave.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/canopen_fake_slaves/src/motion_generator.cpp b/canopen_fake_slaves/src/motion_generator.cpp index 662c2948..34a5c8de 100644 --- a/canopen_fake_slaves/src/motion_generator.cpp +++ b/canopen_fake_slaves/src/motion_generator.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_fake_slaves/motion_generator.hpp" #include #include diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst new file mode 100644 index 00000000..863c4d7f --- /dev/null +++ b/canopen_interfaces/CHANGELOG.rst @@ -0,0 +1,56 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Adapt package xml +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Apply suggestions from code review +* Remove pedantic cmake flags. +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Apply suggestions from code review +* Remove pedantic cmake flags. +* Merge branch 'licenses' into 'master' + add licenses to each package + See merge request ipa326/ros-industrial/ros2_canopen!22 +* rename canopen +* add licenses to each package +* Merge branch 'renaming' into 'master' + Update package names to fit ROS2 naming rules better + See merge request ipa326/ros-industrial/ros2_canopen!21 +* rename packages to fit ROS2 conventions better +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 41eef5f3..9cf58cd4 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -5,7 +5,7 @@ 0.0.1 Services and Messages for ros2_canopen stack Christoph Hellmann Santos - Apache2.0 + Apache-2.0 ament_cmake diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst new file mode 100644 index 00000000..6b124810 --- /dev/null +++ b/canopen_master_driver/CHANGELOG.rst @@ -0,0 +1,35 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_master_driver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* undo renaming can_interface_name -> can_interface +* Streamline logging +* Fix canopen_master_driver tests +* Fix canopen_master_driver for explicit instantiation +* Feature parity for lifecycle nodes +* Add master dcfs and remove from gitignore +* Add device container and general changes to make things work. +* Add canopen_master_driver package and contents +* Contributors: Błażej Sowa, Christoph Hellmann Santos diff --git a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp index e3efc998..1bd58933 100644 --- a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp +++ b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp @@ -1,3 +1,18 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_BASIC_MASTER_HPP_ #define NODE_CANOPEN_BASIC_MASTER_HPP_ diff --git a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp index aa00bf11..776f267a 100644 --- a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp +++ b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp @@ -1,3 +1,18 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_ #define NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_ diff --git a/canopen_master_driver/src/lifecycle_master_driver.cpp b/canopen_master_driver/src/lifecycle_master_driver.cpp index fb8bfcb9..47d1d440 100644 --- a/canopen_master_driver/src/lifecycle_master_driver.cpp +++ b/canopen_master_driver/src/lifecycle_master_driver.cpp @@ -1,3 +1,18 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_master_driver/lifecycle_master_driver.hpp" namespace ros2_canopen diff --git a/canopen_master_driver/src/master_driver.cpp b/canopen_master_driver/src/master_driver.cpp index b047a9f4..c31484c3 100644 --- a/canopen_master_driver/src/master_driver.cpp +++ b/canopen_master_driver/src/master_driver.cpp @@ -1,3 +1,18 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_master_driver/master_driver.hpp" namespace ros2_canopen diff --git a/canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp b/canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp index f972e1af..b55f3aaa 100644 --- a/canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp +++ b/canopen_master_driver/src/node_interfaces/node_canopen_basic_master.cpp @@ -1,3 +1,18 @@ +// Copyright 2022 Harshavadan Deshpande +// Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" #include "canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp" diff --git a/canopen_master_driver/test/test_master_driver_component.cpp b/canopen_master_driver/test/test_master_driver_component.cpp index 5e5b262e..63725ca6 100644 --- a/canopen_master_driver/test/test_master_driver_component.cpp +++ b/canopen_master_driver/test/test_master_driver_component.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include diff --git a/canopen_master_driver/test/test_node_canopen_basic_master.cpp b/canopen_master_driver/test/test_node_canopen_basic_master.cpp index 8535e419..b0c67e19 100644 --- a/canopen_master_driver/test/test_node_canopen_basic_master.cpp +++ b/canopen_master_driver/test/test_node_canopen_basic_master.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" #include "gtest/gtest.h" diff --git a/canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp b/canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp index 5138ca44..51e7f43a 100644 --- a/canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp +++ b/canopen_master_driver/test/test_node_canopen_basic_master_ros.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp" diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst new file mode 100644 index 00000000..a1a3b3ac --- /dev/null +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -0,0 +1,128 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_proxy_driver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Add driver dictionaries (`#110 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + --------- +* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) + * Boost lock free queue implemetation + * include boost libraries in CMakelists + * Testing rpdo/tpdo ping pond + * pre-commit changes + * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking + * Fixed typo + * pre-commit update + * FIxed: properly export Boost libraries + * Update code documentation +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* Feature parity for lifecycle nodes +* Add master dcfs and remove from gitignore +* Integration with ros2_control +* Add device container and general changes to make things work. +* Update header guards +* Add canopen_proxy_driver with new framework +* Add in code documentation for canopen_core (`#53 `_) + * Document device container node + * Document lely_master_bridge + * Document Lifecycle Device Container + * Document Lifecycle Device Manager + * Document LifecyleMasterNode + * Document Master Node + * Fix error + * Document lifecycle base driver + * Document lely bridge + * Document canopen_proxy_driver + * Document canopen_402_driver +* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface + Add generic system interface for ros2_control +* Merge remote-tracking branch 'livanov/fix-dependencies' into canopen-system-interface +* Remove unneccesary deps. +* Apply suggestions from code review +* Expose necessary stuff from proxy driver. +* Add nmt and rpdo callbacks. +* Start device manager in system interface. +* Enable easy testing temporarily. +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Merge pull request `#1 `_ from livanov93/canopen-system-interface + [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities +* Apply suggestions from code review +* Expose necessary stuff from proxy driver. +* Add nmt and rpdo callbacks. +* Enable easy testing temporarily. +* Configuration manager integration (`#14 `_) + * Add longer startup delay and test documentation + * Add speed and position publisher + * Create Configuration Manager + * make MasterNode a component and add configuration manager functionalities + * add configuration manager functionalities + * add configuration manger functionalities + * Add documentation for Configuration Manager + * add info messages and documentation + * update launch files and configuration fiels + * add can_utils package + * add info text + * simplify dependencies + * remove tests from can_utils + * avoid tests for canopen_utils + * changes info logging and adds nmt and sdo tests + * add tests + * remove launch_tests from cmake +* Merge branch 'licenses' into 'master' + add licenses to each package + See merge request ipa326/ros-industrial/ros2_canopen!22 +* update package descriptions +* add licenses to each package +* Merge branch 'renaming' into 'master' + Update package names to fit ROS2 naming rules better + See merge request ipa326/ros-industrial/ros2_canopen!21 +* use proxy.yml instead simple.yml in CMakeLists.txt +* store tests of proxy driver in canopen_proxy_driver +* rename packages to fit ROS2 conventions better +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp index 0219b4c9..b73759c1 100644 --- a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp +++ b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_PROXY_DRIVER #define NODE_CANOPEN_PROXY_DRIVER diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp index 53104dd4..c4fdf613 100644 --- a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp +++ b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_ #define NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_ diff --git a/canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp b/canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp index 07ef10ff..010cfbaa 100644 --- a/canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp +++ b/canopen_proxy_driver/src/node_interfaces/node_canopen_proxy_driver.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp" diff --git a/canopen_proxy_driver/test/test_driver_component.cpp b/canopen_proxy_driver/test/test_driver_component.cpp index 5d18ad7d..c7e03bb1 100644 --- a/canopen_proxy_driver/test/test_driver_component.cpp +++ b/canopen_proxy_driver/test/test_driver_component.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include diff --git a/canopen_proxy_driver/test/test_node_interface.cpp b/canopen_proxy_driver/test/test_node_interface.cpp index 4f285262..b37378c3 100644 --- a/canopen_proxy_driver/test/test_node_interface.cpp +++ b/canopen_proxy_driver/test/test_node_interface.cpp @@ -1,3 +1,17 @@ +// Copyright 2022 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include "canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp" diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst new file mode 100644 index 00000000..150e69e2 --- /dev/null +++ b/canopen_ros2_control/CHANGELOG.rst @@ -0,0 +1,158 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_ros2_control +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add license files +* Reduce processor load (`#111 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Motor Profile Updates (`#101 `_) + * Extend and fix info statement. + * Fix service handler overwriting. + * Consider enum 3 as profiled velocity. Remove some code duplication by reusing private setters in service cbs. Create setter for interpolated position mode. + * Fix cyclic position mode. + * Simplify write method cases defined by mode of op. +* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) + * Add Interpolated Position Mode (linear only, no PT or PVT) + * add interpolated position mode to system interface + * Add interpolated position mode to controllers. + * Add to interpolated position mode to documentation + --------- +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Merge branch 'livanov93-motor-profile' +* Remove scalers +* Fix internal launch test. +* Add virtual can example for cia 402. +* Fix joint states scaling. +* Update runtime deps. +* State and command interfaces. +* Fix feedback for services for proxy driver and controlller. +* Handle init, recover, halt. Switch modes. +* Set target based on condition. +* Duplicate some code for configure, init, write phase from proxy driver. +* Add basic read and write. Divide targets into position, velocity, effort interfaces. +* Update proxy canopen system. +* Add vel and pos interfaves. +* Expose 402 main functionalities to ros2_control system interface. +* Prepare read/write/ +* Adapt 402 hardware interface to device container getter. +* Fix public fcn visibility. +* To protected members for easier inheritance policy. +* Update dependencies. +* State and command interfaces. +* Add position and speed getter. +* Add bare-bone 402 profile system interface. +* Rename canopen_mock_slave package to canopen_fake_slaves (`#66 `_) + * Testing changes to canopen_core + * Testing changes to canopen_base_driver and canopen_402_driver + * Add canopen_core tests (90% coverage) + * Fix DriverException error in canopen_402_driver + * Catch errors in nmt and rpdo listeners + * Fix naming issues + * Fix deactivate transition + * Fix unclean shutdown + * Rename canopen_mock_slave to canopen_fake_slaves + * Build flage CANOPEN_ENABLED for disabling tests on CI. +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* undo renaming can_interface_name -> can_interface +* Undo formatting in ros2_control +* Integration with ros2_control +* Merge pull request `#54 `_ from StoglRobotics-forks/canopen-system-interface + Add generic Controller for Canopen +* Apply suggestions from code review + Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> +* Merge pull request `#2 `_ from livanov93/canopen-controller + Canopen controller +* Merge branch 'canopen-system-interface' into canopen-controller +* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface + Add generic system interface for ros2_control +* Integrate launch files into existing launch files in canopen_mock_slaves & canopen_tests + Merge pull request `#6 `_ from ipa-cmh/canopen-system-interface +* Adapt canopen_system.launch.py for 2 nodes +* Remove bus.yml +* further changes +* update package.xml +* Reorganise test launch system +* Apply suggestions from code review +* Add service one shot mechanisms. +* Expose controller plugin. Start canopen proxy controller instance in example. +* Simplify configuration folder and use existing .eds, .dcf file. Improve test launch file. Update runtime deps. +* Disable test because they can not be eaisly tested. +* Update visibility-control macros. +* Merge remote-tracking branch 'livanov/from-init-to-configure' into canopen-system-interface +* Merge remote-tracking branch 'livanov/fix-dependencies' into canopen-system-interface +* Rename "device manager" to "device container" and disable test because it is now working in the current setup. +* Remove unneccesary deps. +* Add missig dependencies and execute tests only when testing. +* Apply suggestions from code review +* Add internal caching structures for canopen nodes. +* Add nmt and rpdo callbacks. +* Start device manager in system interface. +* Add device manager and executor. +* Print config paths on init. +* Enable easy testing temporarily. +* Introduce canopen system interface. +* Move device manager instantation into on_config. +* Merge pull request `#4 `_ from livanov93/fix-dependencies + [canopen_ros2_control] Dependency fix +* Fix dependencies for canopen_ros2_control. +* Merge pull request `#1 `_ from livanov93/canopen-system-interface + [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities +* Apply suggestions from code review +* Add internal caching structures for canopen nodes. +* Add nmt and rpdo callbacks. +* Start device manager in system interface. +* Add device manager and executor. +* Print config paths on init. +* Enable easy testing temporarily. +* Introduce canopen system interface. +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, livanov93 diff --git a/canopen_ros2_control/LICENSE b/canopen_ros2_control/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/canopen_ros2_control/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst new file mode 100644 index 00000000..e5abfa5d --- /dev/null +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -0,0 +1,99 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_ros2_controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add license files +* Reduce processor load (`#111 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Remove type indication from msg and srv interfaces (`#112 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + --------- +* Motor Profile Updates (`#101 `_) + * Extend and fix info statement. + * Fix service handler overwriting. + * Consider enum 3 as profiled velocity. Remove some code duplication by reusing private setters in service cbs. Create setter for interpolated position mode. + * Fix cyclic position mode. + * Simplify write method cases defined by mode of op. +* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) + * Add Interpolated Position Mode (linear only, no PT or PVT) + * add interpolated position mode to system interface + * Add interpolated position mode to controllers. + * Add to interpolated position mode to documentation + --------- +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Merge branch 'livanov93-motor-profile' +* Fix proxy test. +* Fix internal launch test. +* Fix joint states scaling. +* Update runtime deps. +* Better handling of base class on_methods. +* Add services for one shot interfaces in cia402 profile. +* State and command interfaces. +* Add base function ret values first. +* Prepare cia 402 device controller. +* Fix feedback for services for proxy driver and controlller. +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* Integration with ros2_control +* Merge pull request `#54 `_ from StoglRobotics-forks/canopen-system-interface + Add generic Controller for Canopen +* Implement review requests regarding tests. +* Introduce tests. Adapt proxy controller for easier testing. +* Add service qos specific profile. +* Correct macro names. +* Apply suggestions from code review + Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> +* Merge pull request `#2 `_ from livanov93/canopen-controller + Canopen controller +* Add service one shot mechanisms. +* Add publishing of rpdo and nmt state. +* Expose controller plugin. Start canopen proxy controller instance in example. +* Add dummy services, rt publishers and subscribers to proxy controller. +* Add template for canopen proxy controller. +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Dr.-Ing. Denis Štogl, Lovro, livanov93 diff --git a/canopen_ros2_controllers/LICENSE b/canopen_ros2_controllers/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/canopen_ros2_controllers/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst new file mode 100644 index 00000000..627798c5 --- /dev/null +++ b/canopen_tests/CHANGELOG.rst @@ -0,0 +1,169 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_tests +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Enable simplified bus.yml format (`#115 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + * Add cogen + * Add example usage for cogen + * Remove explicit path + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Reduce processor load (`#111 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Add driver dictionaries (`#110 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + --------- +* Include rpdo/tpdo test in launch_test. (`#98 `_) + * Implement rpdo/tpdo test + * Removed unnecessary files +* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) + * Boost lock free queue implemetation + * include boost libraries in CMakelists + * Testing rpdo/tpdo ping pond + * pre-commit changes + * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking + * Fixed typo + * pre-commit update + * FIxed: properly export Boost libraries + * Update code documentation +* Better organize dependencies (`#88 `_) +* Merge branch 'master' into patch-2 +* Merge branch 'bjsowa-master' +* Add dcf_path to bus.ymls +* Merge branch 'master' of github.com:bjsowa/ros2_canopen into bjsowa-master +* Remove references to sympy.true (`#84 `_) + Co-authored-by: James Ward +* Use options section in test bus config files +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Use @BUS_CONFIG_PATH@ variable in bus configuration files +* intra_process_comms +* intra_process_comms +* Rename canopen_mock_slave package to canopen_fake_slaves (`#66 `_) + * Testing changes to canopen_core + * Testing changes to canopen_base_driver and canopen_402_driver + * Add canopen_core tests (90% coverage) + * Fix DriverException error in canopen_402_driver + * Catch errors in nmt and rpdo listeners + * Fix naming issues + * Fix deactivate transition + * Fix unclean shutdown + * Rename canopen_mock_slave to canopen_fake_slaves + * Build flage CANOPEN_ENABLED for disabling tests on CI. +* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers + Streamline driver and master infrastructure +* undo renaming can_interface_name -> can_interface +* Fix integration tests +* Integration with ros2_control +* Add device container and general changes to make things work. +* Merge branch 'canopen-system-interface' into canopen-controller +* Add configuration parameter passthrough (`#52 `_) +* Publish joint state instead of velocity topics (`#47 `_) + * disable loader service + * add custom target/command and install to macro + * publish jointstate + * correct variable name squiggle + * Minor changes to driver and slave + * Update lely core library + * Add sensor_msgs to dependencies + * Remove artifacts + * Remove some artifacts +* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface + Add generic system interface for ros2_control +* Integrate launch files into existing launch files in canopen_mock_slaves & canopen_tests + Merge pull request `#6 `_ from ipa-cmh/canopen-system-interface +* further changes +* Reorganise test launch system +* Add tests to canopen_tests +* Add tests for lifecycle and normal operation (`#44 `_) +* Update dcfgen cmake integration (`#41 `_) +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, James Ward, Vishnuprasad Prachandabhanu diff --git a/canopen_tests/launch/cia402_lifecycle_setup.launch.py b/canopen_tests/launch/cia402_lifecycle_setup.launch.py index 164b7a96..3b7b95a8 100644 --- a/canopen_tests/launch/cia402_lifecycle_setup.launch.py +++ b/canopen_tests/launch/cia402_lifecycle_setup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os from ament_index_python import get_package_share_directory from launch import LaunchDescription diff --git a/canopen_tests/launch/cia402_setup.launch.py b/canopen_tests/launch/cia402_setup.launch.py index b98222af..c88316aa 100644 --- a/canopen_tests/launch/cia402_setup.launch.py +++ b/canopen_tests/launch/cia402_setup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os from ament_index_python import get_package_share_directory from launch import LaunchDescription diff --git a/canopen_tests/launch/proxy_lifecycle_setup.launch.py b/canopen_tests/launch/proxy_lifecycle_setup.launch.py index 03e01cac..ffe8c187 100644 --- a/canopen_tests/launch/proxy_lifecycle_setup.launch.py +++ b/canopen_tests/launch/proxy_lifecycle_setup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os from ament_index_python import get_package_share_directory from launch import LaunchDescription diff --git a/canopen_tests/launch/proxy_setup.launch.py b/canopen_tests/launch/proxy_setup.launch.py index 6afe382c..867b35e0 100644 --- a/canopen_tests/launch/proxy_setup.launch.py +++ b/canopen_tests/launch/proxy_setup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os from ament_index_python import get_package_share_directory from launch import LaunchDescription diff --git a/canopen_tests/launch/robot_control_setup.launch.py b/canopen_tests/launch/robot_control_setup.launch.py index 9e197f1f..bb1359b2 100644 --- a/canopen_tests/launch/robot_control_setup.launch.py +++ b/canopen_tests/launch/robot_control_setup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution diff --git a/canopen_tests/launch/view_urdf.launch.py b/canopen_tests/launch/view_urdf.launch.py index de2ceea6..74509036 100644 --- a/canopen_tests/launch/view_urdf.launch.py +++ b/canopen_tests/launch/view_urdf.launch.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution diff --git a/canopen_tests/launch_tests/test_proxy_driver.py b/canopen_tests/launch_tests/test_proxy_driver.py index 12ab5eba..dadaf17d 100644 --- a/canopen_tests/launch_tests/test_proxy_driver.py +++ b/canopen_tests/launch_tests/test_proxy_driver.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os from time import sleep import pytest diff --git a/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py index 09eb42ef..b4062045 100644 --- a/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py +++ b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os from time import sleep import pytest diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst new file mode 100644 index 00000000..0bb5a6cd --- /dev/null +++ b/canopen_utils/CHANGELOG.rst @@ -0,0 +1,51 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Include rpdo/tpdo test in launch_test. (`#98 `_) + * Implement rpdo/tpdo test + * Removed unnecessary files +* Merge branch 'master' into patch-2 +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Add lifecycle to service-based operation (`#34 `_) + * Add check if remote object already exists to avoid multiple objects with same target. + * Renaming and changes to MasterNode + * restrucutring for lifecycle support + * changes to build + * Add lifecycle to drivers, masters and add device manager + * Add lifecycled operation canopen_core + * Added non lifecycle stuff to canopen_core + * Add lifecyle to canopen_base_driver + * Add lifecycle to canopen_proxy_driver + * restructured canopen_core for lifecycle support + * restructured canopen_base_driver for lifecycle support + * Restrucutured canopen_proxy_driver for lifecycle support + * Restructured canopen_402_driver for lifecycle support + * Add canopen_mock_slave add cia402 slave + * add canopen_tests package for testing canopen stack + * Disable linting for the moment and some foxy compat changes + * Further changes for foxy compatability +* Configuration manager integration (`#14 `_) + * Add longer startup delay and test documentation + * Add speed and position publisher + * Create Configuration Manager + * make MasterNode a component and add configuration manager functionalities + * add configuration manager functionalities + * add configuration manger functionalities + * Add documentation for Configuration Manager + * add info messages and documentation + * update launch files and configuration fiels + * add can_utils package + * add info text + * simplify dependencies + * remove tests from can_utils + * avoid tests for canopen_utils + * changes info logging and adds nmt and sdo tests + * add tests + * remove launch_tests from cmake +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Vishnuprasad Prachandabhanu diff --git a/canopen_utils/LICENSE b/canopen_utils/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/canopen_utils/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/canopen_utils/canopen_utils/cyclic_tester.py b/canopen_utils/canopen_utils/cyclic_tester.py index 2f7afc2b..f5af6c65 100644 --- a/canopen_utils/canopen_utils/cyclic_tester.py +++ b/canopen_utils/canopen_utils/cyclic_tester.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor diff --git a/canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py b/canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py index 00f1a906..89574e00 100644 --- a/canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py +++ b/canopen_utils/canopen_utils/simple_rpdo_tpdo_tester.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile diff --git a/canopen_utils/canopen_utils/test_node.py b/canopen_utils/canopen_utils/test_node.py index 2b686712..221ee2b6 100644 --- a/canopen_utils/canopen_utils/test_node.py +++ b/canopen_utils/canopen_utils/test_node.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import rclpy from rclpy.node import Node from lifecycle_msgs.srv import GetState, ChangeState diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst new file mode 100644 index 00000000..57c85e7b --- /dev/null +++ b/lely_core_libraries/CHANGELOG.rst @@ -0,0 +1,84 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package lely_core_libraries +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Bump lely_core_librries to version 2.3.2 +* Improve lely compilation time +* Enable simplified bus.yml format (`#115 `_) + * Get slave eds and bin in node_canopen_driver + * Add dictionary to base driver + * Enable dictionary in proxy drivers + * Add a few test objects + * Add pdo checks + * Adjust 402 driver + * Fix tests + * rename to get_xx_queue + * Add typed sdo operations + * Remove object datatype where possible + * Add plain operation mode setting + switchingstate + * Add robot system interface + * Add robot system controller + * Add robot_system_tests + * Add a bit of documentation + * Add in code documentation + * Fix bug + * Add examples section + * Fix set_target for interpolated mode + * Switch to rclcpp::sleep_for + * Fix initialization for state and command interface variables + * Add remade robot system interfce + * Add copyright info + * Fix missing return statement + * processing behavior improvement + * Minor changes to make things work + * Add poll_timer_callback + * Fix format + * Add polling mode variable for config. + * Add cogen + * Add example usage for cogen + * Remove explicit path + --------- + Co-authored-by: Vishnuprasad Prachandabhanu +* Merge branch 'master' into patch-2 +* Merge branch 'bjsowa-master' +* Merge branch 'master' of github.com:bjsowa/ros2_canopen into bjsowa-master +* Merge remote-tracking branch 'ros/master' +* Precommit changes (`#79 `_) + * Precommit changes + * Update to clang-format-14 +* Substitute @BUS_CONFIG_PATH@ in bus configuration file +* Publish joint state instead of velocity topics (`#47 `_) + * disable loader service + * add custom target/command and install to macro + * publish jointstate + * correct variable name squiggle + * Minor changes to driver and slave + * Update lely core library + * Add sensor_msgs to dependencies + * Remove artifacts + * Remove some artifacts +* Update dcfgen cmake integration (`#41 `_) +* Merge branch 'licenses' into 'master' + add licenses to each package + See merge request ipa326/ros-industrial/ros2_canopen!22 +* update package descriptions +* add licenses to each package +* Merge branch 'renaming' into 'master' + Update package names to fit ROS2 naming rules better + See merge request ipa326/ros-industrial/ros2_canopen!21 +* store tests of proxy driver in canopen_proxy_driver +* Merge branch 'cmh/restructured_master_and_slaves' into 'master' + Complete Restructuring + See merge request ipa326/ros-industrial/ros2_canopen!15 +* make lely_core_library tools available +* Add automake etc. +* add autotools-dev to lelycore build dependencies +* change false dependencies +* Merge branch 'ros_canopen-merge' into 'master' + Merge in canopen_402 from ros_canopen + See merge request ipa326/ros-industrial/ros2_canopen!6 +* Merge in canopen_402 from ros_canopen +* lely_core_libraries wrapper tested and building +* Contributors: Błażej Sowa, Christoph Hellmann Santos diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index a67d176b..980568cb 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -7,14 +7,12 @@ include(ExternalProject) ExternalProject_Add(upstr_lely_core_libraries # Name for custom target #--Download step-------------- GIT_REPOSITORY https://gitlab.com/lely_industries/lely-core.git - GIT_TAG ac31cd5cdf2ded4b2a5ba4a94520f388450614ff + GIT_TAG 7824cbb2ac08d091c4fa2fb397669b938de9e3f5 TIMEOUT 60 #--Update/Patch step---------- - UPDATE_COMMAND touch /config.h - COMMAND mkdir -p /include /lib - COMMAND touch /python/dcf-tools/README.md + # UPDATE_COMMAND mkdir -p /include /lib #--Configure step------------- - CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static "CPPFLAGS=-DNDEBUG" "CFLAGS=-DNDEBUG" "CXXFLAGS=-DNDEBUG" + CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag #--Build step----------------- BUILD_IN_SOURCE 1 # Use source dir for build dir #--Install step--------------- diff --git a/lely_core_libraries/cogen/cogen.py b/lely_core_libraries/cogen/cogen.py index 0f170954..7cc3345a 100644 --- a/lely_core_libraries/cogen/cogen.py +++ b/lely_core_libraries/cogen/cogen.py @@ -1,3 +1,17 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import yaml import argparse from dataclasses import dataclass diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index dbe50d87..39ab6205 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.0.1 + 2.3.2 ROS wrapper for lely-core-libraries From 1ac72b1a83908ede1857513494219178316ada09 Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu <32260301+ipa-vsp@users.noreply.github.com> Date: Wed, 7 Jun 2023 09:55:25 +0200 Subject: [PATCH 03/59] Fix interpolated mode switch in CiA402 (#124) --- .../include/canopen_402_driver/cia402_driver.hpp | 5 +++++ .../include/canopen_402_driver/lifecycle_cia402_driver.hpp | 5 +++++ canopen_ros2_control/src/cia402_system.cpp | 3 ++- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp index 2eaaa875..5ad3e992 100644 --- a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp @@ -75,6 +75,11 @@ class Cia402Driver : public ros2_canopen::CanopenDriver bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); } + bool set_mode_interpolated_position() + { + return node_canopen_402_driver_->set_mode_interpolated_position(); + } + uint16_t get_mode() { return node_canopen_402_driver_->get_mode(); } bool set_operation_mode(uint16_t mode) diff --git a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp index c92912f2..e9d7f27d 100644 --- a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp @@ -76,6 +76,11 @@ class LifecycleCia402Driver : public ros2_canopen::LifecycleCanopenDriver bool set_mode_cyclic_velocity() { return node_canopen_402_driver_->set_mode_cyclic_velocity(); } bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); } + + bool set_mode_interpolated_position() + { + return node_canopen_402_driver_->set_mode_interpolated_position(); + } }; } // namespace ros2_canopen diff --git a/canopen_ros2_control/src/cia402_system.cpp b/canopen_ros2_control/src/cia402_system.cpp index de6f789e..42879656 100644 --- a/canopen_ros2_control/src/cia402_system.cpp +++ b/canopen_ros2_control/src/cia402_system.cpp @@ -355,7 +355,8 @@ void Cia402System::switchModes(uint id, const std::shared_ptrset_mode_torque()); + motor_data_[id].interpolated_position_mode.set_response( + driver->set_mode_interpolated_position()); } } From c7d38225b63040b0ffc32a723f01d67ceec69516 Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu <32260301+ipa-vsp@users.noreply.github.com> Date: Wed, 7 Jun 2023 09:55:45 +0200 Subject: [PATCH 04/59] run interpolated mode in fake slave (#126) --- .../canopen_fake_slaves/base_slave.hpp | 1 + .../canopen_fake_slaves/cia402_slave.hpp | 81 +++++++++++++++++++ 2 files changed, 82 insertions(+) diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp index 68013056..8164c0cb 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp @@ -93,6 +93,7 @@ class BaseSlave : public rclcpp_lifecycle::LifecycleNode const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "Shutdown"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } }; } // namespace ros2_canopen diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp index 61f1b5f7..4ae7bb26 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -64,6 +64,11 @@ class CIA402MockSlave : public canopen::BasicSlave RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread."); cyclic_position_mode.join(); } + if (interpolated_position_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + interpolated_position_mode.join(); + } } protected: @@ -150,6 +155,7 @@ class CIA402MockSlave : public canopen::BasicSlave std::thread profiled_velocity_mode; std::thread cyclic_position_mode; std::thread cyclic_velocity_mode; + std::thread interpolated_position_mode; double cycle_time; @@ -262,6 +268,66 @@ class CIA402MockSlave : public canopen::BasicSlave } } + void run_interpolated_position_mode() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_interpolated_position_mode"); + // Retrieve parameters from the object dictionary + double interpolation_period = static_cast((uint8_t)(*this)[0x60C2][1]); + double target_position = static_cast((int32_t)(*this)[0x60C1][1]); + + // int32_t offset = (*this)[0x60B0][0]; + + // Convert parameters to SI units + interpolation_period *= std::pow(10.0, static_cast((int8_t)(*this)[0x60C2][2])); + target_position /= 1000.0; + double actual_position = static_cast((int32_t)(*this)[0x6064][0]) / 1000.0; + + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Interpolation Period: %f", interpolation_period); + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Target position: %f", target_position); + + while ((state.load() == InternalState::Operation_Enable) && + (operation_mode.load() == Interpolated_Position) && (rclcpp::ok())) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(interpolation_period * 1000))); + + target_position = static_cast((int32_t)(*this)[0x60C1][1]) / 1000.0; + + if (target_position != actual_position) + { + double position_delta = target_position - actual_position; + double position_increment = position_delta / 20; + clear_status_bit(SW_Operation_mode_specific0); + clear_status_bit(SW_Target_reached); + { + std::scoped_lock lock(w_mutex); + (*this)[0x6041][0] = status_word; + this->TpdoEvent(1); + } + + while ((std::abs(actual_position - target_position) > 0.001) && (rclcpp::ok())) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + actual_position += position_increment; + (*this)[0x6064][0] = static_cast(actual_position * 1000); + (*this)[0x606C][0] = static_cast(position_increment * 1000); + } + + actual_position = target_position; + + RCLCPP_DEBUG(rclcpp::get_logger("cia402_slave"), "Reached target: %f", actual_position); + clear_status_bit(SW_Operation_mode_specific0); + set_status_bit(SW_Target_reached); + { + std::lock_guard lock(w_mutex); + (*this)[0x6041][0] = status_word; + this->TpdoEvent(1); + } + } + } + } + void run_position_mode() {} void set_new_status_word_and_state() @@ -446,6 +512,12 @@ class CIA402MockSlave : public canopen::BasicSlave RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread."); cyclic_position_mode.join(); } + if (interpolated_position_mode.joinable()) + { + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + interpolated_position_mode.join(); + } old_operation_mode.store(operation_mode.load()); switch (operation_mode.load()) { @@ -455,6 +527,9 @@ class CIA402MockSlave : public canopen::BasicSlave case Profiled_Position: start_profile_pos_mode(); break; + case Interpolated_Position: + start_interpolated_pos_mode(); + break; default: break; } @@ -472,6 +547,12 @@ class CIA402MockSlave : public canopen::BasicSlave std::thread(std::bind(&CIA402MockSlave::run_profiled_position_mode, this)); } + void start_interpolated_pos_mode() + { + interpolated_position_mode = + std::thread(std::bind(&CIA402MockSlave::run_interpolated_position_mode, this)); + } + void on_quickstop_active() { if (is_enable_operation()) From a4afe29e331043b2d30b6f64e29789d01eb6a544 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Thu, 8 Jun 2023 23:33:58 +0200 Subject: [PATCH 05/59] Delete robot controller (#132) Signed-off-by: Christoph Hellmann Santos --- canopen_ros2_controllers/CMakeLists.txt | 11 - .../cia402_robot_controller.hpp | 153 ----------- .../cia402_robot_controller.yaml | 14 -- .../src/cia402_robot_controller.cpp | 237 ------------------ 4 files changed, 415 deletions(-) delete mode 100644 canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp delete mode 100644 canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml delete mode 100644 canopen_ros2_controllers/src/cia402_robot_controller.cpp diff --git a/canopen_ros2_controllers/CMakeLists.txt b/canopen_ros2_controllers/CMakeLists.txt index 9de45f2f..e5879ce1 100644 --- a/canopen_ros2_controllers/CMakeLists.txt +++ b/canopen_ros2_controllers/CMakeLists.txt @@ -9,7 +9,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface controller_manager hardware_interface - generate_parameter_library pluginlib rclcpp rclcpp_lifecycle @@ -24,17 +23,11 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -generate_parameter_library( - cia402_robot_controller_parameters - include/canopen_ros2_controllers/cia402_robot_controller.yaml -) - add_library( ${PROJECT_NAME} SHARED src/canopen_proxy_controller.cpp src/cia402_device_controller.cpp - src/cia402_robot_controller.cpp ) target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) @@ -42,9 +35,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ ) -target_link_libraries(${PROJECT_NAME} PUBLIC - cia402_robot_controller_parameters -) ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${PROJECT_NAME} PRIVATE "CANOPEN_PROXY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") @@ -52,7 +42,6 @@ pluginlib_export_plugin_description_file(controller_interface canopen_ros2_contr install( TARGETS - cia402_robot_controller_parameters ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib diff --git a/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp deleted file mode 100644 index 8873acf0..00000000 --- a/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.hpp +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ -#define CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ - -#include "canopen_interfaces/srv/co_target_double.hpp" -#include "canopen_ros2_controllers/canopen_proxy_controller.hpp" -#include "cia402_robot_controller_parameters.hpp" - -namespace cia402_robot_controller -{ -/** - * @brief Controller for a robot with CiA402 drivers. - * - * This controller handles bringing up of the CiA402 drivers to operational - * state with the right operation mode set. - */ -class Cia402RobotController : public controller_interface::ControllerInterface -{ -public: - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - Cia402RobotController(); - - /** - * @brief Initialize controller - * - * @details - * This function initializes the controller. It declares the - * parameters of the controller. This is done using the - * generate_parameter_library. - * - * @return controller_interface::CallbackReturn - */ - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_init() override; - - /** - * @brief Get the command interface configuration object - * - * @details - * This function returns the command interface configuration. - * This controller has uses the following command interfaces per joint: - * - init - * - init_feedback - * - halt - * - halt_feedback - * - recover - * - recover_feedback - * - operation_mode - * - operation_mode_feedback - * - * @return controller_interface::InterfaceConfiguration - */ - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - /** - * @brief Get the state interface configuration object - * - * @details - * This function returns the state interface configuration. - * This controller has no state interface. - * - * @return CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - */ - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - /** - * @brief Configure controller - * - * @details - * This function configures the controller. It reads the - * parameters of the controller and sets up the interfaces - * used by the controller. - * - * @param previous_state - * @return controller_interface::CallbackReturn - */ - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - /** - * @brief Activate controller - * - * @details - * This function activates the controller. It brings up the - * CiA402 drivers to operational state with the right operation - * mode set. - * - * @param previous_state - * @return controller_interface::CallbackReturn - */ - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - /** - * @brief Deactivate controller - * - * - * @param previous_state - * @return controller_interface::CallbackReturn - */ - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - /** - * @brief Update controller - * - * @details - * This function updates the controller. It does nothing. - * - * @param time - * @param period - * @return controller_interface::return_type - */ - CANOPEN_ROS2_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - -protected: - rclcpp::Service::SharedPtr handle_recover_service_; - - std::shared_ptr param_listener_; - Params params_; - controller_interface::InterfaceConfiguration command_interfaces_config_; - controller_interface::InterfaceConfiguration state_interfaces_config_; - - void declare_parameters(); - controller_interface::CallbackReturn read_parameters(); - void activate(); - - rclcpp::TimerBase::SharedPtr timer_; -}; - -} // namespace cia402_robot_controller - -#endif // CANOPEN_ROS2_CONTROLLERS__CANOPEN_CIA402_CONTROLLER_HPP_ diff --git a/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml b/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml deleted file mode 100644 index accace8c..00000000 --- a/canopen_ros2_controllers/include/canopen_ros2_controllers/cia402_robot_controller.yaml +++ /dev/null @@ -1,14 +0,0 @@ -cia402_robot_controller: - joints: { - type: string_array, - default_value: [], - description: 'List of joints controlled by this controller', - } - operation_mode: { - type: int, - description: 'ID of operation mode to use', - } - command_poll_freq: { - type: int, - description: 'Frequency of polling command results', - } diff --git a/canopen_ros2_controllers/src/cia402_robot_controller.cpp b/canopen_ros2_controllers/src/cia402_robot_controller.cpp deleted file mode 100644 index d399a585..00000000 --- a/canopen_ros2_controllers/src/cia402_robot_controller.cpp +++ /dev/null @@ -1,237 +0,0 @@ -#include "canopen_ros2_controllers/cia402_robot_controller.hpp" - -namespace cia402_robot_controller -{ - -enum CommandInterface -{ - INIT, - INIT_FEEDBACK, - HALT, - HALT_FEEDBACK, - RECOVER, - RECOVER_FEEDBACK, - OPERATION_MODE, - OPERATION_MODE_FEEDBACK, - ADD_OP -}; - -Cia402RobotController::Cia402RobotController() : controller_interface::ControllerInterface() {} - -controller_interface::CallbackReturn Cia402RobotController::on_init() -{ - try - { - declare_parameters(); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -void Cia402RobotController::declare_parameters() -{ - param_listener_ = std::make_shared(get_node()); -} - -controller_interface::CallbackReturn Cia402RobotController::on_configure( - const rclcpp_lifecycle::State & previous_state) -{ - auto ret = this->read_parameters(); - if (ret != controller_interface::CallbackReturn::SUCCESS) - { - return ret; - } - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn Cia402RobotController::read_parameters() -{ - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } - - params_ = param_listener_->get_params(); - - if (params_.joints.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - if (params_.operation_mode == 0) - { - RCLCPP_ERROR( - get_node()->get_logger(), "'operation_mode' parameter was not correctly specified"); - return controller_interface::CallbackReturn::ERROR; - } - - if (params_.command_poll_freq <= 0) - { - RCLCPP_ERROR( - get_node()->get_logger(), "'command_poll_freq' parameter was not correctly specified"); - return controller_interface::CallbackReturn::ERROR; - } - - command_interfaces_config_.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - for (auto joint_name : params_.joints) - { - command_interfaces_config_.names.push_back(joint_name + "/" + "init"); - command_interfaces_config_.names.push_back(joint_name + "/" + "init_feedback"); - command_interfaces_config_.names.push_back(joint_name + "/" + "halt"); - command_interfaces_config_.names.push_back(joint_name + "/" + "halt_feedback"); - command_interfaces_config_.names.push_back(joint_name + "/" + "recover"); - command_interfaces_config_.names.push_back(joint_name + "/" + "recover_feedback"); - command_interfaces_config_.names.push_back(joint_name + "/" + "operation_mode"); - command_interfaces_config_.names.push_back(joint_name + "/" + "operation_mode_feedback"); - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration -Cia402RobotController::command_interface_configuration() const -{ - return command_interfaces_config_; -} - -controller_interface::InterfaceConfiguration Cia402RobotController::state_interface_configuration() - const -{ - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; -} - -controller_interface::CallbackReturn Cia402RobotController::on_activate( - const rclcpp_lifecycle::State & previous_state) -{ - timer_ = this->get_node()->create_wall_timer( - std::chrono::milliseconds(params_.command_poll_freq), - std::bind(&Cia402RobotController::activate, this)); - return controller_interface::CallbackReturn::SUCCESS; -} - -void Cia402RobotController::activate() -{ - int jn = 0; - for (auto joint_name : params_.joints) - { - RCLCPP_INFO(get_node()->get_logger(), "Initialise '%s'", joint_name.c_str()); - // Initialise joint - //////////////////////////////////////////////////////////////////////////////////////// - RCLCPP_INFO( - get_node()->get_logger(), "Using %s to init", - command_interfaces_[jn + INIT].get_full_name().c_str()); - command_interfaces_[jn + INIT].set_value(1.0); - while (std::isnan(command_interfaces_[jn + INIT_FEEDBACK].get_value()) && rclcpp::ok()) - { - rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); - } - if (command_interfaces_[jn + INIT_FEEDBACK].get_value() != 1.0) - { - RCLCPP_ERROR(get_node()->get_logger(), "Init of '%s' failed", joint_name.c_str()); - // return controller_interface::CallbackReturn::ERROR; - } - - command_interfaces_[jn + INIT].set_value(std::numeric_limits::quiet_NaN()); - command_interfaces_[jn + INIT_FEEDBACK].set_value(std::numeric_limits::quiet_NaN()); - // rclcpp::sleep_for(std::chrono::milliseconds(1000)); - RCLCPP_INFO( - get_node()->get_logger(), "Setting operation mode '%li' of '%s'", params_.operation_mode, - joint_name.c_str()); - // Set operation mode for joint - // Some devices may require setting operation mode before init. - //////////////////////////////////////////////////////////////////////////////////////// - - command_interfaces_[jn + OPERATION_MODE].set_value(params_.operation_mode); - while (std::isnan(command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value()) && - rclcpp::ok()) - { - rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); - } - if (command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value() != 1.0) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Setting operation mode '%li' of '%s' failed", - params_.operation_mode, joint_name.c_str()); - // return controller_interface::CallbackReturn::ERROR; - } - command_interfaces_[jn + OPERATION_MODE].set_value(std::numeric_limits::quiet_NaN()); - command_interfaces_[jn + OPERATION_MODE_FEEDBACK].set_value( - std::numeric_limits::quiet_NaN()); - jn += ADD_OP; - } - RCLCPP_INFO(get_node()->get_logger(), "activate successful"); - timer_->cancel(); -} - -controller_interface::CallbackReturn Cia402RobotController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) -{ - int jn = 0; - for (auto joint_name : params_.joints) - { - // Halt joint - //////////////////////////////////////////////////////////////////////////////////////// - command_interfaces_[jn + HALT].set_value(1.0); - while (std::isnan(command_interfaces_[jn + HALT_FEEDBACK].get_value()) && rclcpp::ok()) - { - rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); - } - if (command_interfaces_[jn + HALT_FEEDBACK].get_value() != 1.0) - { - RCLCPP_ERROR(get_node()->get_logger(), "Halting of '%s' failed", joint_name.c_str()); - return controller_interface::CallbackReturn::ERROR; - } - - command_interfaces_[jn + HALT].set_value(std::numeric_limits::quiet_NaN()); - command_interfaces_[jn + HALT_FEEDBACK].set_value(std::numeric_limits::quiet_NaN()); - - // Set operation mode for joint - // Some devices may require setting operation mode before init. - //////////////////////////////////////////////////////////////////////////////////////// - - command_interfaces_[jn + OPERATION_MODE].set_value(0.0); - while (std::isnan(command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value()) && - rclcpp::ok()) - { - rclcpp::sleep_for(std::chrono::milliseconds(params_.command_poll_freq)); - } - if (command_interfaces_[jn + OPERATION_MODE_FEEDBACK].get_value() != 1.0) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Setting operation mode '%li' of '%s' failed", - params_.operation_mode, joint_name.c_str()); - return controller_interface::CallbackReturn::ERROR; - } - command_interfaces_[jn + OPERATION_MODE].set_value(std::numeric_limits::quiet_NaN()); - command_interfaces_[jn + OPERATION_MODE_FEEDBACK].set_value( - std::numeric_limits::quiet_NaN()); - jn += ADD_OP; - } - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); - } - RCLCPP_INFO(get_node()->get_logger(), "deactivate successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type Cia402RobotController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) -{ - return controller_interface::return_type::OK; -} -} // namespace cia402_robot_controller - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - cia402_robot_controller::Cia402RobotController, controller_interface::ControllerInterface) From 019873ccd950b10e5d75102fb50c13b9eeeaf3d7 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Fri, 9 Jun 2023 00:12:03 +0200 Subject: [PATCH 06/59] Update package versions to 0.1.0 (#133) Signed-off-by: Christoph Hellmann Santos --- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/package.xml | 2 +- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/package.xml | 2 +- canopen_utils/package.xml | 2 +- lely_core_libraries/package.xml | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index 4a51d10c..bf828470 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.0.0 + 0.1.0 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 9cf58cd4..8eea7d0f 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.0.1 + 0.1.0 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index 69f425b0..c9151ae6 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.0.0 + 0.1.0 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index ca4efa0f..b7ae24cc 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.0.1 + 0.1.0 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index fcb9f890..8f695b06 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.0.1 + 0.1.0 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index 8a328c6e..d28096b9 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.0.1 + 0.1.0 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index f19b8247..69ff7e29 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.0.0 + 0.1.0 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index 42345d21..13e8badd 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.0.0 + 0.1.0 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index 39ab6205..ddf5db9d 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 2.3.2 + 0.1.0 ROS wrapper for lely-core-libraries From 0b8e29374f4c2bf501025f5396398c9b385fa118 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Fri, 9 Jun 2023 14:20:26 +0200 Subject: [PATCH 07/59] Bring ros2_control test launches to canopen_tests pkg (#131) Signed-off-by: Christoph Hellmann Santos --- canopen_ros2_control/CMakeLists.txt | 5 - canopen_tests/CMakeLists.txt | 2 + canopen_tests/config/canopen_system/bus.yml | 19 + .../canopen_system/ros2_controllers.yaml | 0 .../config/canopen_system/simple.eds | 282 ++++ canopen_tests/config/cia402_system/bus.yml | 56 + .../config/cia402_system/cia402_slave.eds | 1441 +++++++++++++++++ .../cia402_system/ros2_controllers.yaml | 0 .../launch/canopen_system.launch.py | 14 +- .../launch/cia402_system.launch.py | 14 +- .../canopen_system.ros2_control.xacro | 0 .../canopen_system}/canopen_system.urdf.xacro | 2 +- .../cia402_system.ros2_control.xacro | 0 .../cia402_system}/cia402_system.urdf.xacro | 2 +- 14 files changed, 1816 insertions(+), 21 deletions(-) create mode 100644 canopen_tests/config/canopen_system/bus.yml rename canopen_ros2_control/config/ros2_control.yaml => canopen_tests/config/canopen_system/ros2_controllers.yaml (100%) create mode 100644 canopen_tests/config/canopen_system/simple.eds create mode 100644 canopen_tests/config/cia402_system/bus.yml create mode 100644 canopen_tests/config/cia402_system/cia402_slave.eds rename canopen_ros2_control/config/cia402_ros2_control.yaml => canopen_tests/config/cia402_system/ros2_controllers.yaml (100%) rename {canopen_ros2_control => canopen_tests}/launch/canopen_system.launch.py (96%) rename {canopen_ros2_control => canopen_tests}/launch/cia402_system.launch.py (96%) rename {canopen_ros2_control/urdf => canopen_tests/urdf/canopen_system}/canopen_system.ros2_control.xacro (100%) rename {canopen_ros2_control/urdf => canopen_tests/urdf/canopen_system}/canopen_system.urdf.xacro (93%) rename {canopen_ros2_control/urdf => canopen_tests/urdf/cia402_system}/cia402_system.ros2_control.xacro (100%) rename {canopen_ros2_control/urdf => canopen_tests/urdf/cia402_system}/cia402_system.urdf.xacro (93%) diff --git a/canopen_ros2_control/CMakeLists.txt b/canopen_ros2_control/CMakeLists.txt index d0f0230e..a33e51ac 100644 --- a/canopen_ros2_control/CMakeLists.txt +++ b/canopen_ros2_control/CMakeLists.txt @@ -59,11 +59,6 @@ install( DESTINATION include ) -install( - DIRECTORY launch urdf config - DESTINATION share/${PROJECT_NAME} -) - if(BUILD_TESTING) #find_package(ament_cmake_gmock REQUIRED) #find_package(ros2_control_test_assets REQUIRED) diff --git a/canopen_tests/CMakeLists.txt b/canopen_tests/CMakeLists.txt index dd98262b..33b0720d 100644 --- a/canopen_tests/CMakeLists.txt +++ b/canopen_tests/CMakeLists.txt @@ -11,6 +11,8 @@ find_package(lely_core_libraries REQUIRED) generate_dcf(simple) +generate_dcf(canopen_system) +cogen_dcf(cia402_system) cogen_dcf(cia402) generate_dcf(cia402_lifecycle) generate_dcf(simple_lifecycle) diff --git a/canopen_tests/config/canopen_system/bus.yml b/canopen_tests/config/canopen_system/bus.yml new file mode 100644 index 00000000..4b9d35cb --- /dev/null +++ b/canopen_tests/config/canopen_system/bus.yml @@ -0,0 +1,19 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + +master: + node_id: 1 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + +proxy_device_1: + node_id: 2 + dcf: "simple.eds" + driver: "ros2_canopen::ProxyDriver" + package: "canopen_proxy_driver" + +proxy_device_2: + node_id: 3 + dcf: "simple.eds" + driver: "ros2_canopen::ProxyDriver" + package: "canopen_proxy_driver" diff --git a/canopen_ros2_control/config/ros2_control.yaml b/canopen_tests/config/canopen_system/ros2_controllers.yaml similarity index 100% rename from canopen_ros2_control/config/ros2_control.yaml rename to canopen_tests/config/canopen_system/ros2_controllers.yaml diff --git a/canopen_tests/config/canopen_system/simple.eds b/canopen_tests/config/canopen_system/simple.eds new file mode 100644 index 00000000..68d9bb2a --- /dev/null +++ b/canopen_tests/config/canopen_system/simple.eds @@ -0,0 +1,282 @@ +[DeviceInfo] +VendorName=Lely Industries N.V. +VendorNumber=0x00000360 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=1 +NrOfTxPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0011=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=11 +1=0x1003 +2=0x1005 +3=0x1014 +4=0x1015 +5=0x1016 +6=0x1017 +7=0x1029 +8=0x1400 +9=0x1600 +10=0x1800 +11=0x1A00 + +[ManufacturerObjects] +SupportedObjects=4 +1=0x4000 +2=0x4001 +3=0x4002 +4=0x4003 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw + +[1018] +SubNumber=5 +ParameterName=Identity object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000360 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=1 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x200 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x40000020 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x40010020 + +[4000] +ParameterName=UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[4001] +ParameterName=UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[4002] +ParameterName=INTEGER32 test +DataType=0x0004 +AccessType=rw + +[4003] +ParameterName=INTEGER16 test +DataType=0x0003 +AccessType=rw diff --git a/canopen_tests/config/cia402_system/bus.yml b/canopen_tests/config/cia402_system/bus.yml new file mode 100644 index 00000000..7b6f174c --- /dev/null +++ b/canopen_tests/config/cia402_system/bus.yml @@ -0,0 +1,56 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + +master: + node_id: 1 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + sync_period: 10000 + +defaults: + dcf: "cia402_slave.eds" + driver: "ros2_canopen::Cia402Driver" + package: "canopen_402_driver" + period: 10 + revision_number: 0 + sdo: + - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms + - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s + - {index: 0x6081, sub_index: 0, value: 1000} + - {index: 0x6083, sub_index: 0, value: 2000} + tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation + 1: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6041, sub_index: 0} # status word + - {index: 0x6061, sub_index: 0} # mode of operation display + 2: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6064, sub_index: 0} # position actual value + - {index: 0x606c, sub_index: 0} # velocity actual position + 3: + enabled: false + 4: + enabled: false + rpdo: # RPDO needed controlword, target position, target velocity, mode of operation + 1: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x6040, sub_index: 0} # controlword + - {index: 0x6060, sub_index: 0} # mode of operation + 2: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x607A, sub_index: 0} # target position + - {index: 0x60FF, sub_index: 0} # target velocity + +nodes: + cia402_device_1: + node_id: 2 diff --git a/canopen_tests/config/cia402_system/cia402_slave.eds b/canopen_tests/config/cia402_system/cia402_slave.eds new file mode 100644 index 00000000..24be3815 --- /dev/null +++ b/canopen_tests/config/cia402_system/cia402_slave.eds @@ -0,0 +1,1441 @@ +[DeviceInfo] +VendorName=ROS-Industrial +VendorNumber=0x555 +ProductName=CIA402VTD +BaudRate_10=0 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +LSS_Supported=0 +Granularity=8 +SimpleBootUpSlave=1 +SimpleBootUpMaster=0 +NrOfRXPDO=4 +NrOfTXPDO=4 + +[Comments] +Lines=0 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[ManufacturerObjects] +SupportedObjects=0 + +[OptionalObjects] +SupportedObjects=67 +1=0x1005 +2=0x1008 +3=0x1009 +4=0x100A +5=0x100C +6=0x100D +7=0x1010 +8=0x1011 +9=0x1014 +10=0x1015 +11=0x1016 +12=0x1017 +13=0x1023 +14=0x1029 +15=0x1400 +16=0x1401 +17=0x1402 +18=0x1403 +19=0x1600 +20=0x1601 +21=0x1602 +22=0x1603 +23=0x1800 +24=0x1801 +25=0x1802 +26=0x1803 +27=0x1A00 +28=0x1A01 +29=0x1A02 +30=0x1A03 +31=0x6040 +32=0x6041 +33=0x605A +34=0x605B +35=0x605C +36=0x605D +37=0x605E +38=0x6060 +39=0x6061 +40=0x6062 +41=0x6063 +42=0x6064 +43=0x6065 +44=0x6067 +45=0x6068 +46=0x606A +47=0x606C +48=0x607A +49=0x607C +50=0x607D +51=0x6081 +52=0x6082 +53=0x6083 +54=0x6084 +55=0x6085 +56=0x608F +57=0x6098 +58=0x6099 +59=0x609A +60=0x60B0 +61=0x60B1 +62=0x60C2 +63=0x60F2 +64=0x60FD +65=0x60FF +66=0x6502 +67=0x67FF + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +AccessType=const +DefaultValue=0xFFFF0192 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1010] +SubNumber=8 +ParameterName=Store Parameter Field +ObjectType=0x08 + +[1010sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x7 +PDOMapping=0 + +[1010sub1] +ParameterName=Save all Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub2] +ParameterName=Save Communication Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub3] +ParameterName=Save Device Profile Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub4] +ParameterName=Save Axis 0 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub5] +ParameterName=Save Axis 1 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub6] +ParameterName=Save Axis 2 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub7] +ParameterName=Save Device Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1011] +SubNumber=8 +ParameterName=Restore Default Parameters +ObjectType=0x08 + +[1011sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x7 +PDOMapping=0 + +[1011sub1] +ParameterName=Restore all Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub2] +ParameterName=Restore Communication Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub3] +ParameterName=Restore Device Profile Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub4] +ParameterName=Restore Axis 0 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub5] +ParameterName=Restore Axis 1 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub6] +ParameterName=Restore Axis 2 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1011sub7] +ParameterName=Restore Device Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1014] +ParameterName=COB-ID EMCY +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 +PDOMapping=0 + +[1015] +ParameterName=Inhibit Time Emergency +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1016] +SubNumber=2 +ParameterName=Heartbeat Consumer Entries +ObjectType=0x08 + +[1016sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x1 +PDOMapping=0 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x555 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x0 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x0 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1023] +SubNumber=4 +ParameterName=OS Command +ObjectType=0x09 + +[1023sub0] +ParameterName=NumOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x3 +PDOMapping=0 + +[1023sub1] +ParameterName=Command +ObjectType=0x07 +DataType=0x000A +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1023sub2] +ParameterName=Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1023sub3] +ParameterName=Reply +ObjectType=0x07 +DataType=0x000A +AccessType=ro +PDOMapping=0 + +[1029] +SubNumber=3 +ParameterName=Error Behaviour +ObjectType=0x08 + +[1029sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x2 +PDOMapping=0 + +[1029sub1] +ParameterName=Communication Error +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1029sub2] +ParameterName=Application Error +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[1400] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 1 +ObjectType=0x09 + +[1400sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1400sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000200 +PDOMapping=0 + +[1400sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1401] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 2 +ObjectType=0x09 + +[1401sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1401sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000300 +PDOMapping=0 + +[1401sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1402] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 3 +ObjectType=0x09 + +[1402sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1402sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000400 +PDOMapping=0 + +[1402sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1403] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 4 +ObjectType=0x09 + +[1403sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1403sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000500 +PDOMapping=0 + +[1403sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFE +PDOMapping=0 + +[1600] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 1 +ObjectType=0x09 + +[1600sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1600sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1600sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 2 +ObjectType=0x09 + +[1601sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1601sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1601sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60600008 +PDOMapping=0 + +[1601sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 3 +ObjectType=0x09 + +[1602sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1602sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1602sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x607A0020 +PDOMapping=0 + +[1602sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 4 +ObjectType=0x09 + +[1603sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1603sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1603sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60FF0020 +PDOMapping=0 + +[1603sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1800] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 1 +ObjectType=0x09 + +[1800sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000180 +PDOMapping=0 + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1801] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 2 +ObjectType=0x09 + +[1801sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000280 +PDOMapping=0 + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1801sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1802] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 3 +ObjectType=0x09 + +[1802sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1802sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000380 +PDOMapping=0 + +[1802sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[1802sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1802sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1802sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1803] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 4 +ObjectType=0x09 + +[1803sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1803sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000480 +PDOMapping=0 + +[1803sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFE +PDOMapping=0 + +[1803sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1803sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1803sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1A00] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 1 +ObjectType=0x09 + +[1A00sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1A00sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A00sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 2 +ObjectType=0x09 + +[1A01sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A01sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A01sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60610008 +PDOMapping=0 + +[1A01sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 3 +ObjectType=0x09 + +[1A02sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A02sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A02sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60640020 +PDOMapping=0 + +[1A02sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 4 +ObjectType=0x09 + +[1A03sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A03sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A03sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x606C0020 +PDOMapping=0 + +[1A03sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[6040] +ParameterName=Controlword 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[6041] +ParameterName=Statusword 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=ro +PDOMapping=1 + +[605A] +ParameterName=Quick Stop Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +DefaultValue=0x0002 +PDOMapping=0 + +[605B] +ParameterName=Shutdown Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 + +[605C] +ParameterName=Disable Operation Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0001 +PDOMapping=0 + +[605D] +ParameterName=Halt Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +DefaultValue=0x0001 +PDOMapping=0 + +[605E] +ParameterName=Fault Reaction Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0002 +PDOMapping=0 + +[6060] +ParameterName=Modes of Operation 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=rww +DefaultValue=0x00 +PDOMapping=1 + +[6061] +ParameterName=Modes of Operation Display 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=ro +PDOMapping=1 + +[6062] +ParameterName=Position Demand Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6063] +ParameterName=Position Actual Internal Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6064] +ParameterName=Position Actual Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6065] +ParameterName=Following Error Window 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6067] +ParameterName=Position Window 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0xFFFFFFFF +PDOMapping=0 + +[6068] +ParameterName=Position Window Time 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[606A] +ParameterName=Sensor Selection Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +PDOMapping=0 + +[606C] +ParameterName=Velocity Actual Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[607A] +ParameterName=Target Position 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rww +DefaultValue=0x0 +PDOMapping=1 + +[607C] +ParameterName=Home Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[607D] +SubNumber=3 +ParameterName=Software Position Limit 1 +ObjectType=0x08 + +[607Dsub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[607Dsub1] +ParameterName=Min Software Position Limit +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=-2147483648 +PDOMapping=0 + +[607Dsub2] +ParameterName=Max Software Position Limit +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=2147483647 +PDOMapping=0 + +[6081] +ParameterName=Profile Velocity in pp-mode 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6082] +ParameterName=End velocity 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[6083] +ParameterName=Profile Acceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6084] +ParameterName=Profile Deceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6085] +ParameterName=Quick Stop Deceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[608F] +SubNumber=3 +ParameterName=Position Encoder Resolution 1 +ObjectType=0x08 + +[608Fsub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x00000002 +PDOMapping=0 + +[608Fsub1] +ParameterName=Encoder Increments +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[608Fsub2] +ParameterName=Motor Revolutions +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000001 +PDOMapping=0 + +[6098] +ParameterName=Homing Method 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[6099] +SubNumber=3 +ParameterName=Homing Speeds 1 +ObjectType=0x08 + +[6099sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[6099sub1] +ParameterName=Fast Homing Speed +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6099sub2] +ParameterName=Slow Homing Speed +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[609A] +ParameterName=Homing Acceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[60B0] +ParameterName=Position Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[60B1] +ParameterName=Velocity Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[60C2] +SubNumber=3 +ParameterName=Interpolation Time Period 1 +ObjectType=0x09 + +[60C2sub0] +ParameterName=NumOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[60C2sub1] +ParameterName=timeUnits +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[60C2sub2] +ParameterName=timeIndex +ObjectType=0x07 +DataType=0x0002 +AccessType=rw +DefaultValue=-2 +PDOMapping=0 + +[60F2] +ParameterName=Positioning Option Code 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[60FD] +ParameterName=Digital Inputs 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=1 + +[60FF] +ParameterName=Target Velocity 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=1 + +[6502] +ParameterName=Supported Drive Modes 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x000000A5 +PDOMapping=0 + +[67FF] +ParameterName=Single Device Type 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00040192 +PDOMapping=0 diff --git a/canopen_ros2_control/config/cia402_ros2_control.yaml b/canopen_tests/config/cia402_system/ros2_controllers.yaml similarity index 100% rename from canopen_ros2_control/config/cia402_ros2_control.yaml rename to canopen_tests/config/cia402_system/ros2_controllers.yaml diff --git a/canopen_ros2_control/launch/canopen_system.launch.py b/canopen_tests/launch/canopen_system.launch.py similarity index 96% rename from canopen_ros2_control/launch/canopen_system.launch.py rename to canopen_tests/launch/canopen_system.launch.py index 7a0eabd7..ef63aefc 100644 --- a/canopen_ros2_control/launch/canopen_system.launch.py +++ b/canopen_tests/launch/canopen_system.launch.py @@ -76,7 +76,7 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] + [FindPackageShare(description_package), "urdf", "canopen_system", description_file] ), " ", "name:=", @@ -192,7 +192,7 @@ def generate_launch_description(): DeclareLaunchArgument( "description_package", description="Package where urdf file is stored.", - default_value="canopen_ros2_control", + default_value="canopen_tests", ) ) declared_arguments.append( @@ -205,21 +205,21 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_package", - default_value="canopen_ros2_control", + default_value="canopen_tests", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_directory", - default_value="config", + default_value="config/canopen_system", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_file", - default_value="ros2_control.yaml", + default_value="ros2_controllers.yaml", description="Path to ros2_control configuration.", ) ) @@ -233,7 +233,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "bus_config_directory", - default_value="config/simple", + default_value="config/canopen_system", description="Path to bus configuration.", ) ) @@ -254,7 +254,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "master_config_directory", - default_value="config/simple", + default_value="config/canopen_system", description="Path to master configuration file (*.dcf)", ) ) diff --git a/canopen_ros2_control/launch/cia402_system.launch.py b/canopen_tests/launch/cia402_system.launch.py similarity index 96% rename from canopen_ros2_control/launch/cia402_system.launch.py rename to canopen_tests/launch/cia402_system.launch.py index edb7d9c0..2a84159c 100644 --- a/canopen_ros2_control/launch/cia402_system.launch.py +++ b/canopen_tests/launch/cia402_system.launch.py @@ -76,7 +76,7 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] + [FindPackageShare(description_package), "urdf", "cia402_system", description_file] ), " ", "name:=", @@ -189,7 +189,7 @@ def generate_launch_description(): DeclareLaunchArgument( "description_package", description="Package where urdf file is stored.", - default_value="canopen_ros2_control", + default_value="canopen_tests", ) ) declared_arguments.append( @@ -202,21 +202,21 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_package", - default_value="canopen_ros2_control", + default_value="canopen_tests", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_directory", - default_value="config", + default_value="config/cia402_system", description="Path to ros2_control configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( "ros2_control_config_file", - default_value="cia402_ros2_control.yaml", + default_value="ros2_controllers.yaml", description="Path to ros2_control configuration.", ) ) @@ -230,7 +230,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "bus_config_directory", - default_value="config/cia402", + default_value="config/cia402_system", description="Path to bus configuration.", ) ) @@ -251,7 +251,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "master_config_directory", - default_value="config/cia402", + default_value="config/cia402_system", description="Path to master configuration file (*.dcf)", ) ) diff --git a/canopen_ros2_control/urdf/canopen_system.ros2_control.xacro b/canopen_tests/urdf/canopen_system/canopen_system.ros2_control.xacro similarity index 100% rename from canopen_ros2_control/urdf/canopen_system.ros2_control.xacro rename to canopen_tests/urdf/canopen_system/canopen_system.ros2_control.xacro diff --git a/canopen_ros2_control/urdf/canopen_system.urdf.xacro b/canopen_tests/urdf/canopen_system/canopen_system.urdf.xacro similarity index 93% rename from canopen_ros2_control/urdf/canopen_system.urdf.xacro rename to canopen_tests/urdf/canopen_system/canopen_system.urdf.xacro index 1d3291f6..0b2fc468 100644 --- a/canopen_ros2_control/urdf/canopen_system.urdf.xacro +++ b/canopen_tests/urdf/canopen_system/canopen_system.urdf.xacro @@ -12,7 +12,7 @@ - + - + Date: Fri, 9 Jun 2023 16:27:55 +0200 Subject: [PATCH 08/59] Fix integration tests (#136) Signed-off-by: Christoph Hellmann Santos --- .../launch_tests/test_proxy_driver.py | 48 ++++++-------- .../test_proxy_lifecycle_driver.py | 64 ++++++++----------- canopen_utils/canopen_utils/test_node.py | 13 ++-- 3 files changed, 53 insertions(+), 72 deletions(-) diff --git a/canopen_tests/launch_tests/test_proxy_driver.py b/canopen_tests/launch_tests/test_proxy_driver.py index dadaf17d..8b3f5165 100644 --- a/canopen_tests/launch_tests/test_proxy_driver.py +++ b/canopen_tests/launch_tests/test_proxy_driver.py @@ -81,37 +81,29 @@ def test_02_nmt(self): sleep(1.0) def test_03_sdo_read(self): - assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, type=32, data=0) - assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, type=32, data=0) + assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, data=0) + assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, data=0) def test_04_sdo_write(self): - assert self.node.checkSDOWrite( - "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 - ) - assert self.node.checkSDOWrite( - "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 - ) - assert self.node.checkSDORead( - "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 - ) - assert self.node.checkSDORead( - "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 - ) + assert self.node.checkSDOWrite("proxy_device_1", index=0x4000, subindex=0, data=100) + assert self.node.checkSDOWrite("proxy_device_2", index=0x4000, subindex=0, data=100) + assert self.node.checkSDORead("proxy_device_1", index=0x4000, subindex=0, data=100) + assert self.node.checkSDORead("proxy_device_2", index=0x4000, subindex=0, data=100) def test_05_sdo_read_id(self): - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=100) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=32, data=100) + assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=100) + assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=100) def test_06_sdo_write_id(self): - assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=32, data=999) - assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=32, data=999) - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=999) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=32, data=999) - - def test_07_rpdo_tpdo(self): - assert self.node.checkRpdoTpdo( - "proxy_device_1", index=0x4000, subindex=0, type=32, data=101 - ) - assert self.node.checkRpdoTpdo( - "proxy_device_2", index=0x4000, subindex=0, type=32, data=202 - ) + assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) + assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) + assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) + assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) + + # def test_07_rpdo_tpdo(self): + # assert self.node.checkRpdoTpdo( + # "proxy_device_1", index=0x4000, subindex=0, data=101 + # ) + # assert self.node.checkRpdoTpdo( + # "proxy_device_2", index=0x4000, subindex=0, data=202 + # ) diff --git a/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py index b4062045..9fb4f3e9 100644 --- a/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py +++ b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py @@ -89,45 +89,37 @@ def test_02_nmt(self): sleep(1.0) def test_03_sdo_read(self): - assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, type=32, data=0) - assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, type=32, data=0) + assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, data=0) + assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, data=0) def test_04_sdo_write(self): - assert self.node.checkSDOWrite( - "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 - ) - assert self.node.checkSDOWrite( - "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 - ) - assert self.node.checkSDORead( - "proxy_device_1", index=0x4000, subindex=0, type=32, data=100 - ) - assert self.node.checkSDORead( - "proxy_device_2", index=0x4000, subindex=0, type=32, data=100 - ) + assert self.node.checkSDOWrite("proxy_device_1", index=0x4000, subindex=0, data=100) + assert self.node.checkSDOWrite("proxy_device_2", index=0x4000, subindex=0, data=100) + assert self.node.checkSDORead("proxy_device_1", index=0x4000, subindex=0, data=100) + assert self.node.checkSDORead("proxy_device_2", index=0x4000, subindex=0, data=100) def test_05_sdo_read_id(self): - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=100) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=32, data=100) + assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=100) + assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=100) def test_06_sdo_write_id(self): - assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=32, data=999) - assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=32, data=999) - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=32, data=999) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=32, data=999) - - def test_07_rpdo_tpdo(self): - assert self.node.checkRpdoTpdo( - "proxy_device_1", index=0x4000, subindex=0, type=32, data=101 - ) - assert self.node.checkRpdoTpdo( - "proxy_device_2", index=0x4000, subindex=0, type=32, data=202 - ) - - def test_08_to_unconfigured(self): - assert self.node.checkTransition( - "lifecycle_manager", State.PRIMARY_STATE_ACTIVE, Transition.TRANSITION_DEACTIVATE - ), "Could not configure device manager" - assert self.node.checkTransition( - "lifecycle_manager", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_CLEANUP - ), "Could not configure device manager" + assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) + assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) + assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) + assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) + + # def test_07_rpdo_tpdo(self): + # assert self.node.checkRpdoTpdo( + # "proxy_device_1", index=0x4000, subindex=0, data=101 + # ) + # assert self.node.checkRpdoTpdo( + # "proxy_device_2", index=0x4000, subindex=0, data=202 + # ) + + # def test_08_to_unconfigured(self): + # assert self.node.checkTransition( + # "lifecycle_manager", State.PRIMARY_STATE_ACTIVE, Transition.TRANSITION_DEACTIVATE + # ), "Could not configure device manager" + # assert self.node.checkTransition( + # "lifecycle_manager", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_CLEANUP + # ), "Could not configure device manager" diff --git a/canopen_utils/canopen_utils/test_node.py b/canopen_utils/canopen_utils/test_node.py index 221ee2b6..39030efe 100644 --- a/canopen_utils/canopen_utils/test_node.py +++ b/canopen_utils/canopen_utils/test_node.py @@ -63,21 +63,20 @@ def checkTrigger(self, node_name, service_name) -> bool: return True return False - def checkSDORead(self, node_name, index: int, subindex: int, type: int, data: int) -> bool: + def checkSDORead(self, node_name, index: int, subindex: int, data: int) -> bool: client = self.create_client(CORead, "/" + node_name + "/sdo_read") if not client.wait_for_service(timeout_sec=3.0): return False req = CORead.Request() req.index = index req.subindex = subindex - req.type = type result = client.call(req) client.destroy() if result.success and (data == result.data): return True return False - def checkSDOWrite(self, node_name, index: int, subindex: int, type: int, data: int) -> bool: + def checkSDOWrite(self, node_name, index: int, subindex: int, data: int) -> bool: client = self.create_client(COWrite, "/" + node_name + "/sdo_write") if not client.wait_for_service(timeout_sec=3.0): return False @@ -85,7 +84,6 @@ def checkSDOWrite(self, node_name, index: int, subindex: int, type: int, data: i req.index = index req.subindex = subindex req.data = data - req.type = type result = client.call(req) client.destroy() if result.success: @@ -101,7 +99,7 @@ def checkSDOReadID( req = COReadID.Request() req.index = index req.subindex = subindex - req.type = type + req.canopen_datatype = type req.nodeid = node_id result = client.call(req) client.destroy() @@ -119,7 +117,7 @@ def checkSDOWriteID( req.index = index req.subindex = subindex req.data = data - req.type = type + req.canopen_datatype = type req.nodeid = node_id result = client.call(req) client.destroy() @@ -127,7 +125,7 @@ def checkSDOWriteID( return True return False - def checkRpdoTpdo(self, node_name, index: int, subindex: int, type: int, data: int) -> bool: + def checkRpdoTpdo(self, node_name, index: int, subindex: int, data: int) -> bool: publisher = self.create_publisher(COData, "/" + node_name + "/tpdo", 10) subscriber = self.create_subscription( COData, "/" + node_name + "/rpdo", self.rpdo_callback, 10 @@ -135,7 +133,6 @@ def checkRpdoTpdo(self, node_name, index: int, subindex: int, type: int, data: i target = COData() target.index = index target.subindex = subindex - target.type = type target.data = data publisher.publish(target) print("Published tpdo to topic: " + "/" + node_name + "/tpdo") From a68f9eeea9dc459919afad9929f6d70745ef94d9 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 14 Jun 2023 14:49:45 +0200 Subject: [PATCH 09/59] Documentation for beta release (#127) * Add documenation for cia301 * Update ros2-control-interface.rst Fix some format problems... Still have issue showing the example code * Update ros2-control-interface.rst Make the code rending okay.. Still have issues with the code blocks * Update ros2-control-interface.rst Minor changes * Fix the format so that it builds * Update ros2-control-interface.rst * Update ros2-control-interface.rst * Update configuration section * Update configuration in documentation * Update structure Signed-off-by: Christoph Hellmann Santos * Application demos: trinamic stepper motor * fix formatting and typo * Integrate the documentation from the master branch * Update ros2-control-interface.rst * Application demos: PRBT robot with ros2_canopen * Rename and add to index Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Xi Huang Co-authored-by: Vishnuprasad Prachandabhanu --- canopen/sphinx/application/prbt_robot.rst | 463 ++++++++++++++++++ canopen/sphinx/application/trinamic.rst | 201 ++++++++ canopen/sphinx/images/prbt_rviz2.png | Bin 0 -> 313696 bytes canopen/sphinx/index.rst | 18 +- canopen/sphinx/quickstart/examples.rst | 4 +- canopen/sphinx/quickstart/operation.rst | 14 +- canopen/sphinx/user-guide/cia402-driver.rst | 27 + .../{configuration => }/configuration.rst | 93 +++- .../how-to-create-a-cia301-system.rst | 136 +++++ .../how-to-create-a-configuration.rst} | 50 +- .../how-to-create-a-robot-system.rst | 225 +++++++++ canopen/sphinx/user-guide/master.rst | 11 +- canopen/sphinx/user-guide/operation.rst | 48 ++ .../sphinx/user-guide/operation/operation.rst | 32 -- .../operation/ros2-control-interface.rst | 4 - canopen/sphinx/user-guide/proxy-driver.rst | 7 + 16 files changed, 1237 insertions(+), 96 deletions(-) create mode 100644 canopen/sphinx/application/prbt_robot.rst create mode 100644 canopen/sphinx/application/trinamic.rst create mode 100644 canopen/sphinx/images/prbt_rviz2.png rename canopen/sphinx/user-guide/{configuration => }/configuration.rst (70%) create mode 100644 canopen/sphinx/user-guide/how-to-create-a-cia301-system.rst rename canopen/sphinx/{quickstart/configuration.rst => user-guide/how-to-create-a-configuration.rst} (86%) create mode 100644 canopen/sphinx/user-guide/how-to-create-a-robot-system.rst create mode 100644 canopen/sphinx/user-guide/operation.rst delete mode 100644 canopen/sphinx/user-guide/operation/operation.rst diff --git a/canopen/sphinx/application/prbt_robot.rst b/canopen/sphinx/application/prbt_robot.rst new file mode 100644 index 00000000..11e3e5ce --- /dev/null +++ b/canopen/sphinx/application/prbt_robot.rst @@ -0,0 +1,463 @@ +Pilz manipulator PRBT 6 +======================= + +This guide provides instructions on how to set up Pilz manipulators PRBT 6 using ros2_canopen. +Before following this guide, make sure to refer to the user guide to :doc:`../user-guide/configuration`, +:doc:`../user-guide/how-to-create-a-configuration` and :doc:`../user-guide/how-to-create-a-robot-system`. +Additionally, this guide is based on: `pilz_robots `_. + +To configure the PRBT 6 using ros2_canopen, please refer to the pre-existing configuration in the `pilz_robot `_ package. + + +Package creation and setup +---------------------------- +- Create a new configuration package for the robot with the name ``prbt_robot_support``. + + .. code-block:: console + + $ ros2 pkg create --dependencies ros2_canopen lely_core_libraries --build-type ament_cmake prbt_robot_support + $ cd prbt_robot_support + $ rm -rf src + $ rm -rf include + $ mkdir -p launch + $ mkdir -p config + $ mkdir -p urdf + $ touch config/prbt_ros2_control.yaml + +- Create a new bus configuration folder with name ``prbt``. And create ``bus.yml`` and copy ``prbt_0_1.dcf`` from `pilz_support `_ package. + + Now the package structure should look like this: + + :: + + prbt_robot_support + ├── config + │ ├── prbt + │ | ├── bus.yml + │ | └── prbt_0_1.dcf + │ └── prbt_ros2_control.yaml + ├── launch + ├── urdf + ├── CMakeLists.txt + └── package.xml + +- Add the following to the ``bus.yml`` + + .. code-block:: yaml + + options: + dcf_path: "@BUS_CONFIG_PATH@" + master: + node_id: 1 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + sync_period: 10000 + + defaults: + dcf: "prbt_0_1.dcf" + driver: "ros2_canopen::Cia402Driver" + package: "canopen_402_driver" + period: 10 + heartbeat_producer: 1000 + switching_state: 2 + position_mode: 7 + scale_pos_from_dev: 0.00001745329252 + scale_pos_to_dev: 57295.7795131 + sdo: + - {index: 0x6081, sub_index: 0, value: 1000} + - {index: 0x60C2, sub_index: 1, value: 10} # Interpolation time period at 10 ms matches the period. + + tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation + 1: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6041, sub_index: 0} # status word + - {index: 0x6061, sub_index: 0} # mode of operation display + 2: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6064, sub_index: 0} # position actual value + - {index: 0x606c, sub_index: 0} # velocity actual position + 3: + enabled: false + 4: + enabled: false + rpdo: # RPDO needed controlword, target position, target velocity, mode of operation + 1: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x6040, sub_index: 0} # controlword + - {index: 0x6060, sub_index: 0} # mode of operation + - {index: 0x60C1, sub_index: 1} # interpolated position data + 2: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x607A, sub_index: 0} # target position + + nodes: + prbt_joint_1: + node_id: 3 + prbt_joint_2: + node_id: 4 + prbt_joint_3: + node_id: 5 + prbt_joint_4: + node_id: 6 + prbt_joint_5: + node_id: 7 + prbt_joint_6: + node_id: 8 + +- Create ``prbt.ros2_control.xacro`` file in ``urdf`` folder and add the following to the file: + + .. code-block:: xml + + + + + + + + + canopen_ros2_control/RobotSystem + ${bus_config} + ${master_config} + ${can_interface_name} + "${master_bin}" + + + prbt_joint_1 + + + prbt_joint_2 + + + prbt_joint_3 + + + prbt_joint_4 + + + prbt_joint_5 + + + prbt_joint_6 + + + + + + + +- Add the following to the ``prbt_ros2_control.yaml`` file: + + .. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + arm_controller: + ros__parameters: + joints: + - prbt_joint_1 + - prbt_joint_2 + - prbt_joint_3 + - prbt_joint_4 + - prbt_joint_5 + - prbt_joint_6 + command_interfaces: + - position + state_interfaces: + - position + - velocity + stop_trajectory_duration: 0.2 + state_publish_rate: 100.0 + action_monitor_rate: 25.0 + goal_time: 0.0 + limits: + prbt_joint_1: + has_acceleration_limits: true + max_acceleration: 6.0 + prbt_joint_2: + has_acceleration_limits: true + max_acceleration: 6.0 + prbt_joint_3: + has_acceleration_limits: true + max_acceleration: 6.0 + prbt_joint_4: + has_acceleration_limits: true + max_acceleration: 6.0 + prbt_joint_5: + has_acceleration_limits: true + max_acceleration: 6.0 + prbt_joint_6: + has_acceleration_limits: true + max_acceleration: 6.0 + +- Create a launch file with name ``robot.launch.py`` in the launch directory of your package. Follow the instructions :doc:`../user-guide/how-to-create-a-robot-system` to complete the launch file. The launch file should look like this: + + .. code-block:: python + + import os + from ament_index_python import get_package_share_directory + from launch import LaunchDescription + from launch.actions import DeclareLaunchArgument + from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + from launch_ros.actions import Node + from launch_ros.substitutions import FindPackageShare + from launch.actions import IncludeLaunchDescription + from launch.launch_description_sources import PythonLaunchDescriptionSource + + from launch.actions import IncludeLaunchDescription + from launch.launch_description_sources import PythonLaunchDescriptionSource + import launch_ros + + def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + description="Package where urdf file is stored.", + default_value="prbt_robot_support" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "can_interface_name", + default_value="vcan0", + description="Interface name for can", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_ros2_control", + default_value="true", + description="Use ros2_control", + ) + ) + + controller_config = PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "config", "prbt_ros2_control.yaml"]) + bus_config = PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "config", "prbt", "bus.yml"]) + master_config = PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "config", "prbt", "master.dcf"]) + can_interface_name = LaunchConfiguration("can_interface_name") + + master_bin_path = os.path.join( + get_package_share_directory("prbt_robot_support"), + "config", + "prbt", + "master.bin", + ) + if not os.path.exists(master_bin_path): + master_bin_path = "" + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([FindPackageShare(LaunchConfiguration("description_package")), "urdf", "prbt.xacro"]), + " ", + "bus_config:=", + bus_config, + " ", + "master_config:=", + master_config, + " ", + "master_bin:=", + master_bin_path, + " ", + "can_interface_name:=", + can_interface_name, + ] + ) + robot_description = {"robot_description": launch_ros.descriptions.ParameterValue(robot_description_content, value_type=str)} + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # Controller manager + controller_manager_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[robot_description, controller_config], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["arm_controller", "--controller-manager", "/controller_manager"], + ) + + nodes_list = [ + robot_state_publisher_node, + controller_manager_node, + joint_state_broadcaster_spawner, + arm_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes_list) + +- Edit the CMakeLists.txt file of your package and add the following lines after the find_package section. + + .. code-block:: cmake + + cogen_dcf(prbt) + + install(DIRECTORY + launch urdf meshes + DESTINATION share/${PROJECT_NAME}) + + install(FILES config/prbt_ros2_control.yaml + DESTINATION share/${PROJECT_NAME}/config) + +- Build your package and source the setup file. + +MoveIt2 setup +------------- + +Follow the `MoveIt Setup Assistance `_ tutorial +and create the MoveIt2 package for your robot. The MoveIt2 package should be created in the same workspace as your robot package. + +- Update ``moveit_controller.yaml`` file in the config directory of your MoveIt2 package. The file should look like this: + + .. code-block:: yaml + + # MoveIt uses this configuration for controller management + trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + + moveit_simple_controller_manager: + controller_names: + - arm_controller + + arm_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: True + joints: + - prbt_joint_1 + - prbt_joint_2 + - prbt_joint_3 + - prbt_joint_4 + - prbt_joint_5 + - prbt_joint_6 + +- Create a launch file ``moveit_planning_execution.launch.py`` to launch moveit and robot components. + + .. code-block:: python + + from launch import LaunchDescription + from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription + from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + from launch_ros.substitutions import FindPackageShare + from launch.actions import IncludeLaunchDescription + from launch.launch_description_sources import PythonLaunchDescriptionSource + + from launch.actions import TimerAction + + from moveit_configs_utils import MoveItConfigsBuilder + + + def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + "prbt", package_name="prbt_robot_moveit_config" + ).to_moveit_configs() + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "can_interface_name", + default_value="can0", + description="Interface name for can", + ) + ) + + robot_hw_node = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare("prbt_robot_support"), "launch", "robot.launch.py"])], + ), + launch_arguments={ + "can_interface_name": LaunchConfiguration("can_interface_name"), + }.items(), + ) + + virtual_joints = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str( + moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" + ) + ), + ) + + move_group = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/move_group.launch.py") + ), + ) + + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/moveit_rviz.launch.py") + ), + ) + + + node_list = [ + robot_hw_node, + virtual_joints, + move_group, + rviz, + ] + + return LaunchDescription(declared_arguments + node_list) + +Running the PRBT robot +------------------------ +There are two ways to run the demo. First you can use ``canopen_fake_slave`` to simulate the robot using CAN interface ``vcan0``. Second you can use the real robot. +Refer :doc:`../quickstart/operation` to configure CAN interface. + +.. code-block:: bash + + # Run the demo using fake robot + ros2 launch prbt_robot_moveit_config moveit_planning_execution.launch.py can_interface_name:=vcan0 + + # Run the demo using real robot + ros2 launch prbt_robot_moveit_config moveit_planning_execution.launch.py can_interface_name:=can0 + +Rviz2: + +.. image:: ../images/prbt_rviz2.png + :width: 90% + :align: center diff --git a/canopen/sphinx/application/trinamic.rst b/canopen/sphinx/application/trinamic.rst new file mode 100644 index 00000000..a4330c93 --- /dev/null +++ b/canopen/sphinx/application/trinamic.rst @@ -0,0 +1,201 @@ +Trinamic Stepper Motor control +============================== + +Introduction +------------ + +This tutorial is a simple example of how to use the ros2_canopen package to control a `Trinamic smart stepper motor `_. + +Getting started +--------------- + +If you haven't already done so, follow the steps in the :doc:`../user-guide/configuration`. + +Configuration +------------- + +- Create a new folder in the ``config`` folder of your configuration package. Name it ``single-pd42``. +- Download ``.eds`` file from `Trinamic `_ and place ``TMCM-1270.eds`` in the ``single-pd42`` folder. +- Create a ``bus.yml`` file in the ``single-pd42`` folder with the following content: + + .. code-block:: yaml + + options: + dcf_path: "@BUS_CONFIG_PATH@" + + master: + node_id: 2 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + sync_period: 20000 + + + defaults: + dcf: "TMCM-1270.eds" + driver: "ros2_canopen::Cia402Driver" + package: "canopen_402_driver" + polling: false + heartbeat_producer: 1000 # Heartbeat every 1000 ms + sdo: # SDO executed during config + - {index: 0x6081, sub_index: 0, value: 500000} # Set velocity + - {index: 0x6083, sub_index: 0, value: 1000000} # Set acceleration + - {index: 0x6083, sub_index: 0, value: 1000000} # Set deceleration + - {index: 0x6085, sub_index: 0, value: 1000000} # Set quickstop deceleration + - {index: 0x6098, sub_index: 0, value: 35} # Set default homing mode to 35 + - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms + - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s + + nodes: + trinamic_pd42: + node_id: 1 + + +- Edit the ``CMakeLists.txt`` file in the ``config`` folder of your configuration package and add the following lines: + + .. code-block:: cmake + + cmake_minimum_required(VERSION 3.8) + project(trinamic_pd42_can) + + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + endif() + + # find dependencies + find_package(ament_cmake REQUIRED) + find_package(rclcpp REQUIRED) + find_package(std_srvs REQUIRED) + find_package(canopen REQUIRED) + find_package(lely_core_libraries REQUIRED) + find_package(canopen_interfaces REQUIRED) + + # generate_dcf(single-pd42) + cogen_dcf(single-pd42) + + add_executable(position_tick_client src/position_tick_motor.cpp) + ament_target_dependencies(position_tick_client + rclcpp std_srvs canopen_interfaces) + + + install(TARGETS + position_tick_client + DESTINATION lib/${PROJECT_NAME}) + + # install launch file + install(DIRECTORY + launch/ + DESTINATION share/${PROJECT_NAME} + ) + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + endif() + + ament_package() + +- Create launch file in folder ``launch`` and add the following content: + + .. code-block:: python + + import os + import sys + + import launch + from launch.actions import IncludeLaunchDescription + from launch.launch_description_sources import PythonLaunchDescriptionSource + from ament_index_python import get_package_share_directory + from launch import LaunchDescription + + + def generate_launch_description(): + ld = LaunchDescription() + slave_eds_path = os.path.join( + get_package_share_directory("trinamic_pd42_can"), "config", "single-pd42", "TMCM-1270.eds" + ) + + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/cia402_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "1", + "node_name": "pd42_slave", + "slave_config": slave_eds_path, + }.items(), + ) + master_bin_path = os.path.join( + get_package_share_directory("trinamic_pd42_can"), + "config", + "single-pd42", + "master.bin", + ) + if not os.path.exists(master_bin_path): + master_bin_path = "" + + device_container = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_core"), "launch"), + "/canopen.launch.py", + ] + ), + launch_arguments={ + "master_config": os.path.join( + get_package_share_directory("trinamic_pd42_can"), + "config", + "single-pd42", + "master.dcf", + ), + "master_bin": master_bin_path, + "bus_config": os.path.join( + get_package_share_directory("trinamic_pd42_can"), + "config", + "single-pd42", + "bus.yml", + ), + "can_interface_name": "vcan0", + }.items(), + ) + + ld.add_action(device_container) + ld.add_action(slave_node_1) + + return ld + +Running the example +------------------- + +To begin, follow the instructions for :doc:`../quickstart/operation`, which can be done using either a virtual or peak CAN interface. + +If you prefer to use a real CAN interface, you will need to modify the launch file by changing the ``can_interface_name`` argument to ``can0``. +Additionally, if you are using real hardware, you should comment out the fake slave launch by adding a *#* in front of the line *ld.add_action(slave_node_1)*. +Once these changes have been made, you can launch the example. + +.. code-block:: console + + ros2 launch trinamic_pd42_can .launch.py + +Initilaize the motor by calling the service ``/trinamic_pd42/init``: + +.. code-block:: console + + ros2 service call /trinamic_pd42/init std_srvs/srv/Trigger + +Set the operation mode to ``Profile Position Mode`` by calling the service ``/trinamic_pd42/position_mode``: + +.. code-block:: console + + ros2 service call /trinamic_pd42/position_mode std_srvs/srv/Trigger + +Set the target to the motor by calling the service ``/trinamic_pd42/target``: + +.. code-block:: console + + ros2 service call /trinamic_pd42/target canopen_interfaces/srv/COTargetDouble "{ target: 10.0 }" + +Reference +--------- +You can find the source code for this example in the `trinamic_pd42_can `_ package. diff --git a/canopen/sphinx/images/prbt_rviz2.png b/canopen/sphinx/images/prbt_rviz2.png new file mode 100644 index 0000000000000000000000000000000000000000..a4c99c371559e7dffa7bad4b047c1d4e233ef873 GIT binary patch literal 313696 zcma&Oby!@HPEfg(KT#L5F39hF&#frN_a3_JFh2lqxijfGB%j)a7SB`YJLf`o+1g@p7p1noKE3h3!I z2=VdMMNC!=4Iy4=W+900B(9QLuBr||S2ts4b0iCU2Rm~X7gJ|*b9)y{2iHTCb`iu) ztiNv(cQ!Y6wQ{hhR(_t^VJP*wnVETQ4b}(7k?b=LS8D4nTT6XX}BMHFLVTR^bUAbAyfS@i$ik zOeNGq+4uSQUiuQ_{wc9xJtju+ZkROx5=d2Y@)pJCkUvExlS@oh;!kv6*WNx|9VPIE7e)!&!!~aG3!E5scC3bSVz>N z_7^)3x@Fa@#zTWTlU85tz55(CC~pGY@tp;;2OdRf{^Y!)dAv_a8Tz@uFzd}1+EJiy z`AWraN87+$e{QIqF2Mq#KIx&RyzwPm&b)9DfaD5Q@ z!i(LF%pg)nm9ayzjXdqzurdDq>l2rsZ#sqe>o{*Cp;z9!FZ^{i&ziQM_XFD?*C?PukLr$RTSv!pE{Y&8e0-6N3*rDQpc`3RR5FK0u5mF}Bhl-XBxI_w%gdmNSgr03G2(o)RED6~ONz*1xbp-W zVyzmv@*K>!n&{-^Ub4*htPa9Xq6WN)dNM^Vx)ga1+M~OI#*RR%aO-zVFaKNXnPpn8{t`N-Tg<65UAZBEuUg^+zhgO&G8nb+;tX5f~rq~LzS`(ZUeCViIofRgUIth_Ak zM9tb3m4sR8^P0W+G|Ew$V@_4AR7C5nQd{mX7HX^}g2plgvo06cd&9LGo#}BzuCjGA zQg;yBBUVmq9+qG$!&4bhlMND%KEW>hh>qV(UxmSosbC6H!4jx1Nu~m_!CYdqhLQ_8 z%>|qUMo*%?b`9ufF;C;!&zNg1o6qyG!aQ6#`ZJ?3%A_yQ)1~`G9<<4#QGDkY`J;Sm zPf#X$s3VtA=HnWdqAxORKF*d;v-R10o-avbh0UJk@aSUO)m3fgh71*t5$v3*z`!t{ zwy;m&90|zm0A_9V=593K+?mD=@5#Hn&QVuIa)F*oI@BQJP7|jXpB-XPUUJjJK5pCA zV_7K>MxpG2me-@U?9kd&iHIW>w{=F*GezujgVEQhT4K86Wo;IND~w?XEsEFdH}}a% zrypgwgP^jHQu0wBTP z6MA#RNjBr)X2pno@BSR~3+8G|IpC*0Jm+=v>Zw|;@=WKVD+uMun5^6IVfB^qn#Tnr z-|>rbxnid09NFSnEs=L74)TlAIH#~C%v2u#mst-HwJFADK4&BznGb{K94Frmf#{w@(6!+9=?Yp_k_o{G z0pVbmNA9FG<&POd^^`=k3cO>va$}#F(@(I%FUenvyiVaNMD?56nfR_LdQ&%CRz1e2 zF*>%PC!)tF_&}%O%b#h^7!`MbE7b~~aqRA>Qxm@Q%C4{o0?6ej}XVNSK%`w zjB8YEEzCv_CmahC6i@R&o8P4r-gGVfOqmR&yuyq&B=Mtl!Z|Hy>90>ynZ3BQjsi7? z4fO3>5mtoJ%Svlkdr*1XOpT@T%tB(OigDID#LSB8Aj^zRL(mDO%X|+>Qd<@pntc1s zfmn!$2;^jwGcs4TP!UN!Lx2D&ZL|-u1xW;4EfrA<1zkz-abrI~g<50P(^NQE)@p1i zydTOT@~GP>&)W>17jC#!Z0_PDH@;=LZRDyO^$2$ZvKV`MkrLv3o{+AuFzVB7xjcO+ z5mAOaflHRUmK*3eOWLmHIQiOK-inUg1o~cN^})>PX^ll~e6Ero@8iU#;X_P-QXW>av^}XwY#h12Kl6r8PmOOIG@k@mRRcL|iPG$zV zbE?_F!YHyJANHQHZ$T*TkC;9`CUl{`z;mXIC70#3QLEEg_p4WmN`^w5N-@?~#-KvQ zt)gTr!b>56=xUcGjf+0|FUF_cD-)fA_cqt}r@YB$aAR_$>x^N5TN+S+aRZN|rIlSZ zs_MENn#|>bVFk~i!ny0F35|%LqnVwFJkQjNmwP?09h)HQJ@lCjK2dt%^d7QMk(Wu` zc_90j<4Y0PTWq`Zej=*JE(7VcaCp~+!TVihj^*Gn;IyP(pw(qyR4dv{w*NZEou%4p zZ2|FqG2H@0w*8TW=T5B36a%BdF;xl>7ztS3Y@L&v!*#&Z;tqQmJ<)Tx(l>ACl)fPE zJmYKQRNnur7V5R2sC>vj37i6kC-;9C2Zno#YAT;7Ywzm$9HMBrnG7c+YQDsc)$R|+ zEcY)&Hd-ZlCEsCw?|u~;iy4B|vF5mXW;0NGDbo4tJy!V76o>)v7!B?9EZc1qbi3!p z>eQ#GQt`@FI{Wi?7r^#ozKc?`DO^Ua?>`IlMJ_{GPd19z(r9Rs@&@A=vo19a1VB4I z6~^6HJH_S60-$%@L0C*`77UgF2+vW?gJE!2vxPBrtS98TJwd=t+ zGrZQTeMGvfV}Yl(H^P+eQJo-`*?q+l+3;lLzvg(|66(E3o^o5ugNDHu6tlg{XRPk_ z3kpFmfa{82%4+-u3!G3zl$Ga|lZwl#9UQD9aVZS}{Jwj%8Az$v-T{Km>fOWgw4(=s zu@xQn0wDWA`m#4ET>9zK_4eqI>`g0|Rej`wT9=s$Apu3 zc!0w8FLG`XQ?IikcK7u%h20M2c$lct%1B%%Q+5SU^6+-a5HM)on69VgDCn~Lh7}`k zayM?b8}2K@Qa0b8a;SXz=J4H&D70sHKV2k2UT2)NEuAlprN6Ek&`Bpy!EV0M)T){1 z;c5>dyzqWgXxA9VRJ&>;8;%#Uv~LqbHd9Bapl4_k5f>h|-1ea=BkAlm3s=5hFw59+ zX|E10G!W!s%HI}|mDIbzF80g)a$<3OBuU)G5GLw^y%}}WWz+qzZV+AC(=Z|8GPBXs zJMw1y|JnPWCt6nXwz5mm1Q(GpoBo8#u`Qp&O5)^aJ&CucUrN`gGkxq|CKuU0f@u!# ze9&#QQ0*y}V~wIQ!6grwR5Oe(r)rrfm08uqZ^SLkvt&u5YeAi%C7@wHv#FHugRpP3us!}yBT#@FT!T^M^ z0^5(MX{DE;?S=Mqqg+mz5%A3vv(u0zG+V{An@r-UZrfR6=^+@%7K)@>%2N~8a3o%u zufwcBYa7#+Lb8#$b`FJA!7Qc9n0z119gY1v`ET>+GKM^}?q0dGR+&`JmDne=dP0V2 zF>n$2ZWKrhfCrhJ~@PqMF?}&-Z%nBH@!F>5Ow%OM*gx8)HgeYfRI}Q!L$a1@;M9 z=$p-C4Yw1O_dCiy{mgm`L!xD0>uzZz55~9$BHhuOrsLIb{lm^5;CwaFpTYRu?HqPw zxA2wBlW&i`ZInb!-oTj+lSLQ3_qj=z5*51~~9k_LCFY=a(M%Xgwuh^7$5}bFqpbA@+1e?P@QwZIT7; zI@nXi?ldX=d0f~@0c$%V_zQ{8a+w)vxrU{s@Ys=J5Hi0}Bb^gPE6&DfMFhjI;!=)q znCdNzbZo|(K=H$k({^^%|04$ShX|EQ>btv#jOcAttJ*EjiqyG{o#uLJ&Dgb zT3tiP(l@bc9;_?iga`Irb;L72U77Aab2}Yie%$RaGIW^u>X+sB`wN7AwEF1}!zy;O zKxf{YPB$8ExtJ$0o>bi?ohC?%f+^R&)@+lFj>UWWTP*Xv?&5N<6w(4tCpz8hShOQ> z9da(FG;y!sZnjHDf(Ff;9mOQ0q#i`>5iKX1BO>j@eSr|>olCU*m;yogJbf{Xjcb|J zI`dwV3byC;nPre)@0W9jM6tg0?|O??7Lv(Q=TGwWvKB#|*RD<_HeIKzhc%$;>$`fE zysPQ_g--{Qr^`_P@<$_ShQNo0Rj|Q3G2tO4<%Qj6PK_=lU&59mZDMqf8``G%!p7S^ z$MvC93&w|YG@CJy8!z+*dhRw+!dJOfp>o?~;a;=F!_dx|p-`w) z;o>QDQ5QR>+Vez;c)ytk)~|0Pdjoe!oT)of^2g8pAlFp)ZT*C+aVQ$G-jR3HWkR_^ zg+9W$4l8Zu;1Wc1CJ5yQKue0Y4<@S##d86Y5_#jC{Ec7&K|rTYHFqfu1}{Z&trW#q zyXU|YsRsj>C#c{kR>My0=Fm0$xL;k0Y+!~ZYwpl0(&Q*rv!EK-kj?ExyRUEA_Ebt) z1{qxbKGALOIWmjD&qXRk{0=X~vRa)AQM3~~ZTl*yBEw@ne%$0qe7dfBR&3fUFFG)< z(2p+qj!l#j`KddZQD3#En~BIa;elR*`i10JS|BM)D!#o_mG2zqg>ph4i@=YXOva+! zq$4GCcWz7Bk}Ty8j+3#teCx=P)3g@9UZ#z~ryYvc*3Sv+cjD=HG1LKp_>AN1a&)6# z);reNkx?0^OU<)TFw14W2NJFm1K%|hv6|PTV7BW>1Z%Skc^y@ftO|AJbZbL+D59RU zk2q_y+SgWN3=16g>q#kpKQ9{4BOe~AZUM*&fg{xLOSl&%3Z(@RA+Y?^Y1^Z7eGB@K zT<>@3>$l_F)X@!#vW0o3Y(8y>m)2r)R7SblJS3&<(J$W`^j#ZkWK)4z+dJZH#7vQ4 z%OIF9>&CETw=yp8$%4glm>bk72w_L|@@Ifdaw1`0X{Pz%p`elLJKDA@2uA2Lo*FS9 z17}LpwaJtcB@VQGz6j%eG4%7?DrTP4rDKLT=V`{Yp51z$w~n%?a_$yE2Kb+O6&0DU zgR;Qpn?usPI;h(0O68_7-p3ddoshjBkwMF>cr3BM5hAB8gZe6=nM(2$z~_#ECxSY1 zpRH0$^cqk6|JIvE;3%p5M5~~c?u&au6$y#Fb&lw2;1uhgR?$Qg{q-1oJPz$Q7ERD~ z9K~r=)yag#9D6s4r!^^9sA%dGJ@xuiI3(aiiMIqrvo;MHEVr&LxIs^uwI3ZFtN7tN z`#^b6&z2N^+@~=nVYebR&0s zLc&j-m0hS=M7sJqTo@*U>v6W3$3jXEo1atC-5)5VUPA)~%@1ey-#L_N+thxQ>^Z!1 z?g^!@;;2+sN3%D;iH=Mg=m-ufH9ZEPziIb6F?->lbg1Ebd>j^6l6peE+kV_XobsX1 zyxV=-fOjVz;BSqq5wTRLeR?e?f9esHmi7uTN;5ej#IMA8xX-9XVYKBchMuAo`s+uA zmRpoDrA236WeKNZT=Ew`drya@fC!H!I z@^-7hxf$9gZ+2&eC~bDOIBoK^v%-e9Ds4p|iI)Sfxr_IavZl|+Jut0y`e(;#MEU8|Z?POnE4X59VlsXHYR zeQ$q)ERo?bC8&rS`0ZlLg7a}%KcT-3;SZ5h@t#k{v=7ZA8YApi1FNeDv zuC2Cl)2Hzz>PnyqInT_x_MvhNgT)pAi(xBy+dR9J-d!m4NeA9y>;hFom`?sYndR%R zyC7G?Gl6zWk)f_b!k_zV1{Tsa9^82rGo;M-##GzeM(&6&^*XEqABF`*5g71KwSd^{ zAmgJB6dWUBB7<;2w*&b?#f%d#RyNX{w+L@25nzv1e8JbD6Fkk*h3?x6@UrCFe0{LH zk|^*xb9RElJ+SwO(*R1u4q2qMG78|%if0e^fa-@|GMaA*$=f_LkC$q~sV{Yb)8#Sx zeFyJb4}`z%8XIX9PvmQwp4`TkO>E0as2g(PV^=?Ryco$AsM+`B71wRn@-6()5=!BP zBQDtg^1$Xxs(TowE$*>G(G&Z3$jLUyqic1doVTG~BVh6JX9fRHcg2K)RJ_elpAm&UpeD?jk zbiSdVxpTh9g#*mt9MqIQ&)N!}`2@+{jEP=(FEr*lu&C{N@mGpGH+ngH<2)bOAka)e zjnE<*h~t#;w!H#u&k{yCbbNyW$A57p3AU$FcdLHZIs{zWnooN zZIxd#9JRBMNEqBdg<^gi{25-xQat*>Ttj;vv2lVCE|<*lE3S;@kdT4iK}hmcG4?vU z(6^O>XYYw7f^Wk4FIwV3K6_~?*Yl6F$tnFVL8z7DlopP;Cd*IM>s-`6rs&TUj)*@Q zg6U}l5WiFP{vHA;LOcKV4T1bFfjG8)3iS@LWm)uLvwc@P?i0$8QTa~Mvh%Y@UxAsR z@_{Iirq7FGDs=IG?yjt=nH&0o^P34V`A+SYf|w@yVTchyF*ZND5ZTWH?Syb^|MR~) z;FgJ5`2OS zNcySDm%6`2&zQ2Ee^!4K^E{h zwVYG-&TG_yNyJ_Xu|s1FM4s$2ox)oE^hI*{SlP(Ge^#mfPKh&%dL zFERW-8+!Yi2PN0C{A_PT)i5%@`Eo88vpsdP(qj!^Ezlk@oYTtVaey{WOY$&yS9l>I_ljo%5{QaNh@P=F5p;=APz*t^&fd)mv?PS;xo#TG^rgSP46fNq*BcmkO z|CCvM!DTo3GMNL;*miqmOuFxpI3pblK5e)OTZwJ8TQa zskgx51ySB7Dx(})l=UO^DcxtL;FtHG!uv^hU~XSd_}WbSrG<`;cxP{0`$|@PHWqOD zoh_1d;s+vH3Np^lxW>oF9`Y?_IiUPRfssUj6uUWdcB^^bJBOj-MW;Dzk%nBVOX#}{ z7rH;m;V(&FmH5*{f}3-@8*fJ*S#mHRJ@60p4E~}jg2Zb(C~aL$9Uko_zkf>h*X~Wa z&U`>H@L?=8r~3tyiHrq*<-)!hpQ4lP45^RTExnHrMQ=rM>KgLP#q{Fathssu?n|io zLGkdMQG4L`Il#Eblzl&eaP#^LSG(B06JYeou0P*Fc&O2Bk^^8yAfjBvJ^4UH+o}a( ziM(%^)w@z|&^blf_?6!&-ra(r3A8O44D%!A%2jZxPG&J3)S8!wY(08G8={3o9vF3r zSuQ})P^IZ6J!5J5rmR=!zi~$7I)yx}*z7w1$D|1eB zXy2AnbXUAn;2u}VF?IL41|*`Pjm2OWuHFyXZtqg8)MbYY-$ZnHOoo5DcItP+8|VY+ z4)E2)=70(^mjVO0{)%HG~%K9Rt(kT%UWL=q5DS*)Fe8HplC?f%aO{rwsHWH#(!oZ6b# zK~`?1t8J`@t5mJsiPKPc_UsO+MJ$xnFV>fGlxj7c_qp=ua8k$7j#~7QplAAsdJE^B zfd~#|r!Pb6hf^7*;5O3HifFxj%kJ*yiiNaHK8&-Z(PlgS-9@0~PilrVU8lqbJ0#t9 zhRpf!mFla#?!k!dN7hH)J5!i^OHhc3{0)ZD)l8oDF3%(%BSr9vke74U8?hD$)r;^c zf8^wX7m^fnOn>U63Yuh=bKOf!lypUlX|0dCD5>0aoS=KfZ^9wTUIl_I)5mw;@fW`K zLNdH8k{M^J!R7D!`!ivE2M?cQYj2DL-#l|5<&N}gpRR_sQzmIbH>jiZAkW#oil!iZ zP%!1tku1vgSy%9g_jepOiA3-<5y$f4h=j&nE3U;E3)h*Z#lK!-@sTDGKV0MJLtsL3EtauX$uU;7ehZ%g(mmI-=) zr+K=SwLR&eg714uTs+gac4j42;im11Nhq@XfQ2Og1?JlkprrV-v5ZX`rUdIf4f#Mjd9X|8YLyx!%H>>dz&6wsPi}KbN1zp5yCxiDYoP?NW1Vi)n0FS7Y#H*>=Yx zck)nQJ!^k;%f_F>jFr>4*vvVIj=;pm24CMHxQlRnrOH>Y$l@Km=m#@e#I^>@|6|ib zLiAFZpPN@k<^FjsL}txgiGSB$nNBqSXQG;`p~7<0z@mlq{L$6NY4e)>ZQ5__2s(f)nciLqd{ zajN9*yk5JHaA;^K9UWb~*2kpO>}+Z=F)_=<1}DhBd!mvviy7g6In;l1zALM!SYW@@ zq{W=tTK6qokv(T(qqhG}RBf=2Ux#f>pAFsRhq>tl9z*w-f+X(bUB zPknH8;7!zX9_`Zyc_Yq>do&11Bd2XrtS9@8(qd+;Pymjs~vs_%mnXK=Vw%a3vA>|SpB$* z8`fxspxeo^=OMD=DQSr2J9FJ;2P?}dbgLlMEnT;dl{0pN5ITLENkptlTxQ+T-Ppia{@ zL3gswv-7j<7?XF4^qaxQ*at$r%f1&THsb+13E7Zk;bXSC10|&F_oLJzUNsi{(tp0P z=?Ayss}Y^+23G4=>|=McvtyzJcPqD`={pt_#wuGE!GT{=5~Hd|qLS)e1OcTQOd7@9 z@JB?%&J6$6qmR$1__P}%!l;60^oGf?nP1D){bsu`9ApbP7Nl$hVz$#0g6{PMt*j0n zJ9295ma;moVq1wtt27n+|1^tgl&peWPsc3$Qp^MX$Q>KOj&+C>^y&`nDno~0r|yQPj#9(0001VzpFy0w5O-%M}@Jt&(#|0b8K?7 zj=i~R=mIO31;fSoZf)bIl$6lcZz%KVF6nyk9O-CEq02=VK5}gPmSZVIm?CXvMu$Pf zIx=T*b+qQXKcAqSE5!jEAqI{l`%PI3iwXk*bEV^CuDoduONqO#zy{jUf;h)hOj#Ae zxdO-i6v^C?GmVcsvfq~_fp ze?(jZy`5WVqg%sCaCv~|0oVsIUKx3Lc|{A-ztVW@wCZe05D5ph|JDV%|CRSPz4 zeIKsjhzZ)vFxpXBCI@rt>+t3n=K@_xuZRJ4=>R&wy(Xm@UsjpV;zVaKixFU93^JH0B3_dHbh!Gapa3AnUUN9*BiU*h9k zJ4If+c+qmRQyf9e@uYalV=c9C%za()YN83@BQK{7hwh1Fjhtt(L%eC{XB222%ruab zLz$A#%FB8rf|P52ZY@5}d8jx#|JrC%YhBmBFiVQ_zL#@t*fa1Ez%1H&AsvKAIu#(I zG`f|I&%SZ>@Y8^+Tr=`V_Dky;;;wc>pD(ZXckoCECF9yQt~Rv5K=R-#&>fa*M9~L{ z(rHZZdk4kTPmpxOtbOUU;qQ2Oy*u&`&#yWvXbKQ~e>S`m}h4S+Joc`TAbR;%5HiQiwTi{WM z%g)Yj!LOw<-<1*#^1318)l@L+-IOK@WCf4%?U5 zdZmgk_oyeSwZh?g0j14NWcRqcWg=`hz}mftZg3gcXqh|wU~m}UwOmV9bh?S>LuFZU zu&>*=E$$A!p=zOM%!)oFA4D|IgneY7}ydUyO)8N`K1z_=Hb!82Go`MuA9zC@x8oeY_ zNGhUzo5FbYP^p^||G=l3q#xd29__So5B*_3+J7d#rYv8~xD;_e7q@ zZoY@iq2;}omkT=kG)}&3nZ&a6nOx@4aW3<+80m@tqRoGB<`VkTQ{c9x-Fg2 z%}U1LC+tTUr61e_*o5s5r0=11cuQZgm6vOY!FJ*d<@1U`faI(;?GL%ln$F&~+{}yp z!b$C;0oQ^~HJ8YkPjbJ8VxErjn`HLy;;Hn=8etSRsE%bC7Xkc>MAePg1L_+ar%ol# zJp(iug#IGRYQMTkG75UUVw<0==w!+L^$5!t(M^+WXMoeiq(Ay-@>?9WZ|)|a$@`|d zqj2Yb!mNuuM|qMqDn)W>o{1&6)VXY0$f4jUppedxKF03EC%>nNUx>e>asHMs$Jx2E z|F$ckbCY ziu`2sgnU6})TK_7@wk6o_Il%mKp#+P&`N-Vw3e2m9KthE2iVPrLms0Qdh$Itte|;T zqKsn!d{O37Y1W&yPfMMrs>>QU2AIj=lr;(jr1=`ie@LS9(V2>G!>1k4@=C@J_BEC{ zXRB_8%coBtq|1X%Zh$i{Xg_8D6(S184AwsEQXHKemG|CJW*E~SUgM%_*-NP*T^}GV zO;l&sKE6`#T*;Jo4hr2iUnN>a-L^90i*kO$nvwr#|NbgPM6XbGa*5F?Sp;6VcH(Yc z{jeVJ0{@ptF_qTmH)@5zi}-4IAIXo+=Y8G~6s7&ga5u_^G_r#u;97G^Prm(Dzd`Js z;f0T4A<<_%a*LT!E(LN%YA$$G$H9vH#exD~i zb^;fuRjM^eM~iZVT=5c*Q0!wn(e>{6ayoC_U}M%ZIK7#(*FkZ_BV*!a+igJ9YgsSv z8G<`>t?vPNCc;BGw2El_f)67mwI;KGHyc||TncuDb7kWgxU9wl0x^ib#Ka(V=({aR z&Xno1x!axZOu5vJ#?vXLv|U{jYvtOn*WK(x?nXZzJhbtuvmc$G($K#pp=}_Ir9_?c z>_9f=p^ihj#onMI3C%Ljz0O8{igW9i1!t%#*`{WY&_2S%VC1_oC@Aqr!M+}wCH-q* zOd<4kQ&1yf|1ZWBmn;B3&nXF|;ZJ|7@V0zUcbY65tIkUZy(zKwk}oXfkyQJ!G5{BM zN1+^bhU0Yl=(HIv!_%2wC1gXLYfg2{=X8{pNN7vHnh{5oCq@aCZIkNI=EbR1=d9?X zk?Z|wzj7)g)Lz%W>vV~21M2uB>Hz+cfolLV)L~s46MNv%1Ge&bj6Hzgx5IxOT52_Qa#}iWsRuw2{JN<9rjeJBG`P$H9@-LQ;lJ%_@&qU)V3h)!d++>^@oQn9&ajm; z<$597aqkCq9l1=WPN{%#Gtu!vkU05VZHjQ{7Qotidpw_6uOSAsJG~`TXTKys$f`Gh z2tH4}Gg6$xUQLiuQK2wT5xYKSoHZO6A6V`_BX++v16c~(dpen}&=m+h@?d}|%=!<@ zPu6*P%E;zP*gB>+pFa^5|Ifjn)=q+s$ z?kt|rb&n`DoF*nEH@X6v!FI+|4ii@^Rrq|>&|m;<#w4c2nuSU2WHuC3s&#$=@T#=s zm5!n?f$u1g54mt1&Yc==ZV;$W$5+?B+4C!X$i3Q1GIW0M^_X&H@U9NQYQCs+SWp3a z9itv|@8VL-$GvaGK00=ZOKIPK=_GwoVSg{8h!RxDyS!&H(HA$1aw0(a&&9R6f zSBn*spCi%6i)<_#OAJs&kc@{P%-n^)Xs770bOzw)r*!o58jdCK zMsMBfZvn*i5%yC)OC%$TLJ$@)jd+35P82G^5f`u9#^hz!Y6{ufG>|@bEN@)FJ}-n0 zKD0-d^9~k%;fSQZX*r>f4jO@tMU8HsbO^C}KFIZfhACC7B8`8IM#{RLttZ-4ON-r{ z8{JJ>gk>92?j{YYakd!@o2dMo2zW)3)8`E%+w1ANQf+e=+?a# zVBGc8(H6De!_jtxlxKVPm-E6-oTphkU|*o{5nebze}CpPQ9-NB-Vn!`)m(^%w-g5` zJWilHb6pX-t+!G|B7ATY!u{4;KqI#33#Azd<_kJ-&)te@0=UDj3e_ z$cT``ZG@rEbs)lZF{)ocAXUaoO&~7YS#}FUH*!A5PX!8TRVKY*Ccv$AVD+39;^2;^ zK1ki~7IBs*X>-#|DuNifqo2w*qP<-d5%;s~v*!o=KI-GP3<>64c1IZ?t>i~cMA8C0 zYDE6Shj$wxOl6)c>CifqP^8In+;4C7|6u0ckt9*1KI5+!$6sA?TiF^DJf!2}o89L^ zD1#KjPoHh`_RNVkOnFrxjz0#rezG(2Cs!H!VR);hf=Qz!8xg@EJp0W0j4VXhj*E{k z{_$hX0x|-D(>uvXk^EWjUw;-$Ly)C;7F(2pCTDU-O4Z z1ACX2(rTOvd!__5Guv!r$!9D-rKYJe8^nvE67^X&MR)o7!ZJVm^%KD&Zn^@4f`Ss= zwEojintVlC1dGdxXCc51wf%$q(nEI^;0mt-n;IEWD;DAxPla`_z4_U}O0Q8&P&grL zWRyLPpmviG;3HCL{U6|cQj|&L4@`7)lf&g!32AB5YO4whW&RAsi@iC-ynXyXC?&D7 zI*q&_cHRDhcHUEjnf+j5LW9s6dh+>vT6Xpr1ira5Q*OBYZ!lHuO)sU!T=x6VUqZzH zur?ABn@@O9P=0vW{xLvA?#l6I#1FkDS1U;0>wgtEBjPOlf;-Q zU@+Ahyw(-CMgBK9BJ;Brk*wDu{tx3Et763ee_%_=IKclqv6)vwA?y)fQ^U2szK(w0 z6cU2w<@K9ef0IA(Pdj;EE6gK*OuqS>+kUHyK#l0>=mKoY8Vh8~ACLQ~Lc_yV3;#i- zWayI)(MF}IyyGLucnRPBM4(EiLrmqb7+HTuJ4Ku0Xz&bGUKA8%Li zqCWi7*1Oa_+*Bf;&>E)=sWwEaX|>?I?Owz=;_%gU-6*izx^}&__{Cmir*$n%NCa%V z%Q@norowx?pXVUkej;mcYt3|M`MWMwh?A2O0>wV1tS&4#f$o;$U!FpZQu%$2TRkVN zM~H1jM2PZmDJb+4jsH<58ItN(4WE1cgM(2sGc)1$o-nrZez_pot}e~S^C0Of>!jSP zqWnYCtrqjW50j}mzn6$0)G9T(Ko+(=VVlhLq8y9NQoEW#L0rs|6i+OCXX;qHJ__rw zO%74cfBR261Nog>AA140?9hmov!k<~?u|R#R7y(9WJ*f@=sDYGkv{SkbGmLEYxSmi z{2l9Sk-nCoo!+{9>6Hh)d9k{-=422IoPHyJ+Jdd1 zq@*OT1U(fH)pv0g;P4yKxxc>jUP1%s#S+(tgHC--#!W>}7Ahai%f25AJmgn@x7eYMd zi@oN5c~e?$efHi;!}NSW;`Kfgm{AXmw{vkv9P4GCqf`pApq|oM>`CUh|7K2CwO&1Q zW%5%In_P~Df~w29K5r({%~7@RjnFA(2i*ZQROoV01v^t7rCo;9tvNK-?Yv-m&;erG zV{s*N#m}Zk5`erqJ7z&kwFXUOI~^Q&PJXjN%b<`3Eyyjbd@iBl+p*E&YAQbS?A`A% z!TB9-vfBeU(qpZ$Y&W#1LlOr*pb5V{`m%=f$=^ zI4_F|XtBI{rn7e0SeDEH-{il9j0dC&Uf!FP6kgZkry@nEND$Un?fsx4cN6~=xUg86 z9`*T?$J95CGCFX!T3_86v_=SBGf(6E`|Kxc?&6rkz1$X&4PET%UBzQ&Y`Aubz^LyIHUs9Z76QbnJ?^gp=(M0;-C?rQmVaCFSaO$g=EQVfhm zXMAsr^C{3xhE`OWTdwQYbJjD8881~asx+%t8qj6|r(%@tAPZ(90C1+LO?+@~y$OIO z4@WP=eN&$p%Cu{iVVOv=Mt;t-Lg-pn8&+@V5s%`=oohZ!$n{N&I&He{x1KxGle`%9 z;^0U(sK-8}5ae@yyr;XrW@KHx>Ix(-R5HcF?rYYm9l1~V->j-eVXX$S;kg>Z>P&Y8 zmSwaR9v}^U%#HkLXkG2 zj{1ZogonqD{%X}Wj$tyH@Azx~-`f=NYoHYRMZJoxJ5Y86qnu5_{vnsAnq;JUFI^;( zY8~T8w$&>G?E|w_)tcvaloW#trh=-DD|#>o#X(@pQ}E$zv^HP*x&b_{j)cmq5&L0C zL{m0(ap4Q8IV~C+uB=4NzL&?$8+q689X{WiD0N+5lch?$MfF1V6o^LmdzpNo5Z|VP zf$uj=X(Fg`E$83=IbmkAYcp?ZE!%dbExq|v(Wg?2Z`JXrxw{iNEfRo5`q66O<({A4 zhyUG#i@++D;u4j7^r>LXYu(RMl9Kw~h1J2cwRU8_jd7u(bSJK|+D+k+3upjuo2i{a_^or~H#6Zj--PF{ce>$vIY8}nz~ z!&ykp;aR?VU2FuYADb+n8uuwNryNuCrY6#4Nu&GrHL>z#zhBKLEfNJiRefwu1LPkR z*jwrdObFFwI`9;zlt`c%0GZqOgaa20(0!F^g?}fCH4-K_yxgxv&@HC01NCmMy2Pgjqpqa@d_hFU<>5Hyc|ME~1$%?D>U^v}vslUGYp)G_S zpJRWR1j$pk)KSlG6>z&O8E%7$ZdRXzAVu1qnfq@3gA<~g`I5(cEZANr&y?58jD$F! zpM96QoyKK;xS~#I=E-xsQEODBG8)}0Pj==@H0w^wKKkxTa>WkvE_ZtO+i9^k?d~BGOoIVMV*Zs>n(0xjAC#ifPf zPH=5;C{l{MyE}mZMT$!)?(XjHRy4)k-JKBh7y6t&=X~DvUF)4ck}OE(o_p>+d$wHH z9?{_6nW7eo>o05|R)Uif{nos%^YJ&Wh_Qgk7KaAo}YV1z~ z#>jUIc1!mFCu6Jr`9p&G$lomEMk^sqN&IxqaW4GN*Z+C@3CX5n8>Ty^AR~KJg&EA& z0q_3%|Fbd!0F#6LEF6-tfs|U z`v^-lHQ?9&eZ&hNaoCfdaA1+mds6+<<7{?Kiw(q?|NdJSrzGwFA~Aq56m{_R8ADkh z-b$pFr8yr!ok0UKgmn3IE}dma(9xj?Rr7Y6Wv}5Bgdx*HC14j8E&R7_`+xQmvi{< zqhmmg9CxTg1<_JXJfxRT%-6=t=yY#%VK-|2)1S+D;Rg+&o^K6zvB{{{S*L{j>!kh~ zHe|8S;BlBNR07>!POE7vm#90*7~nsnC{U_HciQL7b%JcWx-7Y>*H1BTRt>8aC+%)Q zz(;vfs-DgKe5Z`BCD&Wi%Fi`#jL*4Hv`uA)f04boab44cWZC9Sp(eLPL|?s z+z*IgU-Yf6daFEOE1)rn=ee@)O%;4@>1EhG>$VsI=J7u83a(_u(EMmjjN6N&>;{Ji z@P8}cOK~;Ym1itmmc9wLZbkIh)9s&*0^(05j-)h?=CUDxh+R#90#8ECxtz@H5DzdJ zF@k`TIOSsgE!k7|CMf}BGQnst_Q`Kg<^!H(-un9Z;IWzh79^NIIqRB6lnj!Lzt*mt z6!Ok@2c9k>y+S#A=e)0oD0oiY(rI3K^W9!`7;sm)=YEAv{A+@faP63{GZCV_k#pO&iB=pQ0Ef$`?6rJ$vKs(%7dhsjE$%ydgYq0{GF+r=6HkK&5b@)LiYzw z)vlmj&Qu-*l-2D!-=6gwK=bXn;v68R_F)&JypP#>)1vz1c^82M3pmuQU%VzY3s`Ub zO!KyNE9Mz)DvTr-C~@?Z^RSw)?O_|X{5LsczhAZ4yO#jdV5kIaJDJ(WVyplzu4@8u zhjm9DNxNPL$795;P||^9zf8X0jRYijcC$;`n@OV$pqsF{m>`V{Zj92UcKm@zrh9Am zpvG!T%rL6gTI50Os~)Q57ccM0L$VKbX+CcD4x3`@dG8=%FWeHNwW_w?M8~q7U5DVW z3#^IkW2gcuQJ`q)%G_%zZ?hm5Hr%u8p5dT7o{)vZgqux~M#MaR_{%$vv(-#?6>cS5 zSrgiVSAE>&MsIA;ucL*bNu#;p#Pk^xhcf~meZ2W^%EJM4-=S1bH^-yqPKgUs1@s-JgeQ=V`}CHy!O z_L7>r&0A!)ypA(%?;@4ecMN5!XI$isS;h@F`n|JzSAYAu;(C2uy08`B(uII@aqNu$hX$ z%k6N^9pGbTD7nsfBqI;$(-!x)MQR!XU^?qu$cr;Fq2mNC%l>i?yUMU?5w4Dx0l2f! z5Lq%}9!fNpw6%)!F2Prnj~V?j>oub>op?#?bD-q**n9kihG(hY2IUh$v7!l0Gc7IU33*(^CO&dU z0^3j1$8ChC^3c@7j9QZOuM!U#ER8>%(tY)T>(|Qlz~kv%UXo$=aSWAU97q*&eP3cQoEN*u!@-TNv)p1BlNjV|a5TKpC-=u~2`{vmi zqD!8#9>$u=A;syjCyohPmJk$9mI35@sskKv_9H#u{gr^yTcr}5$=3z6*uf@q6Zuik z6k9F}`2uZBu7-)}uS>hD12yF`ZI4k8TuS4O7XTpWX(zx=p;}0oneK;Gk=!@u>WjxumMc?rM3u1L1!%8e1``?&1%rX*x z3)_c)V$U@M2z$TL5ZY-UXAJ)}f9&u3T8$a6h6&nRB}qnLzUQcWccg%yJ-TW41Fclv32VKt9+^>I{@Yw-$q+J7gZZDUp z|DCZq(h%>+d6o?U$onO0%Rc7NVy3Cg3$4^MPQ=0LWD@984zlNY; z`KB`*&?g7(P)AD|I&R%QOsaE{Go2@T4NiVPaSn&dD_ZD!7v_Da=;h2PDBzy5-ZJ}i zoq>1hql2L79UfE)dz5nQqxK{^o(<>!g|{|HF_=6%`A{w8LBL@t0V~atuhdBzQuFTZ zKBsh3*(YKtUXC*{h$$^DO6UAI*71t{g+}trf+ApnrOL&WaIRk z^>J4lvMuh5yx!S-m+s2Y7<%pRi!U0rMHAa%kKq!-BqYEj$LE0}Z?eG?F0=owt%YO# zs-89CP~b#okN5fR)nCzr<;aWp!vp6>m(EF;}0?b%a8w|N-6p^v|W2yr_ zBreV~eFQn=1;Gf1gj=d}t>ey_EN$Zex+a$Kjg1;lsHWy_%fxUi`1W_bIt?;{>+0_6 zyL44Fo|ejJa(v;$KD84e2Xir;_rLqFFq%le)72QS9C&D|3VK>kJEmG__+ow%a7BvC z)H{aE5_vtK^>{cUth-OVC;N@kr1PfG$<>#En<17h;(RzAXM$cK9*zlvulWp1z6eX$ z$WOLi#RETp`;9)h4u7_e{jy zAFOw)*!C~p_N3v1L#Klsj5jSVNXNXzk*(K@QZw-Sw#V}9s9Mom)ASUyGcvuN2gu^2 z8;U*4=J&UQ8Jpj6@uofo13PDxe9e}OS;e*^rGW)xQ#!_HXFt%piBbj|>k{9X_Es9L z^3}d|^~cb$`Sh~lgTDwS(Si?e@x5y09X0%bpFW(4r+PomXNIGuamd_OpU1a|M)s`= z86}|e5WPNf%31(t!0)<#>yZ+*ZiPsWg#ga->pX1r78c)IDR7Hi(Npk-jS>>=r_EUrC*sdgi^ZlRj=h9t`Z0kh7E zqpT%at5(4KyKf_l{$~U2Bq)cIj{;ZoEh+`FIvf>MErY>i1#R=)Y?BBOh8!Be02+x{ z=s#$-%a!&0IIcXU#|~_a!(I0HQT`4?zfRWZ1Q0PsS4>%?lP_{kDT?=asXvuho#9Qj za+jcFA&WIv|146)EaToqdtb$q$;rX~)?S&oXei(87;`h`R|iED9$Pb3#P>r(XK*Lp z7QUz2CBJ(xy5U|D+NleP-mp8<*_a1Eb^R_e1&)RS^EujicyQRS#LG}(uw`1N1g+6#x-VO>riNOJN8b=i!f0wWiD+{u@ zXzt$PJWzS(eLQpbEoQNQx3WmD_b`5idJ028F?P{>xG~xtV?9%JBk_FRBM6E8c`ZxT z2)**L^H}W^1qSD%es6sQ*XRxWROo?_Cw#tA__b2c4*$J|>{Wf*IBCSz`M69%^Nkh> zJ!th@-=3iTgsK=MN_%6t;1P`>E@x~rnPCYn*HO>RkR&gxj6()TtX6r;xU!=5&}8s=e_E!eqiyCU}c-Vx3y+A~f=@=j>U^-+Z^O=~cZNwEt)T~AbY=r(@4YHpdYZ%hX!1rQEc2Of@i+^s3Tfu#y(c~AVLMoE&>#OVk4V1C>NB#;Lo7nlS2NoH zq$BbowVLUOo2<-V^1i!tty3*kGmm@6Ibt8x2-s|rojuXWxm%mbn1*#kg}}0gAcQXM zOh$$zk@Z(1(-QhJuB15h{6devgbp5TiKS$w?Of-#Eq~$d3ea^hsZx#c8CMYT3bY>DNOx1k9oc~(oRh&b8*fiC3 zr^m6J%G}WqLaLmSE?s3DT`z?&)w4h}TOYYx!X9My+E#%7NFy$uN1JH5I?{KxI{%_@ zDcK(ZTnX%A(wv%vUBQBr5^Iz>5%Z>4+nQx$xwdj$)6;xe>AlG-A@X=k34FeM z&Re-wo&e&!vWeSCu>xZmny5zDK&ti<5(kb^baLJAgZ*$(Kj?G>^EL6|1Gpm%v4~up zGYleqhmxwkE3ORfy|t|6%oc5-{~5_`+cV8GeXWhG%OIBU75%Fm5jjVaY^_-9STwpf zmblysWzb=g7v(0Hrg|Xl;bH$IccGm<`vcM1Z!=u+TAS^B_F%Ufk4lo0<8tg=$q;kA zwqlcW{Q9|O9TfK=P z#edNgYA{?zcwMf7$NMlJW^Rd~%O#X_ArpHoC^C_A4Og)|A z^xsH4EW2oIDP{BkKgCQNjj=M($%*$E!#VuE*OmPbOwjte(A2EIx@xvagu|*q29znb3@7}nyik02x zQBN5VO(%n2@a^$+ps=K!bsK0=vu@$*3(1~#5&F75DiVu9LV7>lVA_tPfF1^3oYj?x ztws}rZpKud{RG?VYd^)QP^bm}c4~)jo^$3pJpQa_3+BPToS+RidYldg+HpS+=Z>;>K7WBpWM~W8e7trOe>*3xoLS5gC)W!Sfg)v5 zo66~fD?*Yw)IVRXoNi?S4+`?^lC6GqDvko_IffK4MLnud-;xk|9V|*TN05yp?AGSW8G_qn@1VlC&~ia zC9_44qGEzscj4qO7oO*7X7ixV8uY&Es~K|v?xbbA($?bFa%80tN>-z_*8(V};vF+B zA7a|lYDLSoHUDfRe_rmIWAZd)Swn*#KbcJM zg2O9?qDmN_&7KzQ=$61W9{1Z`25;fyy}nS#*K9&krYdI83Ld4v9gXA}{mcjO`oHVa{Cj z-2|86O`Qu8IdA1zW>LjI%-Is$ny}}8(yfxDPtXeVW|CR$5AWi(7?dlZ?8;xt;P^CM zJ(4G(ZIHKzf}%>m7M)Rsk#(DG@L~3j23tESwCS2uSBQcU(0 zzs=bwAf`N0xzm#CAX$?&TwLsMJ6Y$|Q8NS{C+tZ2J^VXAcf__|2jY~c6ZOW`&8Qd?{Sedg0#766UjTKXl(bVT+l^BbT{Y6K$e>T4POd=B>w9 zu|u;2+|p8KPjP*cjEEdlmt4jwR{h_u+Q)#DHca{rl%k=LO}3!Kzsog)NqR}k=-J8{&YeA ztLhw;kMcgL$@m;DO_~XoZU5Z`NJ-s#9~^7rrf|?$3*=+koDZNL^Tc2}QvcKzNa*-F zlJB718y=Q^T>ciBQcS8C>E^&wX+BSFu6a&YKMRNei6q%)QS+M+mQ8}yz8=?llr%w3 z&F{}bX*dG}mu>t_toz8%m}(q%zrw;FCuak*?%{)*orlg<7SoL8)1@B1S%cb+j*g|o z8vltNePB{+1woxi&v`r#7`79V7plS++e$Vr8~SKwj?l@@G+D<#crN2yQOpKvoa?0C zwaca<@LQ|0Du_IYE{M-GoWAg{hE$bsb_?ZJ^H zrW1DbZDK9I#JdP1Azz;rSbA0goQfv0*Xx(f&K@lNmm?vW*pW$K4^t8_XGEs?#rhBm z$*%*9CIz99=5py?{=Ri}%ozubQK3 z9q?bMq5b@5aL~WOr>74;O=ibRtHW~xe+^f|xE?TwVCgQ)vuDpH^5xeNMgNEE`;&9l zp7)zqSagasDfjm%2C>mP>yatPfg(+$R=qXerh+R=uSD$n#W}R58P0tK|GO{W;~w2f zDHOBl8$K{H9wkOa5pr18e;WO-@(SG0lG@%-Tr+NsKS2+H@H+7R^ct$U{oE^;I6PAI zn9T|yYAe(DRi`1V9dM)lKcTs-?djpw$;b5~rCW(FdX*cM-@bjL(?%!Z3!Fb3Y5pJd zt35C5D2w((<(#;;QGj{D&4j7Rmne=atGmLsuIz~dXV2!p+VgV2aR%fq3Fp_L!7`aW zTkTBQAJ{b@tCJLM4&%boi@iw@rp%N22{|Gup3i~%Lq|EU5B6~TwZ-lbZ0a$9dx@$^`HmyhfGy!)z{$^ z-|ADgX(nDuKLqVQ&Wr07-I~5NeE$@yt2;Ari!I-hDkcLC!=k`5H5RywO)h;f+JZCJ z|2+JLUVYtkWEtBd%@K^gGdoDV9NyIzarGM0&(nPmT}fpN!jVJ0%{0X%@^L5cY={I9 z0xcKf3V1OY38wzRwpXO6E5srHi4ZyXn^o zbibM{`XmZr^tjum>|*#D>~zh=#)$^H#4Q(y@kXfG1)WEH0xA7UHn0Ydab*sC(RY+B zk`CXbL#;H2wzcnptL)AF|A^(`#`y5eLhYUSP~p2yvae=z7)z(BMQr8j8fR)fJ1&TE z>we1I*zxE*$p3JLuv5TEK(F2-bM3L1wGlE0X*}X!EU5Y1q02R&z0Fx$jGEniKE3p6 z!VY_Dv}YS$QtO^RhbRU=cC?hkiJfVZNhUCj7{eV-Z<6G{)zv*O{h;OuB#8v@IYLB<7a6+=9$;02Kd5(l%A8d6=Wi;KK?ct)R zGF#!fq{a`Br-)T@@YflWqzxK`@t%+9IN_hof?87-k}@p<#xi?srzqwM-YSqT3tOnR z2r?X+q-qWiq|n^ecoHLN!ckI9m*_>P@avoN+T*#tgTm9-kED?IH>;!K@{RBX6qzFV z%cTr1V5DY@wQ}x^f#3$$#yQfo*yFdX#41{b#-$T@(+c1V&*m+N19$5J#}md4awf6> zvy~pa^K~{`ApvCd^{rz zM_m|sI{{*(Xd>`%z5WCY*LVD15e4TW53|LVYd7;c;X?SA0`dccx?yKC1dsy1)hhXN zFWXH?1@$iO&P-qg=7#r);W%pk>eOOVXbSE~#SlLgwFFR>0ez8B2tT5tk%Q%nwm$TN zcfp~(G7aPBcC9)_6r~C6;Q+W6}t!n2WFzPr~rS0Ow0AY1kdK<)l6`T zTws+p>8RiX7x-FRTZ^@tVqxieJ6QFIC_yhzC6oU;x`CSQnR`R1Bz`W*-G#nLYJeP0 zqsN=*r5Y*uEC}jVj`97=zaDZnp@jb6&0%~D!8r2<$Y-VsL{8vGSqZ#n)a;Nk)k5-A zzk@%`F4yaixOO3w0RMKPe^^ERP-b)*EMi`3F4b{hKP0I$Iy~L3@9?9{ltMjx;qxNP zdBSKN_7qX~%8;?D#)j6!D@o(+S{=LGBi;KgY_d#3dDg0pF(Fk)gMr4{a!(Bote+HK zi2@5@YhEK8>l7x>qOi^d^gM-#-7fP##?l$j5uSFVxY!wjglRcE{p80vZ7&GW&0x7+ z-#m5sD#tfIXPbGdw~(1*DKZwxw*8-w>+5S~^9ky;^>us>%lN{75l66%lu=KzXhboS zDyrr6Bro4^k&c!Ae)UxH&5nF(5|C^k|Ln;RRxzH{ozqs^B$I*5dObWpL1qShuH0*F zygH_ssUavdkH^&dX#BQX>9>zz>~TRH8yhjoz}HlD zkulXyaaVco#is#fCKa?tigZG%A{*(3g$RC1nL8(_jhq}-S?!+ZgeaGRMt&*@GMiYs zciPXDDu2MeIGByf&!?u8OO>v(UQ|r^o1%p+HGA(1wg~kR?GY){bv#-Ptn0&RVQv^4 zJ>C8i`H#1rB8QZFq&_mxgi+3&muo>KWk7;tXB->S>2hs4a@*WQ8($hx zLvbD4=8W^4ENL84gJ7w~@ncP6+3@VA2>qw;)Vlm#sf%959-9Bp@Prmy4$P;okLio! z>(sAcHXF+xnlBvrAH$HFiN7spDBNLipO1BysgSKurh)7M&*Z9VuS=fZ?(KTyO-^~m zJuSH^&Ai2HElQWo2KUqssJ^?@2bkz(tj}sa&@7p!30l>RWghNMtsvZld&YsZj7NV> zx990msW%=O4`I_?QaWY3$`-w9$#i|lic_`kL%F~oO?ATB-=s!_Tj-l@bY!6$aJ4r+ zc4=nU$3&vTFgBkD>272Vwl6i$>3>LZ=fxY%!i?21X14z&CFC}=)oP1^_eMnNbUWrT z%ve#F6Act@_uCDLKT*tZRBY;9_V~CX=~8GM$xLX~*t=Zzi;;(B&8F+Uk{Va#B8fN0 z*bkbuDN!5bTy1D~S-Zcx;{tjwjcs)W6a8*~H0v>$06f2!=qXZ@wikT2VSb>qNsK%3 z^+4i|g3?*P&;RH-2_}Q&LvNq67j&@ck+U$ z$Tk47gG-t6b(4>&i;V&387Q-%&f}S;!0_%Q6#3A&YDK=5$p*c2U2W%HA!xx$tK`yS7dF3PFDqe;ysa3ZA6~PpYYo8N)9QO>i-B|jtOmNP7UPg<^?wohFfY(2QPqNBx?>>lK!jHKP;SI z>?h~vU4rl4)pb2(w<8IEy}V>H<*_^O9|D_B6|2EqEi(H-9@`bT)zwuv%em?x7}-!~ zd>tMhJ}5Z2@mXY0;0E#MI7S${@i`My8PVafeS>Y&;=x=^$Xt!Z^2$ynE-`V6MIR0f z+?ay!;>DLjMY98Rq`nAZgY6MwR~Vne`44rgt_D_|MF+xSg@|a*O-E+$K*REnZaD)d z(H0cnWns7XVB5nVGbRVYr4CKXAZp}4h>zbyrCbG zz8z>wc!+v+dU|RGNwU2+xLbQt- zlOPxt!qit~rWf0)@s3Q`VGBKfC3EhRCJiXr1FU13C&pWI9-%NY)`h_EZm_Orhpb>c zB2KU8PhDD*@4k|$+5QU{h-@Btr=u&-+}vEQW;J8Uv9`W;QcV^4Uo_>J+<+-_=erM`*LQb=<_~+f^t$Hf37Zz5T;7De z?)g3>6LVVF{c&kAYRZ~04nM)new9c*K_32TKern+8Tt75PA@J#P*8mTph6~_7cw)W znmnRNk3)wO(ota`&O(nv@9(Xr%8W{W{#H`hSsgINl=TT^PZi05f zuGDACMlHr-WjYvr4mN~E>wpczuRo(IjH^5UfKa`pU&*|Q)P$X{;%{LsiT(QZ$1nnR z>n|ezj0Bi8c-Wf||4SzT!{Gd+T^|<2X2{M}hn2p<>xITv#8Xlt;^Deb0GOF2!A9C+ z<@5%N(tz{W1htAFV5|DLW8Qsoa4CAC>|wYAPHKXu4q@04CT^NndE-Ho<~@CVxG*-#f;FeV-UUcXd z!!}l;e~_N`S;8l`G_`T&aY3}tx5rRJaDJa*Ro48$$h_**k28)~doc6E_ z3e4s;kIt163knLdvtg;N-GaS;pyCp*hNUS@YQ1{PKb5iW_h}JHmSetF_s^%4V_q!@ z$WNYMUEMmF7@62PXBYpupnKX)mH*Pnf+I(x?GxV{h0vrM6Z<`mrRFmDWk)p`>B;n> zJF*7unq7t7FkoDnvgP(lfnu|elzho2@l<^FWR9LH8NsdiRbrw3UlNw@msDg+2|TfB zup|>s?q*{CQemOfPwRYe5T&d9H6C7IS{f;WFtUEt+)4F3?EH|KVh(y`*s^$A1oPn& zYh#V|zkXGx$eVZaM3Z6E78@TQCojPS@Mr9v*z4XP8B#I;ekS0@#53kaWp5bDrt)>{ zj^}a5!4|d@az#bOH59rzM*oC6SV$Z@|FRVWX+(>!I0qI&eealb#8Z=)qX{xNMu-+V z3k9FUe0HXB^{rL&xzrZuN^97@tKJgf|iuC6D@10*ReMJB))4ujb1 zC;qTy^v>=U(PZS5lnmmuS^FdOpLTQXfJ|u4RopI&+W`j9diMn?RQG)H67}kweS^o< zVU?o{3FBva$VK846nwu31LU5O5kidi!u>*eB_9qg{2fCx^V{H$)4xu*uPAwP?IHeD zo8m+TW8SO9B(_#raAsNXP(Mr{vmVc}{4~Gnym(-w+5gCZK&?{8=yVjE3teo%=Qbd? zmm)RZLPFs((`N8TslNi;SMbuj*=aw*m5Oopg}UjSeFt=AP-N|DQ~7*kLVDI?F>2J! zNhIB1IyLjZ0Rvkt%u=8wL{-6C=E9a3S2U*6V zLPCmrP4#ql`ydc(El`ZzSj1H3S@8{F+~00HsDN+G?1RNPz+mgNfp4lFaD1l}_dacS zd%wShrqP+2K^@gZD>Fr%DpW^WY_!7$3F!2w~*;zPhYghryT4k7&kB@gtvE?4%Dz z1Cay-{RrP}!U;0W2D3#JO$t4aN#AABu;NIT-4={^ zTF)CChJcu%7wwo+Auf4GYps2^uivsn#+PE8oseVvt~EbF*cVo(8Lj6fPP%zEI&nJl z%O@N~t$yFgT!m%ke3iO2(5iQX-R4AB4?ZwMZ-d)>GP(Pj`#xc`p2o#ZU<5)Z$DA(x z@>Nk>#nPKV@YS)J(;l}JuEa{8kp;_MXin>6%sRVPe>W*KCe1hu#!5s#O8y<%i8HNa z5I@V#Yz3!*(1-yHPkn=vP7^jrRY7M?WHUr8=IBBN_!o&n8mXNAe~~EI?d{s>AKua` z4;iD$;I0=h}$!!)9zgXMg)CiM28XTRdd2O~DxnTMm7w+1V!$o|s2g$A^EYAJZ z>`|CeQT#VSWancjh_3EHgG5T0eY2ivnPnGs`vc$5kmIS|WD@(iwQtD$qB<&kNRJRq zy~CK_oa|ChR)QCAh+PM%b{!{1C$FsjWQT8Y!dOzJmxt7`dQSeRJL2uvDo?)_5&U3$ zDlX?Sv=0?V_s&C7VP<&to&x-XoSC0Y$&#jzJKrw&O*yUP^$5O}Lne&93-qexH!?w7yqqi~;z%xK9IL7RGQ?V1Z5n6FX3$uO(O`{Z_}oI#&_l?JcgqsLquPB- zlGd6S=0%sdo=tKeaCw*k7T88w_Ghtie`$pkMX6y~yclK9C5YAGEa&Ff_iM|7oeu>_ zbN^#nz>F_#OAgq!iO^*)$qH!M8+}(YH#%t{7EgUPP?|4!(YU`aba72oX`AbwGArS} z9L?h6JQtaADkdSBv0Wb+iZQBw+mQT97dsP}r2c_Hr+r{%m|inc%5h)ZlyJQVvRq8& z+{FoUVGZVB2sVD~p85Y5MloCV*?2rybZS;~f}Nl}4e9Pa%;b&v@%jV3kP!A8ctZH5 z^?=}p#;ro#?YSELeRVwDZ;b=A!4~hH2anl$&tq-h-mXh02e~)A^f?8t>B0DE*QpIF z2JsM(z_Qz(v(f=If$2n&O-Q&FkG}Ho_W5xoQm_WwmK%+f_(Hq*GI+B?(&~C|s%y7t zojP}9kd>*R^q45A9(L4R%WI*R@BN-$Y7x8GSfafnON64XttzvM%o42AcOh?38_$)t zHaFj(BKDv9rR>6##wBCW;oXrHbG$G!o-~P|Ak*m#n@q9gr@~+_Q0CV{ww%M_r~bOP z01I5=b0fyAW)0IX-4c}UPVSj3dFqgV7wKTc3r0bdDRjUmoK@GlYTuUbox3~Jhp%6u zmV$hD>je6X_96NUB%-KuE=(6mVB5hcrmE)07w9{`KaLb5n@tyCwA`ZVuMx}HsYR64 z-|Tz_+GJU8;8hkuEfJ4P2Xf@vN>!aPZXmU4cS}w5vYPc>OvfW@yji&S&XwNpxIfLP zw!Y5_vH1CE{xqOMe1+}-csPESd?{+V5g+rMsBR4~zfNCVxkSVaB{B9qCF(;ET$w@= zplqp~*jtDYK-e5gxa4*^g=L(N!FQ;63Oa7M{dJVz|1*_=Ruq^)^`CMx^h-<@k;T6j z5=oZK6^n~;CDeAqOCJBkY8oc-RHu2HzH6eqggN3`G%NDm7&L^428#aYN$lxO>JUPv zF%w(}sg^2}m%5O~N|c@z>a$dlHpkXwS#jHeM^uNFyWdDVCp*3gSn|M&!Vrk~MX|oC zdW(;t=}f-tEEvMu7guj%pb@cs@?L@KzR{hA=K|k>tL{sn7 zFrv$rPr$>-yl<}UUd%ewaE~O603L3{yw!D^VQGIaOMf+K0l3L zcv6j;dhyk1-(q%w`NKQtmbZrt#3~d9iOBY2PkIh)Oo6?M;TGNTVLw@=QtDhcgD_?w z7yM=kSM^#0C<40~hgs#k-p$>sc4GLLuOEfzqe57(i{+0tLLi1l4UH`8{7fAKM?1JjrERQd0aFXBGQg{0?f{Z!U%jvD+S4s)SDevy+erv61x~ZgHDzd zfahU@JETbxtX?~U@u!`JjU#Q5LGBT2fkgtjle^%Psl(eZg>YxHkiA>3s#`K^UUQpX zB0H8xkPQnvKKaG?`cNEnqsC%}>g0_}s>_GCnxpjS$P?3h&}v26e$e0bc8)lQuKudh>@1 zPl_o`Mq8WY;UiocIKXONHTL^k3y3-FP>T zmuT&lP+TodPolvzr`R7X-Dg+CT7y;8>A0{bU1AWEXdRO~#Y55U81ZvQn2^w#KAOUl z?vMT_pmUJ=^p67iXFp&mVHX3T%aCi_&Zttq49d|C8$xj^9xh(zO6}u$ATuVZq zoUgw(H2_DM%I$NZ%T7pEXNEY;P%%3XEP8nkW8of)9pWf^A1+(pX>K_ zNkdwV)QWX-_PiqzmD-oR2F5?b9(BbuMgI2&0uM{`^YKcEQORpq$E&5)t1bH>C^&HC zwyQ#x^Ww~S?N`v9(H0~i6sqO`o1~94$)*!p89%G6Y2V2ktI6diL>ghHi2@#)Za-%! z%nc9>d?f5=Rj`(EZaW)yaM#n?s zk1$RzbTxpqoE~Mcl3GVc*3+}q{k3T3WzpBubQS@OJgI&L_Ee&(H)&x#bz#SON_U^= zL6f@-_DaRyOM_U6P`ltVEB3w`3~JLqT`I5*uge2MQA-jKbG+D3q0-i_Z{i+7!8nj< zGO`^AAT^QC>f0#SpR&?)-E^Q2e~2~sqs#ut3S16qs4Cs8$iA{D+3(OnChB*boN>v? zM0cKoP#};iT$q|Dv$QtcmO3Wb^W_#zC0x?EGXz;Mytyl#X?qBwy*t=298suAMD*eS5Q|MgV`F? z*qwIzq`LX8jW3t9EBkS(Xk1u#WcbqTo4b!sb+VOBd!MUwPOz2Jhcw@Tq`NFve*Mur zWGSnypQ5^v3xS*2&KtMVu*wE!+wxPb7~~O6{I+ zFl_ws@!FkqK1=4j_Il}t=8#L$|E(6`g+tPq)p0*xQ+esbVNsrg?QaPyJ#4T%?eo>_r;dr2m3FYJBx(G3mIM&M-%xF;y$(o(3%sDwZY*tgDADQ90g+w5-m8w5O z3t0A3=m5gxg0vxNQP&LyhgRdl!>;?Yu;OoH#cBZc8f#HD+O*|ACK_Ap_ly;naRMiI z`D>a!=8frddY{hq^0qeA3D2>}}_2;1FOrVE6IuW>E9!iY+ zz|}IvQEEXpcD$NJ|buVVH8ZIMgF4QmNtUc|lro5zi_ou(g0-P6e zm^^MS?BcW!X>RU4r~mGp1U+mF(R%68w-`hv?BVMl3h|r{bR=mH0rW9fbhWITS*4b0 z#%JNOw-A-eD7BUnlPbq*r64HVdQ2v4Sx^?Tp-GRwqoN9MKAew=O8Ojv!$9C^_m8{| zWi-%;$-ffy&&_Q_L{l7Tjc@4h%Z?ld++i2fs{Rc3nM6H0zJ#(mMf7#UEEjwIzJJr#PG3> zZbPBx+knTAY3{bN6sJ(PNtA_4$7o+LzyGs<VJa z4Y*#DSATl)RiM$Ho_xfEm;J}kY@{z^TN|y3aoFt}`-uc4TqcvPp1T z2lV`&LH9!YK>xly++?{|Ap~PBITsM`HcnN|cg~;MY{>lV+p6y`ow=}}>BT|ZfV&iJ zI92Sr<4@q!RzxuZqtz^zQ{^afpo!wMfjT5AxxEg-M{~N2Ii*%Z$A@q_Suj5@`qG%CgZV9+S!}(2x_EHvIW{ z?BuBfuH1=wjOreI4cs?Br~JnWUBejOhPcm-^lpzSnjIRhuj-jYjoiYvj2~~QM}f+m zZbw5RZE6eW7C8ooCI*lvYkh8HYHG65^^^?cDM-j^{Q(m_i#Qq-_ax)b{P=x|Hsx>Mn&1S?W!miAt{YYN=SDp2uMqJ!;nLFiqa{P zGn9Y~odZLIfDFw514DOrckRLFdEfo*wLgBqn#HVjUv-~xoX2&XkhFaE>qp$Pj{K{A z>%BpU=H8rZpBFdbN@BQ!#$yhGbWPkA1AsAuEt0g3Ttn0-KI&(CNj*~1zwhO2U>M6) zJc5@HdTDU|=_6@Dd?w9>iH}$kf-l#?=FFMOYwfugg*(Ny?>+kIXc3S|PsaR>W)I&Y zgUz7l5o2BO>i+tbm$gt7ugf~gs|EnkCgpGKzAarvBeRU?O2)TYMT?HY)%}?;h~mx1 zr~Bl4QOVs6l44Mp6Na2vT@psClJih9(!;U%D(YXqe&zV++Xoh;EGBrrmV1#X`YjPS z9Rg)qRwicBeqY<=&kMYmRcG4vvtAt^mhJmvs?-k2oJy>#;;Gu<0n#5k;g6)Gq`U>fzoBL!*RLXmgz4P}2Di3Uhbv9LZ;yLk+rms^T7V8IjSxiP z3uLJMzEwxdV7dLuNPWI+?PxN!!g2lS;h6h~4`&X!##V|yASKQdYQkD7yoc382A1Wz z-U<%L7%og;*ZY(&pbd>}*;^)>xZ3*vg3HSsS|v zt4@?za=wa5+nN6E2Tpu=Btnh%__drw#yaUyj~4*V^Bkr z9Lz1Mqg^2MtkGRNHMyyOKFQr;KW#9=PsluD5&zBA9!i&`%nRwy@VwAj?1C_!%L27J z+|D<+>EXX(!Jl?9RR8G%d&n#GqS?=eBMd)#Jhxjcw5um)LC52(J~y6D=X;61<+Hw_ z3@_o23w}E~Y+QQ^vD`ZF&KB+*?Vyqs|KJf{xl^g9(L1z^FOF5Y!LM4)yG}o|hO*CYxCi08VGe@AvCZGn;Nx%ziXG|Kn{!#UP((#b3Q}kPt>XNytmk zwe}9^)K%C(Kr28-p2mT8a9mPEj5xt^ZJWT*S8+mY}D0OrX4+# zuH=0mWqjayJ+@d7CePB8825O2|ImhybJ;Vl95+?m@OdPpaUEF~e3@&|y%=C; zmnSwwHXOtm5I}kN&o3E9Lz+!3q`2gU2si z2a#5aUI9vc3)mQxZiG6bYtoD*Zd)Q>hJ$?0qvWT&AJr8-rcv|{_Sdh&&FP-bEgE$| zi<1bBQp5O`tLP=SFF95!G;8b*K7|v0iJ5uX)N9l7yk_krj?~+eGAQkIZZBkJeuHMR zrsY>7v+7QLIv z*`pk^xDF1JVw+lpHu^1Kdw^31D_@0?I<|%~xfixbTy9Bi;I-_Med<36`a%pE5kzbH z&cU>!)Gf&Nd7=AscmTjjtU_A7in;I3XaO6%=$3gZzu1AtE*X?6hP=1@D-cX#rpg_3eyN%7u6FhHZMKD?6n97LOj9$&n5i3Wl)iNRq@Fk9MwcRv*PrZxBFu~|$Kj@e1l=#Zfa@HH&#A57))ZxVZ^@4cGl zzBZ7F(PC7lJSm;6Owlo)V&7zpZUxr7zqY|=J65*sW_R}{$8=TzzqY&*orI$s$0J+0 z70|>5?%qGbaZ*{>K$a(wRq0XHzZBG5n21umxmcg5cggk-8&%!}SBA~0Y%VR1B`K(P zFM5ovVN+3_q6YbOvMCunH~IM|TEgm1vlR&$HeN(Yg?`NX_{P}D`LGN)iOs#(;kxV5 zznI=O2p;;OBHPnFphJKYqE%^$r=E~7j6&V;it*ps|4=D(cLl5P;fC-)tN57 zI1Cj_rJrQ=QVec|=%^zhx@u~K1R?_jV*54Y7EcGPc}z~kJE&&zkW8S{SFAS5-1*bE zxVXFbYV}ZAU0o8Lx5f+b-NhgK46mv@|1q@IPDJe>^{US!E<$}Rm!lbYe^xpmMZAIfl*^mlPcgq0Y1ap%;jTqX zA5p_1x{`V&MfOj$`1EA15(*u@Uq3TOqQI8iKP*$oqohndOA`0~&Z3z-(e2mj-@?N` zgFgdGzQJ=kJYbhcV(<1=ZuEXk8aK-Wt?{O838vV7ND-g|WG$#&Uc6R-wfN2z^|ElvSPX$O_C_?j|__M;w?`9j7$M|q2Aky z%1#w15t0P2TkXqoy7Z0-6lbY0WDM_HDsToNX_)r;a#7^>Da{3x0;QgC5mzeFJJZhSn%Brf!-%195PjXND%0T=H-halA zK2z0rt`ukk0kNtvh8X`+d+RmXN8i@Fqz9h4$AuRP25bl?zdcm{K?;Bb1@21=mYdr8 zPF#VyC^(2^t@%hJAA>+d;PPx{`~aHtjo$drTpA)^A2j=*?KL6JJk(cPtm06^_5s>C zi7PiXr5+qOy*Hg~`q-Z1mTLB59fufMnO9uc7AD9j5fI$a;3F$5x4V4mD@^8g^M7JLk+y%xLyg3-z9ND||^WMQW=?x#NQnPO!1bq{hN|R_mK*5#c6}qx1>LIvl5d zqfu9LTJheqnuXO0)|840PF~xw@B7fdQ1KX>B^dVNJCReSKd8n|MP(}pooEopMz=FP_$0~yVc>5GIq+8*B zXE#b;y?9b)J6;j*x(@?jPv1S|&u)r&eKA12ND%oM)6>%v9Xk&;cse(M#WFTFtXU=@ zFbF2HQme|Eu2`oUEsO}_1OXD&RC1=hGUN;s6gWHNxziQ$Rpdcp$a>!`nFI~wn&?X# zTsib&brq@b5O#Ask#6mB+#G)X>0#=ZY#yGGk-!b>{c|7MiJl*1)?eZ5^72Sp{HVE) z*>(}yuh`gPuCA`e6a|k}TgMh1__1V-NddBXAYoiWA|iq?sr-VXsXxrDcL$*Sd@4{u zu>S!#{5-~SW~{05EsoKp(_5R_BxMHC-$}1SZ;l@3MhnG0zI{1flzlUC%^7`o%pJ`0 z)Pxi>c%7#D7tgf2*mthno0MH9k=-}yqQ!MP??+GQ&iqToG*@5lO!Q&ElAmy`??&txeLzI*ZhBs%uYxWKCQ{aL-PnNzDy9ffsW zUd7qhMc%NoGWn%;Y~=*S-{6E}xc2F206UE{@=G0C-eFd4CPC-KN)HCaaF$xoeSexp zsg^nwd6V^%kIroAfxGwl{#dr2S=kRZPTi+SyxDZ27eR&h7}mv$*uFov8kv+f9vo>9 z@bSjg2%QaYTeyriX_jlUSR=x9P)8hJT^2k~=Enj1dJ%Xp-GH}wh{&1CLiBsQk?`*r zU*3#06dt{NDSl;aRm@k*NVjW__Whx=%sFaVBSoB%A3v|0UaG>LkBrQpUJ}fwn`;vi z5u+|CuZ|+w{lrW!;|Jo70y&SrOz38SjujJq)hf1ayS;Yxt1DHl5PGRI*ssv;i8a%K zQNvXxs~cG#lTFzg%ic8R1KI)XZ*-Z}YuiU?xp0e3xT_Tq&!lQg0!J|+!qOV^cKQLJwUh{Dw^@sJ8VAGa?*aOxd< zDKpAuE){tv{oWt*DRFq}`3okl{7^-KG8wMp5PZA?f(qxBgU)59E8FEHs?$o{hwyyH)Nu#5%!UJ!I z9uvMZy`!c4t&wW3zUSX|Y^JS9ET&GBN<`iGMRb-HaS4%DYarp{#?_QBg9#0M$0WgS z_A(#JzG(R3&e@l;CRp>7Uk8aTr(1D7rEp}%RRaB9D`#-pq)}a~TFaQaTA1kdXX3N= z4mofo@3b0Cx$(Ui$W@|7ccHDS~GFOz^UKMhbtB^@;sQ7lyxw(0z^?3M7pDrzKeB(#198NAe zn7oZp%Eh2226dphnl+@vAqJ##4sxpeGp2^IsP=Xe&dk1e8aJA(-LkHqHOTmLAR1o5 zVmk@1V72Wj?yoedh5;Fvc!87f%Bfde$pn9PPZ#+WD+H1CexE?AIiG*cP9VAd~PHQ-KLeKoBkG3u-ydY_-(`Jc160TBV0|vM_Q6$3N6FMn1A(Mi(~_0us^iAb+oX>DQNlMD z-ahP$eu~y7c$yTR_e*WD$~I@(I&ZD$cYR(`OH(?Fo#DjNI0h}(tEqje^NqDfElD27hDjBNL*4@O!;jCp91*v(HgL|9y4`h=bG(_< zbF8G}>wx?as(#>XPP2x%t!_Vp`d}VEuUD}CE#cI8O6xO^lhr`~z7sE+LXDAF z3-qPj3hY3#t6OiPQbQMh&Hl-Ni1eE;w?2FL?$@&)j_m5NS1ySD3DvIXv>*vZ{Ipd- zYjk$7yxF7O;K9MHS!8C~F7-9wr*3TWU=EL*y;kXLThg(}qC94D*K?V-SjUx73Ogk# zC}zTW2gYU3T{dZ(O1CnW>S;7*C=6AaXl^_noG!lnA1=WDrQ3#AkGPG3qT)LkLYk4# zsjVlnRv0_fmiC2yWRCUcU?+s}WSFc{4j~bb-KJr)5*`*Kc%=k=;N1QH=5)oM>R!M} zfb@>LG_wxFR=E|q@ONH@4u|xZSxy{UQiL0>UMoOIOQ2=-xTwnZ`T=T zQ?uw7?^yb@6mF)!GseZv516;=MI>2UC=MnTb^A;s;dSs-Yt*f1N-q79nSj2Wn}~zY zR?Dl4mwsKyZyED#_B303wiMpWqCJJBYztZG`s_5boR@7?l~e+$?*OKyxyA)Qnd@Un z#QmSU?UsjYW{8mA&M-KQC7TdRtk*0(GHANezfpB&0;Y{7yG`DXEr;9JdaCGR8GWYi z^EaeMpxunqNsv%kLNMJQK*axwyYB&aPWq)#=8nfuc8Eo zQNuiY#MO!C8U6}utC!=|wC`WNc=e()b25k;uizrcu)A7gyhtXArhhdLV(;WrdPCuJ zGu=>IR?$*D)8gmNRcS?+fw+EWqpQ(4&qc*=W9YU%6h4%NKM~y=(>|zUZOv2QVUI%| zhiG_d2nfUI4;N^-^(Cb_-HB*SDCv|Vs)DHd#g8yAZu4S;dF;dS-sLuk6KJ0a7~176 zUmw=}AP8;jZ@)a;D|pi5ZxGFmBJ>voFev)Y3;F4l2Xv3j?(M$S2f6p%r*LSy>TO08 zL|!9=;?fOMm2PJ)omMK7&Iv|d_`fDgU=p4$@nH|@s>C36Tz?an1QnTiCk>tHS++lKEMhxWV zu&yOl^*#7))Z*}J)|Pi)T!YA^ufau!#{})W>hH?ZQMpUaNKe3rKv%9=BfFMQa2y;n z0TgPkZP$4*Y)4Cv}QzE<-Gt_%Ytt0 zrXl3)xOQK-I(T8FZ_j@I#H|Y_Qus7OkE?$nV$~4v0dD9exgJV>wsu)7%%6{KIPJyn z>rT-=?0I~gLi~@l4Ta0~rsb>1zTiSO6!9L`_FdQ(HbuE6<{m+IAX@cq*qf*l;MtKj zTOWaeSGDT6d#IptwaE}S2Rc&bCWtTwCkMx32`DhLo<&xFbNz-qO9ky$e9j?^UNjOa z#Q#ufO!hXw`l@_VeC(e2XG)dfARfXsWoxmv3x>{rFHsIJ-?O>Xx7XX zS3`FFUkh^oEjYZOAAiEx`t`Oyy8wx&=WxP*q5Hn(t6Ob&(0X6vX!S&7TVHTBP{RvDWn$tKIpxzUbaWKUs~|wY7jo> zQU$=98F`FIFm;3&iEicP%DpfMS02a?X1ZW!m|a&i{7raDO>>&9E(*hY>!S$lwH0^g zG1gH9_teXi$8imMCI?}uH5L{=3e5jsQA6q1tm5?E>>-Ql?nD%qyS=31l1<_Wt)XhR z)h^2=W!soc9I5*$hy3`h2NN5A{0*xulix+SYg)|WTFayHS&hvXJxv}l?!dUZs2=UT z{X*d@gvA+b$ziAQR!uPF`-HSQbfqyQxv7HkS}@7T7R~qbD{5zA&0+WZ+Ij1708U{f z9Ze$6EcMvhy`3vO>2PYRQ6W^8O^N^cva#=oRC>TKgZF67ptEk&eigpzJXUWi%5o9= zY?u1Ap}(TA=PPt(@gQj!ll1b4{a{S-*~#)0(u;Eoo-M%cL7E%AC2s7&^PKpC#BHOd zn71_dMRR!L^W0Rfz)OTsip+FtYMrV8o4%vS;e?V2{ERdH@VOF&Xed_`__y;d2)@^# zV2c|l>ieZoY_D9d6h^pK<-@i3uEdW+_SOcD`V>@TmB<*%%mYJ$jtBd*k{`yQ( zMmx;q&A&7|TMnYqxkC+$p@C6WzV^%cZ+rzuS0x{qPk9}|W7mI}(%gg-F4!mOXF^~= zG|3_Yv6^iK1za5=V~S?WW{^6g`hHiAPvw$sK3?KbT28`%C}vq)E>-da$mvyhd%{G^ zDwLy7E9&bB)$8LooG6Fd#vpW1U+io#=tA+BxvsmofmT2Xuu;?U88h0AU1WB! zv?yppuoH2AvZ4#Y_7egT+^l+j7cbeq+Z=6fmEqKj7-Yx8|M-JX@8zGn1x+Z%Va z)uLn}Yrgh(iG1xYCvmFhV}@;K1`TK${|2ml%e{A6_@oV_=XY(4rLX<%8L9=%1-F`ZK5VAVbyy3hf%7|KP&-|XNd#9euj^}BWW+}rvE$; z=r;YhY&3q=^I)lb>wD7n>ROpsi>~nA=_tRwHl1KDsnCw&Gu#=@FkZ~Sdus3C2*hD^ zHk*a^uRzL>j22Zs!xn=__vnQvF8TVC9f?m<-Ptaq!QC|}=?mS^cS|!eG-tXw@C-v) zC?)1lKEI#ex9E-=>c`U#_ml`zyExjaRXyqNeZVw0$Fz^v`?Gwi-x|wTN`r4JmGr@4 zV3YPKcl!xoXg!|6Gyj04zux@KWzjLxF2>aJXmFF{$7B9-wgLJZ$GWiXL0i4%cs7Vw z|8&w#rx!yAL?4sWZG&9T)ytjprbfs{Tln(ryEH#MIlhm3Np2={E$({*o!X8XlN!z4!<+$Z%XNOZOP#Xs1 zwVV24OyaUW6!cENuFPZbjo;ON$Df8c*|&*ox{8j*=#k*bw+d0P=4#+!T^I*QU5${D zpEM?@R`?YnD1TQb`ud+T5#tejzmlx^i(HdujI-d?1+1DhYu0&jM~KUJOWMarM(L?^ zV5oM5+0*UG>e-pLJ8+{)zQ509VFZ4 z@9Kz-)5^N5Bh5%Ma9()6zHnSSve09a9lDKmn{)$jEZobh` z`I+fZ*G}9PrjyuzOHCvd%0_)9W~b%)FeV)((JFuUHr{=GfsLLl%4CxFt$O0qV+Fq@ zAewW4{GBLhFsOw{Ebs1JB5`>$R^*OF>a0}9S6eH9tq=qnDACt#y4(>`IJwu6kR#V&szzw-1*sqxzNYU?%QCr!ELY^s$-w`KOt z667jw<&w)IP3lx2C(F%MLBfz?qBb>xKP7vJS+pDY$EjYTD|WI%5-T>bI+SSf1v_S4 zu0_wZ{oe!=@ORSYzY@ToXcwqPKhLRgd+2MMzm1;O!VaDX^h7Uy%DW>thZeVXpSAv5 z_x-!~x{?)G965p<*M25JDQfiMeMj4B2bN64cGk*r0kMr6#Iv^#QD(~AmCca9 z{UkRz&pZ{faTxC2iNRO-1BBwC+eSN6wdk_4G4ure`u#$~*Uf{WcmMyp%c0f0Q5KF9 zalP$@gjH_e8iy01<(F$?gdnC(3yrQ<1Pd(y>*RIJtR0Hf@=MxpeZe{ER@MsWg6ryG zP9VsyIis$w&MU@*#O^04g7f<3i?@6l%1NU`DSvwOEzRIjq`b|87NhRECFV3oqF?sO zz9+q7xDup!mXh$M6pkD~Hx7BhPX$nbx$b*2=a@Hl!;1aD#2xg)L>tvGRa*`Wn%IGAoKjo3>&mi?r75;2eW@Q>PyAKOg zDDLe=^w_fxsC9o52tn-1p^K&gb_c)GN13SZ*=*;p4d)*k0UCI_y8DiPLV1Bj`xdRS zPlAnqI*{*M7enndsXH9mqqdyQtkrC94bvT96FPgOXj?jazc|B6dH|ik=z`qvWVl6+d7MZard#$ zIZs2fD_O>QGc51uEei;x2Yvjj40hxT3?9GZ)F2}2r&F0rXOEqct!o;hR+M559;5`A zZogPYL)f(BQ__MRhbwW%^FAzdXCrZh2FkVjSE|oC`A47>_-Y_~yG6OZ7wN@NMr&jO zWjzeSewnoS?{=XN8QE}wdf~_SNlzFET5=SNB0^jRVef1PlD0Kv--8-5Jjt6g$ZY8! zC?bB;&FC6GbIp8_`I(Wo#OzOUMn=Z9LdbRI4KalXqWIw zcA9R}LU{ib7oH1Q*GVHda`Y9Mg5OMnAVh%*xr>WHJZSV z7VzRNcSEyI;!Aatp2$9b_Ph&$Pzr|bf94{lpog|EIioALT~KtTk#0GEi&@P{-B&WP z3!CpX>Oh0f=jWfU_)NCYe9rlitOkGpqIPdRrPZ=tH**4|KnT_!{1 z3~qxWbyTvSPM=bAqpuH<3YuxUF)EYL>5scw+D@lWTbX=3JIbPb>e8HCi}${CW#p|l zv zX90c>EWr>V*c!ZO6`@19DUfJv_EdcFenGZ!aBPv?U-)^R z&v4z>T|jfsR~W~UTMJm z>l5yNok7c4dVsNDp3tMf*yBf@=^K4s5>@dh#pw?0Hp!&Sm9~sUJ;b(buCC?X62v&p>*v(VNb^75Q8TbueU-c$o1gB^M z6vj3H36&2z)dVU4QS5yC&@Os8>hV7wi1K#{j z08DTLBd6S@Yz;J}cU;(i%#~+;BC~v-y?af-h3#a^=(io~`OP`0{AR5f1EugYu`=nw zm%@#giw&oeflu4Eik-S{PB2J$Y?XYrWNz!Ug$%{rGC9 zL8}Z3wfISXn8p-4N8L}V z>Soa%C9z&R8m4zStfvt?o%`mw*lG`URfE5QrqhG_4+8E5EL?kfs0S{Ku`w(PSxIq8 z>2Z7ul4~})ILFBMT=ofen+}uMO;d6;g`2>BDad{6Rxiw#p{RJI@YZ7cVs?ex;YI^! z*-fTV5=JeS1n7GkNTO>Z{8#}IBbgz_Vg^Lk>uRX%VS9`O1)O_Bz7!m zKYqDQ1E=3Uf#SmFEEGQsemlTun-S$i!B=#Z@INgW1)3c$`!;JZkKE2WDz=yj8CRTN z$)r%tshVqH_H-uLv|POZW2Ol@!tTLLu~vgLp1NEa01^OFW$RDh5NvA`WLk)*=g+Ll z=;rbyn$u0R(6=HD#{tDn&lD)B3qiPE>fNq&O0JzDS^dDqzc&-hgNn9A zzRY5)%brrPhKcfi0~IRG?Lqd5X>B|%qm)8<)85Ab_K8U`gM_yF*7%iQPw_x;2(r$3{cY|D5R_IvL!* z#JFQ6M)6J=shVS-Oue!4|q;uQ#K}nrX`K+T&dG zll{F`Jba{P>{TGcM`;aDJVG_h6bzqvlIMLIGwanx-00Z68GI6qXD={0_FHY9A18Pn zHsM;x1GOKmWbfUGYVu0|3piO)p!?F4Zz^>H_TH{mthTK>O{|$}`JCL}Qd4W;W1EE^ zBZ9|{gOTG%+>g!k=R&vNO6`?&mIKc`SA0+^AGO0CJ-T$-u@WJ5TO6byX^2#ncFSaM z06%)}izVbrp|3(8Xv`J4HVgAtmGW%#^!VWdcQLJX^^;&=&l@AP8kqQl@Br3_PM=&Q0KA^W-WX;#KhIO z*>H+~=&%v#NvDeLY)+3xO!JFP7J*hWuoQ(-6gqE*2F4!;gA^Ca*NX>^&;r-&3?Um} zG6Wf#u#%moaE5xu@UWIHJpfd}6umFt_foTmx&IEJqw zm@gJ&ug84; z^m#c80Xq1_B532r|6nXGB4P@Kr4_p;UC44l1Ho46VA2*q*2NF6wjTJyRBdKn$}}EY zTVW*({WMROHuf;7Y;?z773GNmM|?s{^mPjy2qzR1OkUU$CZ!zgLqdBOPqF6e5OE`o zvxF~>79*)dyu4$i(ZNS$B8#^8<;nK_vn?osk&SJ(=sF@IV%&2z&Hr2MQ#5I-1KnSh zMA*Y&QFJYWRrc*lFX-;pboFFS2sSxvy8n(Kbww2<{u<39x$lp@HVv-;qS-o?^73u8 z=f=^3uOV z9~zAQ04aj{ilP~vXhAs~x3iEx4Y|i*8KY@5BP|t0V;rISnj1|Rby8<*HX#VVvu|#1 z?y8x8N;z#%Ma6#dazJZd7qpqxO?w#kPdE`O#)al5F%rTZv-qReVd*>+m8N@Wp$_*i z6t35|`>Ph+J2TS;J5Dm6GU@K)dtv_xOdJpjt)ymp@`RY>%d0s}+gEvvZ$h8EF+6|3 zr1kz$Y{`po{P2%>u}qXIwG}{sXtNl=&E3tOjrP6M3xgQ`>SKU~SO@iH!^yRuE`c&5 z7S;tN;xz7e?*M8>iZ?C_qvDCf4+eEz1PWiI=li}vPsoNlgxcWdJ+xfh*(v~FmOXsT zSK3=7+Hu<0GM@$HA2JusNwTpa=cYy;3b(TV;ZRgD22TWk_Gaw=v6+wopW_c^ul$y7 z-}`yL@J_<7;&4V|_1VATDtNKPUw?jGt<9JH7~%LPk|()Sg@w3l*(qL@&cJtBonjF^ zs$f%!NYhZ;Kve}!8t~U%^Mr8LkWRD~Qq@tr(1;VSHI6{x2Mgaq4ZDd>d`V)&VL5LB zbsfhM?hz7&nsK<2)BT|%4RO`|4;NrhATQ0fN|w8IpW;m}O+P2H*ea>Qlw3;Yvz4k6 z<<;|8o-+{33zlWZJ0+r-RKBUl|7~ksn0|8N#*|>40FfY+OzxZ`kubwzYd+$Pjp{Vx ztVxepX6F;iaRit1D5JyXsl>bDSgiRo?eyw4g622c4tod{ z1gDI23_*FapO8z|G{r<=cEmkjK`>Oq$$BmrEWMz|5GYCqz3HygIzPdFV#O$C{RnMc zbpmStV+3-{4P_GfjRQwQy7*3ws0S{=#r+?ROS4C1D16NOv77i(IEsYTOi|xTLPKc- zi~8*%DJVKnlYKbPcvy*5FZtGvEqPu;mt*OYqExw)7zS+fy5%2%i*@^WH7qokIc#*9 zh9k2sc5`yQ$rd&`7{f6eq2>ZrNj=MqgwKvQx*5ZT&gs((q9yus6Yf(<{TZ8t4>ugFU1HF z*J~d)Db}4%DluK78dtde{a!BtwACi_VCdD8&?y`&`xiWwm6Q#m8~Z#>(f0{%KSXj= zZ49N_duEH-I-31m><&3s_)EOl1hcLmbUl=c{b@q(k0_(i=LG!Gy(LkvF5n{yZ&* zv&T*JcDtmEpY3?nkE1dH_+}_{_Ofke(_$_W1X#@3^pauJDjksTe!EUh@{=_w0=F#x zP^YIlKCn)iaD+*i#*~biC#ppdW{e47j*crTT>L_3#URHz#zXI?7;jeo!7?;BIp9vu zv_hCd-J(h9A+F;JuW7Q&@G*F~zjmt{P6}hnPLe@Bo4YNao0$%p zhDS!K^iuwrsJF7WF1hiG7Y|<>lfVOX)Eec3Jo6n8Qpa*BmBy454|qd@6o3YCv*mr6 z3ojDlN3Kzme^3_s8oMz=)xgC~EMK4FYpE(2^q93EI92&0p_Ccdc(Csg#4}QoI^;-5_+mRq!Rd=P1#PgC(dUaSAb&Xe!+mCWT7)K~p$);&! z|9iCFc<1C|o-ni^Cd0ohYAgEX-mGO!Oin`R>bsc)#p;O0cisWZq92i68>$daa}(Tg zq65;Q12}eW#i{Y?VYSL4&A2iFsP2Z4-esfh&ZeS&IxiLI-`X5>YOK_b~Z-ngY!&=9AF}aeJQhBe||R90*DX%G4h?0QbVJycjY)w z9Z277nE#OQ*`ro=pE%Z6SmLd(UPTs+N;uWpN_ddc8C&M4y#7hq;Yubxbp!}q1=dBU zs1@JMbGBAv+-l(jS{s>zuO*vMBdB>N#=)SD>>Bxt#oh@qU8z=1fCs4=3u_eR8L})$=s34=*-V zV{)7OSiDSO)T`CY%42!JWQ=~N;Y&J1c=}4iwo{SYR$u)6XU_Z;fXr5o)6weWUB^8UEpE)mll8`arWQ3Vj=e#X0+#e&*fo zjsNsA)&|Sdf2&G-^~72|P?-cjc>Oa1^`P0MXWaT;vYnW2X`p;{KF}Ilw=ld&^AoC& zDMhykLM5l^^v=9_y?apS^SFc0ItqHs4xMm<8T$e70c0JdYG{C}mGKRh1TmX|{D58* zDW;<&;01)L&>%+lMzBQgT~eW@6Xx-%`e?UfYOt(o+`|D(2mKf?9kL8%?kXejaO99j zG=T1ltWhTb%6us%fHvsb?fi0}L)Nz^$FqlBX}I$pY>1^E&FSCAFR44o{6)TroL-Un zGyg%XrySCqjGfx9D4Yu?UEg4q@S8iQU)($%Uk*&sj+eL3xLu>{^#pP$)^6Zlt;aMk zP2KW^Up6)2)~9;tnzAPoi}WRkT@Yl8x#3fT{RlH=ac}mrGx#duQ3l|i;?pFM?+MS< zeqzemvVb?<%?_=oOSi%iY{KGPd#8E(gCK5g{Oqik5TD)wN8Rm}tLSxKGjykX)v*tI z)OpZ&d+J7vdfvwWExg%OaNOuN?!2X{+Tm26JZ!j6C4p}(P2Z`R)JWz%7`XKxB$ zjOS@uTM@DPubv9U=PT1)siVBh@rCwtD`MARYM+`E0Yjl=C4=LNI@*8Rt2I1EyS+cf z3Wb?~V$O0h+u$fD5Bjq0IMZCj42w_Yx3&)Wy$VOMJ}$P$eF)npbgGx6++tjmW>Vi~ zRDmnzK*|NrylTk+Al2@&VgB_&-pLZ9(?`jMf&;af)Nz|q1xAHA9BQN z^)mI|;Ys2FfxxJ3tGLh5YNNc5KoWVNK`sY6jACQVd&$? zAX>FDM@+H&*snDQ7ud=nHVrOv9A8xpflk;ZIheadEThO$p}xh|~F-%M1oEXv#*aY$cc+>L|T&hGerc)Q@)6$)G$P6EMit zR`tB@k2;5AJFkf?7#;MSzx-FXKXyK$>(8tg&ewY#j#@gS>~d3H#EV9u3vvXHZQ&fr zkVGhaSho*Ss%txhIr2w2)e38=Kn(do-??!7=-5Nitg6wrZWKmWpl)f zA)M#Nz(BhEp-NhRhPd8-myIgWI#mnnO>_wcM$VZR-IIOa7}=c2JC~-rtR}Y9IYj8W z6yQ0g;08dAU09p*PK8l25PF;i_~WVFv|!*Sfw;XHQ2BiV?pn?#{YBKGtr#&H!DH1O zb@jXO_--0u_EPoPanYTcEhd9>R~Bk*wi!peNmBu@?AtwmvBs3v?;Ts^JHuW4&kare zYFKs$&0E0)?4`}!SJun9)ntnuzq5iO!tP~_f}C$U#$6A)B6rsTM@!jfeXXG;nU!1W zarUlhQ&q_ocG2Beqgq9O9l~7%wksndA`&l~^Gk(GoldJ*91A%T!CFABT5JJ~x1QE% zWYfd=^b;zvmkM_H@nZ2@C-F^=9c+o_59(7#w@P9 zhwUS)>Sv6_Bs5BtCE59(X@Og6<3Z15*mU(ZAxW+j}~_ zPm>ZC)UAnbG#(dL>q*B6o4$>(rFh&imY7iCcC&llWp7~8m1UYdM}gc+KD<&oM`}aQ zjWjo}y_#@&-{`>avB&kjC&5QG*{v>KVvc%B;5S(6e8+(zpu6Um}c+~}TO(n+*1M$m%< z*;?M!Ay<9b92MWFfF|+8SYR4IByjiqo@1Fdf_1ZZKu)_v>+1^&4D-@BJpI>=jjSPW z?liGk2s-kFok^F*TneL%W+< z)&HUEt)rTL-#=g*l~O@kP*GGGCS4*b($ZZ5L+Ow%73mU@7y||(pfpTk#ArrrOhj^Y zO*%$zY|k4%-_Q5?KIiH~P6md@Ewar@eQ-Z$<&EB#z5Q z2syP=Hyv5Jh!Z_pg6IjGZM$nZxTc=iNd>npwkEExgsJL=MGD8n%@%uK5VD^gc z9~k5MTr$^->PA|upmqPJJdS=-i+d-*3owkY+lsPQj`Am~imHKV*m%X-#`rO{w2oEM zBC3Y2`S#9q6{F!R-yc;NER5EQVVjB#gXtzMi+cJO3x7FSLFfe8!QuIMqwrf0_3E6< zpkt$-Z)9lNYPT!Ray*~h<5rGuC&;Si-R@)o97h`NKrA;DbM?aMj>qp*buh-4cx&U; zI?G@Y-L{|u2u+T{X_VI{NFJho0VVm{rZ;crlID8Zxh{t1&)7{msqq0yGq&3^JfknxhUgCW$q}){ya< z&jL3c=%Bv=;}sa~Sel{avR@=QdlowIB9e92ZG;MmVGi*Yz~0z0<|*Dxb~m<9qU|Cd z3CXdeXokbr<*)`;?R>RITUwqymyKfbnQeEug%*1XZ}XzFXC-wN zFi)p~KWGIKpX}BD`USGHL?Vu<*w1RD1Z4sj>}1#uCS&o&Kr#QLYrh8dm1JX*&Oz@e zf`3U5=_z53z;P{KTkHIZqj#E}E`lPK1(RL&P3T1(Z-G7QM-{EXUh7~{Wsuk~+yKGa zsh?(j#jxOl;v-X28EY2t@U+nP=oVaS7&WUxuxt*)wWle8$Ry;XMReY6BNmoF_A{+v zlJN{>%B^1JV>ldO|Kj860A_Q@&WouHgCio2x&Ahx!>0ySo*m3eM&ue)RA_JwUaV{d7VRgJ}pjC*3` z)PY_#Bk=)1Gmy5uG2jAh*e!TKp2jdGc7wO%=&=NMbLL?$H#6p8cehU%7Alkoph!6Y zkW8}k#eK$;ClbOQpPt%h1%butl)dj=|M0yFBmAw$t$gU+BD4>y9tMdUgkY|xtC&uF z#kgEKS6iwk4&}aK+xmi7(@mZmURqtXP0|p*3+5Fjt$|Manz`IH-$nqEcei7*Qk*GhB=`(p4zcHZu4(R&WQ(&d@_lDfbDj4nW zZ)t9Rv`o?)_Z~87E{X7sx2YJ)fx|<+hRmr#XA+e;t*xxy?#0td%P3FR(IGb&!uo#- z*gKXD1U^F#g;YC90oH!>_N!ujN4l&I*04}h%mu}dKVoUw#=5H=tf6$G1J~2lCXHG> z30b*iHO!HGmIRwl8k3b_ivR&S!5|p$ug5%`50DXfqHk%$oMe^V#n!izkHh=MmtXi_ zJvFs2!b2xsWbN*MolkMjpH0ZpRJ$tUWKlJ}pzyDmVgHG)fn@>L{WJ>;%d3kkX2UN_ zpk@X#AR``)ECA$qBK~r%YIqTW=RW}tdFU2mZ$-C;FQeboLd+bOFgT2ORl0`j;7!n= zOSPX1xWecTjgo5KwIP$Sv9XKqv%MtQ#KIv<%Np4RYX{?S?5A$qKTAu-%hHB?;+_bO zcy}Q7s&X*WrM_3>$Vio%hraJ&!S%u2Xqm{#qGUuZRq?OatXD9dKq>*LoJU;^oX zp;+D^IfuOMk)sUMgDlWE>ffH7+w0o2Vnia*^h3Hk(HLc7U>Rgq636>U85wF;T9Tq4 zZf$l+Wk43=pCa|<8-xxNQP@{p6Xxu4ss4un{FZLM=0b9+Wv5B+r@Lxhc*2jWDs6ry z$9G`QT|3XWm)!3gn-?uOPo;O;R*2tD>YlFC9Jd&ftYRWB?Ntog0q=s_J}ZvEQCppnF+S7#WL3^&OULTKQ)61#R~B1V1Kgd$$8b9@uvxNH zFzMX(q3lXAT+p>)>jGAu$baL;jh#KDVuexQz$s8;qyB;i>r?`+=LngAKV+d{im}Pi z>oOIpc&M^qXKBeL$1Q$)IiPqi)`2)G`TOBHpsk5;t@GR^$K+Bgj@-){fI9IrmHkO` z^26W38hjX-rOwj^Xt5RjbnOp-z};(gHz)6oG#S|d!drf{+(15B`P^&w<-RH}P0=l| z^lx?Z>Wn;ANAny=9mmUH09aHvG&V5Bgj+wCpVF?M0P+ojsb{}6qJB8MGB%I!QAL>L zmk0zqV+jD7&T`L~uYl8{lX+__Z@9edRaZ?(lmPN0KdfaR#KOzJM870tkSY+Yzw|=(9y3r5Fp0coj{+vGUn#y>QHg#^EVRzUjD9nB`;1}U=Q+i zE@26qvh>4V?zdxBq)GdSc;oF(_MQD*>)23t8qE@ChelCQ7A>M!KfRg^5|e?uhcEtw z3$6@8w3V_C(I9O3%rkcDlDojQun)BG-|851_Qjt?gR__o?K_8$$Hvud)Jq-@=;1K= zg`lDpmhfA7=vSh~F#_@lRa*q6{rZNw`@ymnd~JUQd4Bebh3ixpL1mUD=@4e|FLTOU zCv$=M7LWb*@HS4SbPHh{V$TO;VYWcHaN;Y2wCR4B2T20<62=vj6r}uMmC0Be$gx$9 z&&-b_JPm*Tr-^Js{Z}On5-ATH%41CAeGhl@2taGk02(?52khCiSQf<^4=Yr)1eb5lUF z08(Kb#Z$Swvb@Z;uJlg>DnCOt>|pGdi^IEa6CZ)qaTHXu+RXNLJq-7xO-}!6f^FT_ zf0e&zKURC&^F|m$x#IfMJ@*_ghRyJiUKE$uCt1 zyOdej`EP*k0}GVL*1FIA`Sa)IMP5LVdKN^JRr9LPD_)6k)-Ywcpi;|Xs1`ohE3D3` z=DLe#ynL31@iK7aX@}HICvOU?=Dlq5Tq)CiEt#@x;QRnLFreh>x}jRqsa~YV``4nj z?aKvX%M8ngFE<>(LYpTwK9&KPp)$0nVn&bjDhx3g=6yjB%az z1fEGOtEZoz-^gnDaf!1LE7zKVuV)ckjq@`flG+w^!Bx;xf1os^acdq>gOEPgVWYp# z(i}#uObhM@)0)dA^K`!36^#nhIMx&?E=Fg?Ac|u} ze38lk^lgfvbLf?k>~bnat50dJJCFQQ;AV$8sYgIMS)L_uVgU6XZhaom5Vrc$EH*EO zjW3e@?)fwhVIq)(kigxvv!emnW69Md%bAEN*~mdh%bR?j8^iXZ?s_tacoFr9L*LQ^ z<&d^uRV{O%vDEk{v1jwIxT*wkA*-_M{$giT$K6Tkwmh(|4At%VP4~W0Y19~uNL^J= zcIPp*MR|@3Eb(xJ$43o}xDWAzG3gz2rp1cTB%57~k1f!R4xL`bJuoEbj+dZ+Sx;+N zP%ogwqY%$(FaOBu|Ac3^C4?wTES>Y!c#{2>>($kOf%!W&J^(Kk;fkxyDLw+UsUAOF z;drsZ3xoG>vrRgTUl&fRSKqwIBI6BM3%@V-Ut+3;zSIZYnc!CbH8}WG9GVwmm0i8z zu`B9_0btvAV0l^rO8A%S1M-Xn2tV!+}UV}bqW`U8E<7L)W1peiE1D@RpU_8zX)h7hN zE{(cG$C$eb`B32K-pMIn@$tl6v)=bj6DS5}oafQb_;%yz`ZQ&iqq7O|Qc=A^BS{tf z`NH0pOS|DMnB;~^_2(?kks(J_$xwT-oL!6wjku^t~<_18QAx4*J5JUu8V=n-&9 z&us~3Esmzgfy7MpUZ1hL)bvN~sQHM<9Gux3x?lkN;I^i6s=RkH1Y3pbeh z(v>~CuTy5ZVPr;A%_Giw4sS*-g*}_qn!`W8ZzI$v58F6uP45XqS;?W}zSA1xJ7w@R zBxEM47rV$DB``q)u^wn25FAk6n*AFv9?f8%PCtJ=U z65pVSAWx<&w7<;}0w3S>5*!G<@vI@)$GS@c(h*$0`siqmFh3e0Dus{_;_JaahxZ zo`vmt?~)T+K7kR*pP|;>u#sn=mxW1ThcmZB_ z-WQ&wWcFNTAa|Dd>^@#98$U`tW4h7EUeu^-2k&1&OHt-l>ke%@_uqj0x3x)N>n9a2 zeodX=5+}oe74VCITA==c0bOd=0gzzm*IlQ(Re3n9V44C9u)NV7x2c)`bc0H6LvR9 z_I7KzzcR=Nc?4HUi`$&BQ0+!Vqh@5SNaXzIL4SNvpH#_i~Mf}f6^R?zLdI*GCd(ki;^Ge zhGv2m4QWl2A(ZUnulxHhbV5*LM<8&EEds@b;tw?j)0Vs|L-BU$`BrehqOf$#U}@)- z@LMsd{PDxXXPDmKJlW3qlI>WEO38jA`W211$*N4T-EWC!z zgeFZwBpJ?R`EFi48kWis3I-n1@_Uq*S|x-2`7U4N(Gy#N=(_)`ZN9NoYTC(8-!aUD zpNUoEON_dB4F2vQ1R3#KJ5%c0pcC(Jr_ynCdMw9wabgtk0p2d+${^ z__pu-nRq#hojC5bG<&i9nE@h=r=S89*IFg%9pSZ=w$BsbW=#1rAhk2wCV7~hbdiid z#ly@sTVS8jJI5XzA4B&is;Q;A^E9dHhSuDHr(h28r)7oiG?UQo1*yiGB6#X^ltQpbvb1OQzgA;_<)kym3{Dsx?ph#=X?unS0jx8$Lv1aUKFY32}= zJAS@56(q5D{3o>*JR_cQR0NgsDTucli(n17nL6$*ro2Di!WyyKnWb#mtP6#;&`M-> z$L=jBbrN!N_rXsjqmSvQAhM58PIL-ZG}b6ij-W1?K^LZG>%U-5QvUWx4V}%<&_Eh; z;K4Wqm!>ha{OVBq7i^2QPxhrVryTxa@Kys-?%sdY(zeC5r&n)u(`aM&9pE5#liupI z%uyg0ygUU3khBD4Z9Qs`vu)IEwJV+QhZ>#%CVzc6+o3;f_I%>g(T-&=_a}U~*5?3} z)J~4L3RlmNy!tkR(?1gMnFWS75;&wj3Dk^xoacZaeZ3YIx@z(8J6Fe-E5WO->1B#Hl}> z>C+1ab$~S?_w~n+&Vy9rAHG)=@7|2QeVg4n=>?$)D_7H_-*!S7u<`-=jL0Chw9Wz~ z8Uy+BN&hUm@-`5%&e;c=p*r@1WU)n5X-aywfNgei5y5S%M#oW@pPo|$GNtFP7w7HW z{c%5fGbp4(=q+G+ia*H~$^JJcotG=oj*sir0cNUAXm%KE|Gjwe0f1O{~tEeJo! z|Nk>KG;SQq02Rk>tUCE@X}DSI z*hNPHYsO9ia8!{LU-DaZ9{5XUC1BYG#(g@bDqS-C^GlYQtuwI3=mIgSe@)JlNhZ*@ z!D85!?u=d4Ww@U(cuxM;zK~J*DaTt5S)r^Q$*I~N6Ox*;?4WNic$wyw(OuWVt$&55 z9QYLHnF;FhGa2-K0(PN`R|WLY@LRBpf_2a{=Qo#quAjbbQMW%#^6nehX-aM#+3uC~ z=%pp?=FMm-L8nsx-MV7+AromC0sviAlc=hGd%TDlJrZ*ikmFY70SR-rh6~(@e*8ta zSWbdw&H&z_lL(}&0Jl8?Cf+(0>GA!^cRf|`o0ABzLCih-Fqk5HwXrL~yN}MLNqb^k zOp@HUZ8}n+yq~UX6w<@?2GP4sx~63wa)C+BKmpw)=NzicB9}Id?Ak?ahns;|I1M<8JdVh=#p86Vw>=p(>l9IPPNT$HzI3 zzMJtx^DCCmV(o#%P*)*eR?cBCSME~m%v*wsUE9;ESGGt(@%TS{hIqavq9~5|w!88O zOg0^HPbooe2CN^pR?5}C*N@|~M=lXW@iFD8X7*j;+TX^mC%!30|CI872Mcl% zlK0uDr08m!sV$KkKM3|#PwhJn}3{~l(?U&wzbqTZwRC(^K_!_Urs82_|;5sj_A5T zI{ELu^uSwfTV zIjYqvW~#t>c*bF_P=#p?>kg9F2k=iUJUgDm%Y__CGnL2SJ(dc*?cr5*uQynF!viwf z#V7`4CVovZzDue12nfgM)sXJNz~HmVGBJ^-0DNW0*#3t2h*hZFSiWXv&*ula;l%P2 zx5VJKQUGBbee3<>q$g=9Knw8u4#llB@3Y;D&H>tFY)v)qjFIfrSsMpW5OvrYwG{pw zBi;_7lsoBpI*A?#v!cD&FOfGdUI+hWpUoN-UB=ky_-EjP1L!A_3e$C_z>^q!eG6?+ za7$*F;Ju7qeTn0+dRJuZT<0WeSf2FM#n;Bqve|n4^X{L^T+`_=a@^s}3oUj`OR9W( zgC8Mne;+DoM-BV4;3x$i&P7ZrL~dAwP=(YTwAs2yW*uwQMcqABor>B(Cfyr4kW?T} zz-pI*DW&9V+?;b zTY}``8m*#YAV`ar9*|`ptDTKNn+>HS6zVrdAa$;1K=iUV5#-bPUUEHEpx}lB+O3{} z5yTBEM?t?CI&sMrw)HwUp*@XYx%xiu)ka*O?OXG`j1Gqw@kIu_S!XKU(Pik@n6~k_ z!`5Vl#U*-Jdk(ufiYFi_EjK|fK4D!xxzxa>a1144m+SthIV3pnGM?gf2G+QxLuc8W z7r6g4BVeB`V`mt)3Bkx(@tB%#N=mr;2n@>~(B@-**AV7|(2~3#ng621iFP{Q_5Q4L zMr{CE#Fz@Z>tLO)!Yfk2gR6-A0Zk?{Tl~a2PfK0 zsLXmm)`^d|1irTS8F?R$VPp?UQ;5aSON=g(KrAdi^HXh6m=g4?t)Bmbn9^+FwTNUw_s$~rL& z;m9$hrDVn3j9qTOc{BOFjsVC}a*(G>eAu-h{t>lQII0ogpLuS z*AG7-vHl&eCnmVNZAEQGgWJZ{9RC?Y}oLgX&fIqUlgC3|Y8yIg8!mGyk4ye;6t>cy9-qK&hZ^W|8{N@@42qgL9^Nk$@ zbs0yDY^<(5XXmARq)7Tck}N9F&YtYK)E5dfP3x6HWpvB|xp;DtMDCHupb0hEJMdUY zVKLNvDM%*PNKVFA;op7v)w~a~6?Jzs1~`^xRM`DUr*l_nL88u|>s$n`7&_$_iNuLi z2`s&cIWLRNEh(V|AbYbiaZfng+s+a&v0|tbBpF)1=kE?36~110;hf_6VXvwAi4zh= zgOqD6{j{6_Nr+yKB+b1qohWpplAHA=x^s|`To=sjHQ^y;cW(PQ}N{h87-UxT7OHb_|lSMg&@PYgU9dE(^EWDBf z$d+~4!fUhGsnU7m_ZgmE)n>6yith5Y00*`{sMIcrD;7Dmdul+^J=<7ARO6ccVx?3i zfZ*nWAxGI7BW# zoTBs55Faa#36n7ILNqzsCv<0S?s)Fdu^LFnvJHd=9f_AB4&8Xx#llKWTA8<2ufrD9 zLSS8`I2Uv}chF@a`DURe0y|T}WW*@+Kym_FamaRFkT{u&D0B=lFeTmTpiQua)t5OU*3iu+gnt^%gGEQK1uy)VOk<(4cB3EZ$J+f{|#}zirLVV z;%DkyuGw}DejKc>9J~Ue6Bb|z1yW-Rc7>PWY_E9iqYD)a=2)WhE>BE2*IJOIwHdFT zQykB))&XaS+@u2~4a3D%8@caeax2r}|4JS2lXr_a(f7wlq8gHn z&_&GuF+XsC-Ni{oSLhbN`??IOA_a97_VndF3HXC8mDKWmmnj>Z$rt9U-U+S8#N@zT zd&*4S-P5Cqj+Om;u=$WO`Owf#7Au|tbN>rkfqM}gHN#JRZ6s#6nOED8RKaO{>u!ff zOL&i#g5d7nW4Z}Pm!60|`Hu-hXEZj)E~EwY+aINKmqwBYqvA zG$AxQC(fDC*y*#LeYm@RewF)0wsXm2<7gm6`n`V=XjT@Z+01R6!#?trBBCdJrh;k{ zlnp-odEJMzRzg;_Z1~RHtItjNbh~YC)E;Ix+aA>x!~*aZ9BQV5Wq|tiF92@>lEoFi zm60EEykdF6Z#d62@e_>Al5;Bi3$X#rG^t2}Eix%Xp>p0<6w8Hq!u zsNM~nx(CD3Zr@6zPj78%-`uqR@DXnUnk)>5bedN@U& z(Vg+Ipi8AKh~r=Ft}{oJUy};8dFv*kcH|kmx)c?PSp;CfLJ?L*w==(~McGZ!U{9&K z6WW*=%>_q+WZOx*E9Zb5?A3Fp?<91Xeu(=tnmJev*B*DM9&!=C1nON7x$f6GhKKx`R&9(23m` zz6EenFyJjSdZ0$^6+}-DUI79u^Yt8Am9SohUsQM*f?`bi?aL>88{5i*bPk(9%Tiv zGUKiPBk$Z5{S#oG*+cF0l!Z=ZosG9??riybx)|7>7jFX1gjywz8P%SS;eez{hiUuK7J?Kc`SO*ZOP57%> z{QmI7gd7bYA8^+;Bmf&1?$%;M^6NXpv(_k=G_}TO91Kw=lWyk4?Y}(w;B!-ihI1AT zsrNj5reM9k0GJAAjCGvv(xfE;CzYAuGDXgbYA}4`!177b@|u8M9Uw+7<;1I z4F~8P%dBFucszH>Ezxk*S8g0E{RTN@H3@;r^51`cWpWb#koMCJCm9~7%!!RD5 zptLQED*|H8s4bZKz>t?Q`h0Z9%U>^3Y~fYBXb0Trx9`&MF^6iCOxAO!6<_4&JXley zJJH%Y9WXb>=PF+s4#b!~;I@;YP2GZ{qK##8n9v%t?z_sG_+Ldx6zyxbNk|MGI|YS*;A3~N)je1`wTz~=!hA324R zs)ZC84mua|yikmaysM@;>B^R*?DbRL-yD2npMf)&`|zttMsFBb7AD^@?u%SfPSDPU z3^&Jjn_HAQhI;4j`9bOKBdNN3L7y#=7W19C&l1%Py&>Uv6CQwzSNmwCP9NAO19t05TT25zmKA%Jto9T~WkDeF7Y|6?jplkq z-}<1fwH`QWeJ5zd7GU*=W7E9kn;%G^2)EuV3oUNm5Vv0I_p1y}A=T23?j|w2Pmyxe ztlL7&y2$LB*?jn9B1J~J{tAuu^yb1k%CX%Za+6KI zG7Ip!T^5w>v${&@1*e64Lc&Ts1T5TJs{=`B$^vzfkBvorLX4(-9Jjn&7|K;L91?)e zCq&EC9#7o@iF?1h=D9)UOOBJr?AhqM0Hlp=i=p9Rl&+TCpc|}ePowNa5@{l0?O6w| z8`?<=`U>!hW`_5u3wJvUkKC6}n2~k~Y>GRpf%0NmX}fS@idEp2cleezkpR$@w? z4=R)FD~C(r-KXFM*AcF>1+`myhLY|M*RG{sEk?|AI++pqnYTw4re>t~ZN_~UUayYR zm5Ze|xKXiZc+vWp*D@2&Ojiz}k4J-0ePfQvr!y+lagIV@twJVj%B1YiV$Kz5T*bbN zOwwuZQBS5Kfw45L3{O@|=|6l<7vG|~qS!08I$d+_MlM+wl?-GuilFWIe$_HFlE52J7-N53<6#}y?Imahw7piTi z==EKlF=JSB?Cw-;Y7-Tz9JP8pf|wf~_Z=9>KyLZhw#=asKBpa5>ljuLq24FChK*Z5 zq8S3TYt*W2xl+>4#?^$+3J52hV1x6=JiH+P4|eG1Ts7a%z?ohelohn9qoV3N(rxL$ z9{>i|m3KBAkJ$}%*=+tS{h0Sagp0%D=RRV6W&=3m8DKJ(+sdnf)<*4Xb|o5BX?GEm zI|$xFi!wudpdJN%BtIt%e~c(bK!90@26E8;H1p06kQ-vBCRk=-e`0iE&;bV`Jwj%k zlX6lX2o8jq410z@*8-W@&%Iw&H6inVuob?e4znQ{5~Fr;!b=ywO4sjF zemH{wVaF*5(C^uvNB~ojyL?o z-~UMts9jOW9ra2OFd9lfDFE|xH8M$iopkt@Vb7-<9;VL8EaLr{t0vXiL0~11R}5O? z5SD)kysr@O(RRt$5vh9oFBf3yaD$koW&!lgEh=;c$$>sSV~ex8@|?TDhQl#*bIj!D z^X9P+H!MOohQ;WFyjRnkr+Wn_JIdcS@;C`~ii`KYUmLmD5gu2?(#z}=`tif5*UryC zkJ;kD4*vP|1@)Txc3HUkkOus`A3-OmWW8{i92CAPTZ5MMd;KN3DKM+xk`%Y&x1#9A z;|%^@QbTXuwt!tgLDcL;nkU-l=!S-emhv8GGtOkcK5tst)P!AkS+uu25=Adk-m9Xs z2aD;2Qw_nIK(+z_u`}jHpE$?d_>i7FbplVv9g*K&9W6rXxuv&)@vs>s*ufnb$=35Q zS!;`Oygh~YnetH6Q?eEKe4a|ZL3YzyyfSTZ*k@7NK2Uf3uKZp<{yVlQ^qvIMMz{Qt z%wfN!e|!j0X{vp5ifkvpX{WV?p-i7IJgbGhW(;r8H`~%C_eca$D{LO5;@5A}S<0WZSHJA!JwRU}Vg7ZnVtgSG~bM9_^F)DU0fw|A-gLXTZ#qllOOwWkw->dda6Xt0Sq4~{Kh{mYRh z0|hFd&e_D4W$<6-NxBpe5Wr~H-=3xc1UOtjt~gW+fbo1Fa~807GO$emf8ozQ@#~WF z8`4Q_zto-d96u`mw$l~$=;R_ggRzgU&8dwmDhCj)?Uf=|d{DyJ$|NezxGzH?hKQ#` zVPj`XVkV4JJzX~mly$>zT$DE1DhDgquD9lswI-)(j_7|z8&VE^wweNKr@vFSOb_EO z@e63L$)Q0__z| zY%egRY&#cra$`LUbccp(A_Z6qq3KDZ1~1vN`KddJL`FN2<9jwB6qH1RhN}Q*~=zIA>v8x2T0hrS)bF?~rjTaQq z$eG*!XR>J%&v3U~Y4PV+KRlH}vuV>t2p)S}QP724Fn1a?Je>EY zb)`-4G&>$`YOy>r9X=4yqVZd>Bwq=cfiVW|(jO~M-UyW=UT;`Q3?i>n_LvX0c3r1e z*|8wrR0!-a`W297xmke)lPils{#GLNI^A2EHuFjasQ= z2%<4=Pr0kGJ2TY=r4$J*^a|>cMy3v72T>7C5TVh?T2kv>hdRI9ua@0hhc?(vi6%Tf zA#?6DmOQurCBy_UcH@@vJ2~`QIrKpnt@4XPJ>%_RK`Fr3-Z#w?B#-ly9R#E*8s3=U zX8tR+5oKjUBNy z{*Wl?I;X$~cWUjtEp)%P^0g2y9SU7qgm#!~2Rj!4dXnPO- z`#&4b_9umW;v!FKZKII;P54ksSHb z{t=Gwi5ETNWZ72(|B?(Yv0}b=pY3ZLV+##kg8l~DP~0R;Hlxl z6MDsbFoAYzllIiW3;6Ck`oOoqd3yV;_yf|JsB{Gpi%E{1$wqzO9neTydcXPWEy!pk z@Efb1je%Xuq%W+pq@89@tjLJ^*y7BE*{(MNj>H1vDOs$QFhCKB6+zNEnQJFsf2#e0 z@qCq|iT!y2yG*Dh3#;xjWl(+A5gPEnWcB1VTaAhPzIW2unh~E zrkfWEv714Zg#prUI3*EA=@ZhG7D7$p<@co~NsAI^H>0!y-}4!kj{jPd5Eu77S}NFR zg+xVia48Vs3cUfq=Rt&KT2(Q^Xuxu^kdK=v`?3TyNZEUGBWNQaVYTAKr`cpl1x1Qp zQ0W`$5>sxm^ zLmvAjnE!jS)z5SJ=6oZv*azu*m0k>*bzmd#4Rj~XzE;^vopEXCP^HFGu(`J3b-Pi$ zVDWcLWOCZ>t{4V$Vmoo$i3x%8KR0w)KV}HqAUBI(VALY1XNm|XRmC(#gd70lL@g&!W+Vm&ExB#|*(Kc0g9iTKEs&Uz>09RO({}|AHs!fXh#4>R2sB z8FpwMI292bS%#|}4Dl9W?o?}$lvIg>%STwkW#z=s6q$kFO#F_yXe z(HV^s=7p7M)t};8Qz|_b!R#({{wXE8a)p-*$7<3WYkgAWh>}H6ZsG8{LbpEc!Rvlm zy2HDVjex(WwFe&!B81^|V;Idc=rX!m2M&Q4i&=xS@H_r;|Lm`g^1H?3X8FDv%6OWq z!ZJbf(UB$Pepwv-Qs1l)UM85#Hs$`@=U-kIJo|MWnjQr3j;b0S2dr7op?LE2@Jvus(bALPR3L$%_()uH!D(k0~Kox=Ar}wHQT5$Rk zVaFig)Mw{!MvTAZ^JqNKhrH+x=Ux-ckcL=3z1T4gX3WbOyaEmevxBP%Qvamq-T$Xl zi1VU$W(DHYRbv)99nF$5QI3cDkVJa^-k0L-G^Rb8e4?Skp2i>jz}(g&HG0Vkuk1tG znM=*HRr?0mcdbmsjCDVs*VosgdRbRW+HPJ}f4{80$SY=kLk}!%30WFrf@AUuRhM6< zE#bkXWo29)rohT?d+45^jsU7#VK??EVr6yM>|`epSAj#&?#zaPIKBd#@N1)>T_cj! z*b;$&L$yjbLn1&nZ^=1uNfSU3enY=Vpr!CQ;a4H&4sV8{k5hTPE?ku z2*a!3gElVtLc*1+hz;|4?>I|hm8Hz`z?G{~d%=tL!HeIt6ag!_&MoH&u})8FFGtq3 zj*Ug$sYW(Bozs`H_OVYDeZ}X9aZ0;Hh3z$Cvb7lH+L|4tX6*Mn%H?!t&@u4R&`;60 zslp&Bo}5-2cE$5>741!#T}1-Su}6I{_b2lMjCe zE<)_%w`Kq1!9ICH7gqw(+zkpCt*W{{G;EO03ZZkn;iX>=UDHW9zn2}2&Q5l6f|izJ zst1$PKGlzj zo(JMXhLPNn+-~|U>wWC2W&xpv{3Mb(t}2U`?;(x}p15@@SERi98sUKZET`N`o4O;v zA0Kh|0Ri) z{K3^<2#CIhJ|-OURC_u@_peOd3-_}`YR(NGx+~% z|070V-sd8_jWj^v#~*<03lDu`67*U?y)z&dMFa`hm6&HFj2hUnrN138>2tZ`RN_^2 zddS4W_n5zamnTe4xonsq;f>_|WvDTvanSWJ(uq$vh<(Jr`V4&iOi%T)hc&`Qskb`Q zNjFid@qiUVuA>zRf{uQ<-$lsS@N|by9>bl26g4TejqC=X+`hN1;u7M3$RzGh6$qI9N^+Fqzontc0C-jDp@!j z?%Vv;ms?y~`iU-Jrv+PCTnOgLhZ>8IV*d5ML6)0Imo%9kH~FN`zGb~_F!6p*8V9j zaRZHVmJ2D0dQvqLhW0>4NX4josZJcO_82pU$bUR=J~=(Ut`MqbnrEzu2k-gIRi&S# zLm2Gy9}@thJ*Axq>=Lj~&cw|Kg}~nhCvWEM8_kP8d)>KsblI_p?}_%XNi{EZXHpPg zRt)UIox!R4b}y~GtfXpK;i8e{LsrEL+mn&Z77A+C*b0A#gMU*`=8 z#B?bLMiMi+tX_B1Q(6d4D+Vd*qTv$*49silF$e!kMIfB^y}!AN`8ztj|Gq+WRZ1kb zm-Pv&>57PBQGv=eC5{V_0HU7$Ok3|=eTcVU7YA)nq!gP`Y?YB?MW>seYZ!Bx{A13{ zf{;Sxrb~k66L;bXW|I32X-TUuwS4lf>_u2DWqZTd#3kV4)eyOxz*0fby~ z$&k*s9ftP6HSJ>Sb9oXhqr=i+TzyNtIN zb=H@07LXY*@D6qX%ax$t^qn7s=zE$|IfK><#uGG z9jQLR(w2OD{Y1Wv-J|Y-;9CjXMq>?ddi4GRNxMdtZmBhpLANd5wUp zy@7ZiyPjR&RfSz?eAR*(-{}|W5(o&(FayN)@ zVTD^+DE|4i=L||hV(rymOVz^VloT`~OKz#qsIXggGC9^i8mP$#;@Yj{9P2CeF| z7so#Pdi)vc*y_*F25QS4-;N^;LR|8tlR7p%z~0~i2l3**;xPo|y1uz?ig~s%fd@e2^0X4SukKH-5%((Ge}725+FtP*{PO4! zVpS{6>EYGT8r-OOJu|N+>sQZ@rY{GAM2UA(*LY2-A^C%U+iy;ccXrv89B+uK){ zfP=iixR*mzP_MNj(W1hy{A$~>_QEV5^Cb*JPPkYe*Zw~r&OsM77irO?)ilhla!t@2 z{oO&?Sn-;v15KyVm;0^H9_IW%OnqlqliSj^3et;;NCy=~1f(T|4k9XBArz%|1VRl} zdJ}0PO+ZQ#ib$0XfzW%GgdiP46-cD_-oD^@_c`BO7eDxsCr{RznKg5-duBL?M#7Rc zNRu8h<;?@G?2^g80q8_6&AL+qnOL#4#m6ov>p_nCCOsDcZ7V?pOiz3bVyC0V4=nhp z1Jx#;k;C9*y;y=|d{3}{Q^|i6a7N?Xf0^=1IReVZXQYI0Gb?79pNIffLj(BdvZy7k z7BtT`&sIs}>namJG3eY#zXxVe*%0^v{zMkqoy=yDC^uZcWH`uCEsB(-5hKLpujEsU zr?yGn(MO|t-2g&^VvI01Vd+6Ertsr9YzmDo7g*Z!{*QZX%=%v`dbEmOBC|O$0nrk> zGQ!-KCW{^BSJzXF?ig0DcKhrSGUxP3EObt|gE^=SADu7cm}*d7TR&w#T!Ba}>PxW` z!3l(!K}H==`%nlGR!h`$1M3|w6Ti&4@A2P+NNCXf# zNr=G~vCyF~+tKgPCeo-{DBWh-tGtF;#l64*vSUwdZsLvv48z_hB%8V6?UTp*`s^n# zBXX04{4)+CCVcr22Z+Y)$S!l@d;2J1FC(4>34_z5^h@Kqlx|sI()GU5SlWqdPN#l_ zRMU<7Os8e_Q-zL<(r;5Gbz*Dm9dZilsK8v1V`VKJ3=PJ?9g=n;j2vB=>dn2RYPn#4 zJo}eG^gKE-sK?@Clz=mPJkR60aX}4s5uyTRCi6xU+tXGi@_sIjUDQXd6o?g>LEUI0 zTUbd(u-(89B4n*KaI$8>A9E?-7gIoKHw8D#QPBSKaDirmBe^tdlv97@XXvx;ay~yx z%QakwYA3_-nVi;TX5S}C7Nl^)B6JP1a<{)Qk3RsB&-b{u9F__LFw+d#UP7kpK2-lQ z5*D8Kly|X2$@vWB-V3uAWr-&+{C;5O2TeUUZ6+cH;J3|0pj3mMLFuM*%8ibjD|Rc- zH6o*EP?XX|Wx964Bk!VGn7KUHH`b0!2d^-rpJj~?e>gUcT`KH7dWw|)gRHH^Tj`9r z4T_Fvj6(nXA?Uxfe6De)`a3CmTLR;DPw?59pSy?0tGHl^=&}((O2AabYozTn4U<%2 zZ8~vELVV7Tzdp$8?tg7Cmf<0mi2e~N|C6VCnR1MhK)GrpvC-y>c4w8i_6M(?!E>@~ zUJHT4rW}W)y%}Qk1if0Q!lixZg+iKI z9`1(gJ@jr7q&H{EUPAosUq>(V&wm~LD>jyC zTC`d<7_Q|(zc)+P!-T;)fLA!^!od8-e_tc$zvd%(vwZbhsfZDFC#*+Q%Vyfd5YX=8 zkUIW!v+p)-JlVBu>zBWd9 z=)FQR3n?+Q*GtsY8b)K#ui~En=l6d3-|sCW6aYp)NQ}X+AK@WS z;5ghPEEjk0`kG|VRBV<_{5fVfRaPX$z`YO3ykZq7Met2ts-?md58H#qh)s}qf5w#) zw?!>g%1w=!ZV>m<`TKu;Uvy=$6ab2<$oO^T&mbx-A^u14ii2hG4Odn>K@4a^Yj!D7 zU~00+@C)&thj^gK_3sYDEVVB2B0Y7`4q!(ncV;6#XQ@^8Qb}ZZ|G!_q*oGcvV|SyH zz%2IK%KE&)^rgNJV>RWBk;3LQ2$;F$%m9keDWR~3#K$d2&Tl0H!NY%TnbY%5vHTxH zqE%nY0HI9T0X5X?(8=fN2YGr3Pc*Y~QP@wrYf;nCH{ zal5_soiQ;3E4BfwkYLL5c|XUM^YVVa_Vn)tQJ6+a3N~q8X499lG->DtVY5xU7HR>@ z2dV+P21fFsxN(xEHu$(ls$(sk*i=eD%HoP(N`~ara44*C(Oyeileq6+HML&-^|~Ms zh;ApCn0-7~75Lj^=JK(z=3QYhgO)@#4E-v}7ea1Nd7YcKi+Hw83@X@}_1g3hcGGcj z3|G0(*fy8uAXjg3F9a$sNvO4C*V1}BjG082-*wc-(1lSEE+4Ev^5efHYSAzOT)U1{ z@ziX$Lg{3ej!kji-YVt0NSorbW3-2s`04brU_jX-o#9m)iNRG^L8CoNz;|OL7&e%C z2~cV29go8-UlBp56TiR7%S}q%%&Y&l%yx++9$PJwBS%ND-W1#7s%}xgIp+apbKg#J z=rHT*ywv#bn`gQK`b_$DHl^bZ>1lCG`J%Z?Id(kOQUN~-;NJxves@;=qzL5oW|inx_kL*DzOTPA9-8d_zDny3z~qz8SNtsalu z;FS9j49(6?3Nxx32cd&&-8Hpd=n-QmN3nvxIiN+X*1wJN?svUlf)tY(lrUMh+UJdH z+oWkH;KK)pK!Qt>KI-p1g>tMoCFGELa2SGxeb%tF$IT7la17+AEclU61ck?DW~|K3 z=LKc1UT$1Gb~$~Yc;sD*3%i9L*O=`{>o{zS)`4Z+u>g3Lb8*|!Z^wC(#k{oh5LMV; znM$kJVv5fi%?=^2{GLh{R(<#RDjk*R%fy`6wYJc@B;S-s5&tXJ)!4(eHMu5xkr88eKhW%b{s{FBy2u4&a;q-Fh#$(pWb=bo2; zSMDPZ7(D>v>oLwYwfrTzhJ$=+qlo-ZUyb1DWxf-ey$U^*R=%_vqAaK{9!VzZSWT;T z=*#O)zgZbKFD7RBX-pMP*G-nHS3%-FGgW@QlK7Y64TH0qiW!$5D(;fnSY?x)0pSR1XWHIh6VWr*A!GBNidQgP9Y{~q~nF=?LZ{IHg)mQ|> z%qBVHKTAl=%{7=Ht@ds_Pd9ir9$x)ydUm^XO29GJ(kJVOA~#h``h%^&_r&?k&QT)j z2@mIck|%~2UZ}Enw7*ih{o*;O^`*Nv6dEi>#-)nwc z@b<<@M2mE)-|lE3JRrX4_J@eSz|rLjJYQ%@u=K0dMv5Dx)~x1^iy#9b2HBEip@-@_ zw-AV9y#wdBx334ad@&ZXBz9(%2oIl6max5s&Z8FOwksQP-{Ik?uFXM_=>&>IPaO6S zgeB!riY^r+78fTg_$mzcuksXAK0&waGMSvFX93UeDB2)&LE1PS02Tez+x@c!nrgd(J@CGiu!!PMJ8TwLhr_X%k~!lgMN2 zq@z~lr4a+~u-8haAF2aB=dCQOs_9?uU1Jvh^#8uV1$(5841z%Qh#u#knD@30 zei1TZGV)n#!YQ+GM0u)td zgwx9`S-M;ax?61xCt;T*l9%#N;@*-zhO?a+j0}iCFt$zPWvR~1HFGnGRpSU6NHO+-bH1IoDHL)XT;D#(U=8&=rX2&J(OALMrl4Zke-M zgGMWr^Eh^mR2N;AS%0!Fv%79Mfk zBC)47cn#tgJ$O|LcEi7qh72Z(r93_wLre#g>>uop;c)W<*~%%BKKSlF4qpjLTCrR% z^+DCclp}O#29KGOGL!X)R+LESFYAOY&x>g}mdlqj7Yq#w@ZkAZmzk$)Vz+iDKsqIs zhLa&oCOh}=BRAgRLSUfk3ZoZD!aYZ5CwK_&3|cz1ict^4+uNcDB!E#M#QQW8W%$ zm>t;@E6lEBC?gz2DFbGR8}Znayj1;^=UcQF5C8jS2$k5a=yQ-24@=WI-|o9H=EHpS z7|1N=svUt3uvAQ_bOofYTmzF2fi3YsV#kPx2^~9B3;sc1!mZAS_1R;PgJn<&Zs&Oc z6=)-ym@CVXe1WVmfd#<#A)o^adIqEU1xJi%@PtIuWIIYJKCqRkGeru#UYCtcNkOf@idO>hWj zSVOi2e}<{c;HC0z;2dH8Zc{f73AK2;_=&{M-Xjx+Dw-po|F+2H*l5!`(IeRNB((%* z4`TBT$J@j5V3fvn1SpN}N`Y1;Z*3dH<#j|&Q#%y#e{5Ugy|viSDleB(j6OXVq_a^J zK+6Y+M`7%Rg#fnYKaQxtA6H%hHeB8uZwT`|EX;DspScb<3xTYCb7(~$sRL{U%WU$W z7mR-pE4&MSgj+S9z-KSZ!n#Z@9f zLX4P;(T3Qxg4xhJG=k7P*mKiPT)qEwZJ@zUe_>(aY>Hj}EhxjGTf*ncg4|))pdV%e zBemLx+%a2>lf~rU;}xZi7BzavH$^@T1SsMn#QasQ(+I@-#%}Dh%eS-m-yK5DlSPU7 z@M%y?u|?z?X4H--LKu*mPUKi1D#=OV(3R2Q+k%BhT}}#CKS0&Ke)Pt;y)HpSCd>6A zEAIIc{`smT1u<*QpdyHsCyFqPYD*Q2sj6{d9B>PPhnWQgq|Z4~bfVGyyXcB%@k@z| zXt#Zi;zc(&IR^_U&B!}0hg#wujaVHDX{pCKy>ywyrZ|b@E&f~eGRZ35bNI7mr|`h< zilhqSCpym#fFrg$QIOWLBc{UpJ!QG&&QLxF&t`IeOLXc>GeF=ughG+f+TvH= zJ#${H>@I&G-AM~ZkjXP35pg{#^WzU|2*7F@??u5v1ggQ|ZMMy2A=UkX@HykNxJv@l zrL2?ZbRYhl+bP^LtsK>Y?3;60s1kcZCcx$<(RcfSomF3^gg&n3kBeWBthV=Q+o`sx ze7w{M#1*8w=?Z>pc`*>erVj^=*^dV8WQ;p>v6M9J%Pg^kz^m+JI05X%rjZYACou54 zVvInuQ^;yPaX7uiJ!BiyLYXhq%!++U!6iSA`(CjQ&U!)0eX z(&NCY4eieW$x(2_`xX$H$v}cW#{c*`{jIXiJ|t4h)68KJ?$rG&1h%6EhgRziD|{Od zhQ)5-dS}aW2^wy_L@a*rS*^QQT$$@uydhg-)GzDzYR3B~uWomYfnKFG{(_!pHT6^% zFWQUCx)vMmc;BHD5qB*>pen9H4H|3=t1^tz__`-3*!b}GSaw^-Uof$M4KA>VTnT|2}<1F=%`D4zVq7v(C> z7ajP4{qos~&%HK_cp@h2iyMc1#_)ivz&Jg^Zc!>xAu0Ypb<7g4BaAbH005jYAk-f? zY75HiH)MrOIH)dG@Wq)LKWy<}Tzke~3W^}!&P0D>CEnz%8Gf@lHN=1>Y3$DMO-#;c zIn_Fu!2#5dYkp_L1U<>zePmUueuUTvxyMKo3G|CiP68t8?v>pY3^|Xzf{$9$7#u8; zq;1=0?>MiQV>jr8G@@@+xsduG(zv*KelF)+NM?7biN{R;X#q;Sh3QXHK^=iX^Y5tI zhQeTHV>(tjW%QzhD;4lmN&u!M5U-GVt4){ze+_d4gDEg(+X#**ykN3M9Y8;p+Ve2E zPR-wtiEgW^EXr+xbav>ZS)JVrfs1DfC=;vqAkX18xZ!m_;VA)3&2p#Z)!oxXpR=~J zBfg)goz?d>MiRVvP+Xs?LI3f$QGMmxFFYcApKZ7k#%(Q*GcFEL2i`$1A8egHJ-?V7aA@u0YDxpAorp z8Or`_sG6ACaho7ptZ5A$-gSg-xtmzz!A%T2-v!9#o0eB$I&|h%YkatDA$l*_EB0s! zG3$)TuIQJUK0Yq4za{oFrhp2dK~%vY71-QYd19nOA23#7vD7b$4m$3h-aI^F6P*54 zF))OwLa*tsqm10nRw|ei{{|hOejA)CT{b7Vl*y_b1xOl+(}*#i8nJoIqD$F|&MNAz znA%e*&>Ost02Dzmd;CB%&==~EnrK8_#dsEFE7;txG<+v!uUD;A=TzUdl=j&GzH?AG zzUCbP}uy2 zP~&u2$g4$c9TOI{P=t6?YjXGj$<e^5quUB{N#)!9u@$XQEE z!&EdgE-#|!>#NVm6aVw`Kgm?%gH%5Fr~XniI{_AU%26oK8|3xTkYK)fZ#GkbDW0Y4 z3651)NDaO(g5=IYoKBPk+rs?0Wqc|78vB4lDbwPv_kILP5XV_SNH7yewwUrC0|K^O zy#8|7!zjI^y@IJ%uJgLHZy#qSXNB8^4Js8rr?5;&DQl#*wx4}q1noJ z&jUOq?h%FyMww}jO`9ja(c0bE2dhaKrb`#gTI{>JJfCr5K{9*8YwQv=Em2o<0_(d) z+(P&}cY7oA^YaSgynUlI8rqh&91ae;TRFMer`+@uZ$@=zW)?LQ=?*t*4F0oZ#8unA zVsOXen);%BCyUA0ZbBgR6KvvcJ52!q^DdbMs|D#Jg`>5tTi^AL6BQnO@#iBZNWV|2 z)(HuI6cwaYa(KF(lonjl^c?UWX=BDlil8pCvf&d8QU};P!{o2)WA88)<~aR8A1zKr z3cY&6iK{^yc%aOv2V6JV`Ycr7Ps{x{d}6WMI=2GS!}ka#nvDbHV%B}mUnVC`>I8l( zdn3t*F!Sq7a>-9d=Qy-FjblXw=YGnaPOLaNGa0)RdU(`3xqqJ*g;-hY-FigXIf1W- z@8=Jqkd5yygZ0WX=jI(@r9=bcnK+>_8Vou5I^y(b)Qj$jPWJ6DtH@V;Kg&Pnj zdl)h@3~6uX^SZAek|k1Kem(QseL!J*3M#2`Dh0hnj0Vs&1weW#AZ~NJ`Dy(4;!z?J z007~L?j*k4X-?GFnc9D8ZL^r*Yt1re(DyQj@+IvMC=YG_aiaFeSRs7z=QBoyiD38{ z={A-28&5Ip;e?i_Z6&o=4GsO?J+Gm#%r#f~5Isv~$aFLt#J}Y#G=aby;=DB4%Q|v$ zE-70<7325}6xe%$^TmrAP6YM(_y=@;@&mjxd{*o44J4%VK@hyQ_*-c08O_KIvme4k zBc`?yKWIjpy&0aXDIS=5u@+0~7MK%{wmMiX(2YZ1D7{v#qR# zh-T%{S!$iR0ppj8qWPrxJ)jp$ihYqcy{-$g#L}GF_th37Mze~3K1k*mPkYBqK^BgG z_S!F52#uMsG^?R;DoE;)X!IEPif+WCRi;u- zEzDyn6&jGc1i{Jeda)g1H6>$T@=+hlo0TMFYw2`YLVKyS$k&>=i7`|2h=Th(Uay6w z(~qt`Ju${qyl^*49@mDVHqNqk(=l%sLF3aPp;3Zamg!|ctxCpa4Z8UKmkbu1`yZ1h z0^)=5NkVD`qJ$z`a3h~Ed$lz!Pinc1$ z8wa_5`ylj@<)$kzpzKE>PSvVFB$G!M9$E%$F@D>_e4V(^WGTczTtg?j2HtwHbNucb zWj}5!$&Q|Q{It>hwjHhf;&ynqAb+l=cQe~+7N&H%=zV`SU6$PBf>g$Xa=LMN4LQ=3 z6@ zd(wwbk)adE>hX~AVgH6anxumaE|!;mitlLSTb;5ivR*xxPb;!^xS$I*gB;d|L9Giw zL-rprX4f*`A+|gbI+EC|lD~@_EzV^v<$1|GJ=DBRt%$PVpo?)$tM@%>KJ$x5-aN|3 zJlGDL8V2jq=8H02Q(+@bL?Mz`$dY^WMU$?>ol;TAme3=2IbzF( z&3eNnZwPlrIIpsqt;rRO-gbCub28NVM#;~9!oV2e6{L=w(z-2KCZ=hrDik`lsB5kp z;mX4}21&^kldG;!(3K1r9o%!*U3uR?DM&cx9_1NZKb;(z2rQuKle*B?oGr`{4XqVz z%r7r5Y-bD?+l`!Cc8D=faZ{5#tP?HbYc$RqCtbI>enzp~Ud24k|1HT8SyzWCz{?D) zw_MV-*wt($FX^B}ap98A>#+ahI~kHn#tXVjn=gV}K|NV12bDwBB?BZknG?}i!&`Ts zSWCx)6k@51ai7r>ZdVx(&jz0s{^%z?Jmix}JyI`r=qsVaA{MdtQs`O9>IZExId5Da z2GTd6>R%%(kEya?L`Z9e-D40=*gq0Km6U6|>L}l^`-Q|&dgIK|>bO(pptfYfH-N!Y z)g(UW9U#iAfxZN?**x%4FK7y6JU}!%uXYLg4u6nfsm2=sWAz=B(F1ZNv4?|Z@75^>$Q_cE& zrb(A}*spwD%WvGL;_}pGL!*ft-V!FrfT06u!9sA%LL>#&hs{FBTW_{xXCM+-_R}lD zc{UPv$4CVYLz6)86X(*YAmseGx~rbnl8Tt^R4^QRGWG2ZGF1!)oSSAk@Jql;moRAW zyMMc|lN*xB)8BzK)j2ma6<;1&cGKmj$w&QT92Mq0YiTlTLRh2i9l*u7>*o2Rx>2uW zAk@R$3m1iU?aHYC4;SdiF_xKTw|Fq&+8nX;Zir0(DJ-f zr=wGjw4xKZkzjLepku#t&dQNUd90d)1Ytswtk!8^p(}HZAYGamn=}t2Hd~rL30rfx z@U&^F2`Dy@YM)B+90>?4+-wI|kB4r}-%2ya z;fG~4i!P)3uGf?2^w)?n>!sAlPZCwvj{9d$5hSAkSct;s1Hj_qgMCkgXvKrRSo7Sq zKrK7lSQ<8yjtWi^lFvlBDa6`LhtNg>t}Tq4TQ}}ZUYiwmg;`!DL~)j+796d{Zw{t!tvF7t z8plTk(t`#mP#&gME}N27J91OQCfWZqvXdds0q-ysvZW%7uioW88i9n0sY}im&>^&& zjJ`P3TH^UW24RgPie0x{tv}vNj=)VAktvEDw6o_FG}Gu0jC7_SRJ>u#BQkn&^ia%( z7Q%=7NRhs9ANeJIEqobUkRFF=X+jju|te_?igKnnTVl0F6kSldE3G zq;K_YL-1K0_Grb#v&ST0H$tTe>(t!UNU`Bf;r|)iua?UpJmueKw%;#xt9o z(5aBuvsVDj{Pr49Zj_Bj%Yau*unIIL6u98&Sy??lmVs29ox(|kP>TU*F1FI3ZmJ$Z zsitUh5Zr{p*3X*6m-rPBhnSr2l{QzVT|{bOe6*|PjK8F@HaBeXFAO7osL9Gf6>y>P zJf7UR1MDnGEgZFdldNH!Plas~at;vpYjj$f~a=aPWl38f|*o`ndr*$FLDPC`mf-aG#1z(&> z)q65>8l)Hf3>J*>UUP-FSwUyo+zlKr zTo=kR9cb7uPeVe_IKp3`_Y@Da&~%2b^x)sS3-;AyHB9 zkxYfz_$}txg*Mdkm;w_VV+;&N(S5ta$(1C76{+C+)NYGaN{MNYt>h5&MSa?~@FlK@ zK*-`Z+g3E+X+3LGk@Y%2)JS+nPE}|Izf+gEFm&B1y0#dm!t>zON8+K>-Ed3jJ?2VU zz`2*ZyL`n5jrUaOf8P~LI?F8nukuxurA`&EvIxUK~<2t@P zrip}1bdxbnn4|3v%L~s4y`!h~&A)OSiB%XiPzD~Tm0Ukf4CPKPY$us|-gxQgHO6(G zq*dom)suQ29&2g4+T{_6?IN9vCD3WKL1DF_-*{6@fp~I(*BCLtF5GVB=#7T*GZ?hp z?gN%!JB+#u#^>7(e$f8#Ice%sPBP&(@kt@J;qDHzb6*GC@lCwttE>gzEY9=w8L^@G z>Glk>Swos$FwU?tlB-a-gpwu)iGuC)Mu}Yg_n?)WeQb7T*z($$o_SuO4Zh=;sSwrp z3=n-9Zp6_xQhFjFSFud^+E-;4+VK==tetJstlbOukGlAm3t-h13UK8TGRAi(*UJWLL{EskEazq%KK28`->RDX_ zV1D7xZonS$*0fh|0oF}^pEN=#iO<}-;!&4j7 zs#e*jR@Wxxoi@Y6W>@41F_OIPKmjkXE{zzDs;?VJyp!;Buv&l2p3Lg*{-MxoCou#b zO0mNLEC0s#$93lf;s$Cfjjpgo<^gXB`k-z;pi=9h9pX&T3 zK!T$`nkq^_<9a8{h>v91UR^KivGc*Wk2Rz5jDatwAO#SLv_~wKz8>rgfzQo&{PAz! zOs@O6zn{x2bG*B|C*g`0sHp9ez5$ zb=X|`c7a#s{BS<9Yo6p_g{bqfe|7#3fdBsXw`6*NBDC~~b2h{D#MW`Yn*T&DJrC}V zG)uNw`dDY9_0C6@>kzX>{e(0_p?iq4UvS@i3^UguK1(gVximUE$TYX(c-R}^_%yLi zb)BSbtQ8?XOJDCJqM0fmt~B$f?@_(KrK`iqX$^dwAnpf7zw4GP8lr*REY9Bi5xK4! z)Da{IrG@TOt=5-&HY+tz#7tWxd-a!Zt*QPN+6~yQF?vVLfzU9qj=W(ogZHTVGlDz%GN&?j1ZvT6T7B=ep1dj7 z>iUW1K`g8Rvnzr|*J}lcfElxFt~t%eAVa$k-7C%wRo*Vy<=d%XlzQoFq)l?sO!DEA3%PB*%scqkT}_8S$jN~2^i(`<+jjQV7Db`u@U-@7k9s+JP^=saM5TE;ML zp%ClGZ8y``xGy>dXu$r1v{ra$5gF3K-R>IoWWkP_CQh%|GDu`>(ysJNY;&R@f{0hX zV>C50^J@8^AaLi_OV%g{99nz^g~eUnZ)aC`~Xdw!ku_!hDg)VWtf|Zr`~WtfZsiXAoVgn zOOvk86e-F2{MXn@QIC>_A;JL(*A{tY)DY^m$!`-fTO+)a)se;8Dhmb{PYj+l&~j)0 zQ6l4hF+WMudPH{SE-a|7cG9Q9fwAmT4mf)l@Wm97RY^9bpV4p(tb}n(M1LR0#6Phd z5175;nk1y`09Lp`s&K>j#xW`QX=CAShgTNUR8_2^Rv`vAVO1aRQV=WG&4k|NU}W$#JCzARC1^Y{fR_;WSoJ)&Z`SF72ZBfvk^2W zZ7eS->A)}3y>m$a!oqp?wy*}IO%AVLBbK>Yk<&EcDE?2G4XMhe%mt5ci)eRM_{Yfk zZwbns8X^jB&4Yj1hpK<$5QO(5TXacVo07txgBgQU_r7~ai;b_1M)W;*`{=KAx>;jq zM;Lz(B#)kK%d;&X?|bXWINlT>pY&qrHgN$-=dRz?>L)#FedB49fjY@DySJn5`Qxsu zb~R(H7yqQ~s(T;R$Gel!_2s{+c|;h#E$k8|WQsC!W^1yX2&2jjg7M$A^v9#{`ZFQH z-_ao2JXW;M;0sBEPvF{ z^_YFg9SgXp8lm@RA+oe)I4T`rEl}RTXe|S_JA7`~UD2-c^_tKv?q0e=VCIe9Akv*7 zwo?0Wg=qAZsj6tz{``w-mrv548UH+O-c`ZnM2l`X+DpGD4wPo>;xF)(1bOLf zD`H2a&G=MH1(B)BfNJQ8TM8P=`lm1rbWm2mE5NVFy^526C9)CMYpILXtUgKekqB64 zKk~h=Q9{(DyA-W%{Yi>%J@1VzD03Za_-~Q~bTr>yTG#X6Xt?-nau~5^cm9EgV)SLw zaD8!+TV{67w#Hy;byg>vOF9Q7M2d@L^7R0X z#;=@xoaXkS{E;_C+%aeLjOben-W5A<@VAbHgUToHdQ_>F0fq@p?QN$Jz}9JU8psX6 z_tS)hT`kRyOO3NPcr=nwvRYEo=63>Dm`2}Kf^^oB&;QX1XGbWo@zrvzG>S&Uq>G~u zT=T}#)qsgBfNTM@6`c7(H|;Hd73492uqMzlF|IVOYru_gOU~PqkQ&U=eiIoLvtgD< zd1-=oSJBO1kiQ$N13m40m&N_#OyJ&<8%LZJ(p!64o1lmt%Ke zRimHQ2Xu|!6@e@})x@h0$KnkibIrAw&j&##>ul@V@ZKaPGB3gY$>U*Yy*cgY_@c{i zsjoIHNv4cVzEk>3^nORC~UK!G=11)S`%DfOi(&HdiZQfGUw$4xbc9JM+h))t&C z?(1eH7!(W{B3QaUK}DW8vAqDSJs!nrB#$3k@}{UP^@6U~`H}wEbHCx?F*v<_N98MK zY=J#qXUN@VpYqRRA^}?sX$=2V2P`A?e6ir&{>h<8P;>ii&5>?D-|(&Pb12@26F|hi{yw?$CpvQG^GKiW+Qn4N2a(PXSlgqy$n!!{dWaYIGbbzQ78RlfmIs z6+WGdIv6&E;fIVGm3($W&t){YwwtN+P*7#E=65T-XK$fSTU3d2;uIsYCEa1>QofzFjq_Ka(?hM z<*z>&jiG>#e(*UaIo;)I-t9I&9l)MF-tfE_Ei1|uy%7S$QNUY-W-L}q5^#{|WzMN$ zCI4*-rQC?weQWWRLIF8!yiT#O>+0<=C0FrH?}_7%Y%(epPCu6T^Rks&K=Ws3ZU?)l zc@zvu(r=ukiZxr>DN)i^tHPNp)1Yd5`_@(Z6w zz3sn4ydKPmv2VU;2rzKv=}(s*yaOPK`wodT`s9uioegfNh%fYc*IGytW-4d?ED2TH zKYX?qKhLpB50xYOcMIRbpn1x&vbdlcDViwyJHWxu?|-_xNaYkKJTuS}Q!E=4stQqE z{E*4WIpJY_O9}{$*mz$23W5kcsGwwILyLzT1wI=E>~F#SE!EhwM)x8}(s&&VM!~iURHfiI2HBG5L+w zIlUXOi3#YNIcJi`hRqyR`Vy|LOqm>93)nT6Z;m;~cS$r9CVP+TlscA?$HyuT8k~~K zAC#PnHJ!$GC3`4I9&O%4c{?RZ083!RJv{!6iWhoYq^iISN2j!hUnVua&-`koC z2?ohOMzU=tvIP>``SXz{Wwvlvwavv4sz+S4hSh+pS3EFu z@dp96tghArkn?aM5gn;M{&f}+uCd7{iGoI^4nI_TLsZN@opr?k-n!`dtUpdOvK?@~ z7hfVQ}iw+`5$Px=hijq0$P39u-1N3aykx+dW-X7 zxMaO5a#+Mw!o~LKs;&VpN=yd(J$Z zP3wa*#cK}7<_&*v5*U?Uo?+MFn24CI!{K|XvYz6K`MsU3*9D&xmyPdte+`96c_yE6 zAF*xs9vHxIUeoK|eamZvu<6>?8mkg@$I2!6D`m^~iHc#6yyK1f31xzWt675fXoyPS z@}d8_9Pm}o)q1HUtQ>X#65&4jqCj9V)+Q&+P`jj9-O+th^x;d$20qQN@(*T`uH@mR z;~H6f`5E?fH%LD{aU+p>Y8Yej* z7Q5WJQ8XHo8Oh9CM?a&mbS3RgBTh4q3_b8@aIHs4vg@0J~u8Yrrk(pc;rY zkmauJ#K9d2F;`f%;-H&~G3L8f@Sw3>lrn*-dLu9>qg0>2?U$b*f5Bj3QSmzP&)?&l z0(XDLY3-)p-oer0#rigW%c4s7GzlhbbQOy3=~Gf>kBP9Ect^0Um%$6sDxk~c+kkUW=nf26v0 z;38uAJ#easc6o{lsifw#y&ysdUa&Tl!o0B67*_uzz&s`D_%M4ay<=?Hr};2{H)0p`X$VG zDrpFrDT|-c!U+65!5B&wfvxqF8Z5JI$??W*G#pcWy;wBJ7LK0z~!{~-edvR z#UW!U;3h7otnn4(lv6cLs-wq;r^nv#_joVjb}qg1>H_6@T6d}Q7K

*?8?#{}wSw zaL`f(vaZ&}zT-K&KdVdmxyJhO1OH=o22c>R;3~hca$)0>5Bg!|YY#iRfmg*IXT|+d zvrlzD3%)D)&uN7Rp(}{Xdf!BPJf<{yy(H=72-ApY<3D0~`DB7ndm#wfp1#|rprgr# zceS9%+YV|S1cW0P_5^~7b3RTg{QPW>%rd%(b6SR>_bq(V{ZnwWw>0tS+Cv$v)>8fg z)>m3^_}&yqrgQeZ@9K&D+92l@71CE+Hd`jr*r1!`-5w8e?M5 zd%GwH0Vgl8i1Vq0N7UH=#($ zod!Euhtw8+;9FD#kE9uz)i{56P5V*OkS$+BL;Z=nu8|j~m1o@6oPc^}zp>fg02!^v z=XzsHheieeW8>)60=UUKjh6F-$?wTUeqbk^Iw#A$s?tu|woGlUem!%yK{HR(=j9@i zs{SmO*{gJ5__)zyE(_N+fnNS6IR}gz)=79`5aI!_+7rZw9Ub{3ZubjUUQ;iKg%<5B zZ1d4NCf~cBFM-zm)#Q}@+z}eSvqx4e7`Judj{m7RWXtmHv#r3+kbXj0xJEuzo;4^j z4Pu@duEXCJU6L*BHFBA?P7JZq;scLNx$=&2C&gu$#H0;sX#{dQXv1~>``z}A24G9M zT62ZH{lvuH1uSCw*71IO^YD+4F+h~{?Fu{U(5&6Ab>zc}obUp2ZHmn5T+5<@J&sKLP1lto zl(q=ytYMMXdbrPkQ4^GJ-EKEoVW8LekU&x%7v|mdsqN)ygnk9VN@BDWr1{fbxJnk9 zl44e~Kb3ex|9((7gM`q%)6?RwbUPoUtYr#R^PIjjW1%_pIU)O}r>T6bCAL?Wi}FtP zHzsf6-E!kq!HY>JT_4XOl{G?YfduBT9E6A$kXnzow%@jBK;Hmj=OSPEn#Y%ioJZ_n z#z#EeNAnr>G8+8owD##+4O6YjLeRxlZ_4q=!H&9WW@Xz}z~);0dTv0l$;G_K&afkA zi|m1AnsxN{M2XBR5IU8`@AJUK;!h%qm+bW=%tQ;-DD$w8-#h?13ECNfi~AkKQ_$PV zl`7%RjWt72a+6BWEX}Nwzc;qxbHX^IgRxKhX^M*~b8ur>MSQCzQjh0xnT<}dy~=Dr zY1xYSz_grMFVApCneT69Q7E`)>J!xfe-VElUMZufMn$=qNsz1i75qr8K-#%->!TtU z%}4aEzh{~Cik9ph2Dad6*>g$!I|AQp4*DXk3--Q+9%(ovH)7?DWlZV&nnh#=!I*{VehdFPS5+@d&gj8 z@FU4ccGffZeCC>aO;voFeyKX3?{_;Z;DoC8!2JAo6Ot@){m0<@=Siy$-%+OiZj0}kGz0=<#YEN> z@1DwL`|S#Mg5kS~47|pA9^>e{L1KzS>!;q%*bW6uRK@QuOINCF{6OJmBQ@R`ttBt* zR{0$v-@sD8gv^oX};Uf-#xYMYBycBP$&nnD!cvxea5aK_=(?Q_vG%N{g+g` zI_k<0P+J;nuJLaR2IeXi8RYNoo{YZHNO~y72c!>Wrw_B)F6+6@Kjl<34B%|1J;mw1 zIZt>odB~Z=n~rUkk4B=cJ-?W3;aVu^u}Q|qB*@e|l$$_R(l2pVrXYTw5m4+R0)-nw zVOiUtPuIY3)H6W21X^N#O@);ffW?yVgkN-VxL4E4;4c@GhA&Wm_f2>?)x*MZ?o7Ir6;jO_jH0@1Xg{+?^mNn zz#Ws31~OAeYdD$M6`Cbn!b*YokT>1_14o=MnA)$Y@P;gN_Tc=6oE+#PBVfhDot1N{ zl|u6H^p4a$8wRUwF$9&qm{p~LMa}PQSj3R5D)Z*eND9FzQ?^0MN!{O~Ga!of>uiue zrwF|5Ua`1nI7Wu0`vuF=L?lbxoeKQC>%+LI+A*w^rPcX) z0L4At>{CA!Hdo8GF=}&gPK@CKxPPK_4Q~(al0ascfoe^v#UJBch96(VFUwRK&=(89 zN|@^OZS?f@oBO$eMY1Ky@`1SlNAFMh(c#L%-+%2PxoDh>;(KX=Zj5;GktbKu-O@{dOxny;uY zG!4>|O}!ZwH#)qSgT1@?t!o-nytvZ1YwxLERy+$^>jOeHoD-d|VN!b4f+)$W*G?O5 z^kIc>iS=S3ba6oTl%h@p9fZn4{*?99QGHAGVgcMbUo6g#L$*dIM}zg}D9|rvqRrDa zvDU@?7nhxVb5)-ItAoHqT32iJo7bL6oY4#J;Ky`rYC-(V^}R=f+(Lu1g;VDtOT}Ve zVT#U<-yTYeueRV|Rw5oj1DC51gr+0Vm|Y=E{QAo4;cmG*>=BW2Q%UQCvVssJICVe1 z^H@$t4wya-HN$Ozp3diCBy5xV-)MIR9S1&y-H@&zJWC-DfX7*Xv+6S;4sVtAm@Lk@ zUWdSTPyOhuKpdmi?g1HsHku`^pqawAvm$7W#~o2MUHv-bCW1|U>j=^zXsinOeHo15 z+MHUL0dC6+t2=cE_70`Pz}q{IVq%{Regpk)?xAjEUR0=XD*+3Mk9|%(Q%O(YO!xy7i8F& zie?udEUbgBk438uvn(nRHF0%&@g<0JG?95uXHviSvdoo-Kja#Nq$zO)cTrsiEL1?!IqF+KZ__AbHe z_Uyk`_7EQ0t{$WahWqD$ZNk|Xo1z?v%ryg(_nMJJ7PTY!GZ#+|gP92D6u)_(6WCGU zTEG3lcJJ~-z!E2B&*8{LWLgkrX6C3;5?=drGD~B5Y5(k(JkOF3;={=!(}n$4uo_VY zb0ag8t?7$sHHpHrQHrEi$8j$c3b4Zb^MH}nu%_zv5^{DNG$XcGDHdBbI4vXdbqjf%Uvwg4Gp@4PSL-tfkn-iQ&}uJ><7 zMFMCP?>uWo4C>|$Be7Dtp?!aj?L)DvCwc9?L1D#WqEB^YJ3i;X$5Gluy^w)O-g0j( z{KvYQdEB|WD}Ho0;!HKl9OZ1YKH;>)TB1rM0?RNO+IU)cAI#JBSZ3QPVt?~gSFFnU zBSvUbK$Ziu>Zi`9ywQm!4olE{TWZh5mL>6*Fz#GSfbhZ?kZy;3(DF5kzCIfv(je0+zTzrHf)c^_6*U&mK; zb?5l_dWQB+HQ>^+BZ`2-E|-;v$_u8A5IANTnJ=!+;BG;Z11BC+WeUYx_`5*$I8))teiBka~`=N4VLY z?1QkbjHRWHZ$Nw$jx;VU$J@TV=Lxd0`5eYT=ObMb-_=by1u6;?t3Mq0O&nYTfw2HX zJf%>(%087bf|ySsY0`D|9PDTJu#Xyzee;IWxN*_W*MeZfj-GV{o156srhN1ft;rw$ zVT6Y)1ojNIM_yrC)dGmGRC{|aN?H}`WLMiY9ls%%!dWNyM5XOZ!>XUd6Y%3y)&2}S zeU|>2WzIZN$TB;GYOBOGVl)bh09(W<RUy(89`};(1;n`(QX=`Ma|Yg-?4ay0*TRh1lLax7f*7$;pXJIsDRsM| zHMjnQ;WstCg(n|g*Ed=qXi4kpS0Q_6t1SC7w*9LRh*e8QZ{mITyT!)7p+l zp6-5M;1N9yB+8+w42NsF0pRzBvcB8uD2c{&5=Z z3vjYRhLT;t9LnEYL~i+z6k>ja_;EnXjKzBg#ocE|KC=GXru<$F*k2urKH8^I7V{+e z8W(V$-*y_~c)oIt)gzBG=zN_ZaYM9x5-LzY6SyA1)co{#XR&;l3rqKn1lTfU3O2;m zlIc27#BQ9-o5eY=_+J&$qzJLK5&zc-=CyQWtO`U%6aL{qGR2IOUm+nGE?FcKp>g$!TRkbmRV)W&j;D5Z)8LN5@NTXGzFLHL$ z-3R&TRR)b0Du(V)*UqSNBP%Xu~8#zf`_DFSQRzHFQ~%N)|`= z&&+rWlAfw;av>CMNJMz9eWiNCc(i3^VPp78f?xNW&2sdP-st9+MI7vAo?GGc7U%D6 zNqWAsssPfls-B0bqUqBFcVL|m)u2gX>99rg(C-<(yvMcpRqSqL$SiHTE)n5G7WjIXxlstpqZrLr%K8Sk;=3td)xMz5P!bw^N+hE1LR`2 zNp7`ov2Sq0igMI=AND+2@2jBTIww9c-2`L-&|x--(>`4fI(lV1!ZWY+!El)W>&5iN zy~RoLbea9h%5_K9o49?tu64L2X zBmO*K?V^L7N~?)KwM2>fK0bXzrDh2?g5yOKjxIwqp6R%|;atD_B3E?TbJ4QKp72yy zX3&PYlqBFwMd7?af$B)OyZ%zMIfcncyPd=JoPdY*vF-qsccmZ5LG`TcS5hF0q;5z6AuNFp(ruA1`e2w-*o#{u8 z%;Em~b08l%Ndy?C%OP?dBU>*wx?IEr>rk`XF^$v1K(Qlcl`>Tlem3dzFs7roA63?+ zjMaz_aok5&`6p-n^VlL>@6DJxJ6TbQVk}qMa=KCY$k*D2tDeKm@ZaGrh$^A`NseQ5}uj|I?^H&k6 z$mAx!*UIAAOc}#Joi&VGg$Dg3i@2b4xPehGX@Y!|^4Q%7F?!5QMKE*A2uX4IIUSgJ zuur7BE1MoR0#J(Z%GwRtg)P|4q|F+VRJm07E62WcphM}miyA`|2=A0Mm=3xoxV<3X z=uj*2GUI$a-It;>^QjnR5R3j!b{WP0q16kf3rm&tju%EY;kLUAED+H8H{E|?aG?N=r_)j>^bA*`G_>>$1b(&=+Fs z_28%fb|yz7!{7AM)!ncB`RUxnm2Zb2Rq#IU?NpiM4sl`!Rp-tv4amYiY=fc%ENRJ% zEoK{+EBD9h77aPiy;0hkz+Z88F2bD|6aUM2&DLa%ih&yy#-OU868e_lqfmS+w~Nq# zuj^d6^9LV2^`X9xl6UKRWn5Ljabs%RvBXqEKAoEjIJ3p;9p=Q@W)POXU9A-bQwe^E zX8$j33-&t1N`y*Zy#w7)S?PEDJAcmxthx+$aE%PSO$o)bG`~x}nMc!9!mPqsA$5Nk zJ2Um`l&d9~h`3VOh8nxMmB;Enx&pqmc=pcYM!u-8^GAd9zlHfHvcnTMKR@3K6s9KU zVvfa$MK#!s5S5XY#h5MCVC8WPNx|*Ttk7UZIT*@(Wn?qd_R4|J17L}~WpQ|Ago|_n zmHs*xWw1+hkx|%dj!z}WbOvF766nou>5Dg_B;+#s@d5B1{JL5EczknCk1ZAOo1-G! zOX(})$gkGNS3*n?`@!@@?T+uXBc1G!om&>0P}3sPBn)p%Uz)b<9QShciUh864L3NR zJ}O+2d-3x)ZizwY<*-`~Pw8ntD!BeP)-$&hLp*r(9KX%i$KqT5^>S_H_)$T@*p0Bv z237(=rV7D)FjdK`Rg0;hJMv3DQL>0k4>*PX`?sekX0A%J+-rJCYUiSgqkLP8xOMdR z%7ty>=(+22curyr26^<|S}6Tgn}AtcBr(A-x!!g9+{_ykfF|Uo$Zk!veEIIGyH%nw z`nhlge!mpf*FyIa;v55wXgo{rCAQ=BI^%RWBPVf zAwhV-U~$i)cY6EXJ=<*iPS(5=ASFRw9G3OMw_2gOUPK$Orf~+ihZL{(A5LF%WW3s? z3X3SttXLm;IQyZK+UzJyz^Y^I`^CmhNON?QvqDjJK&Svjy~_Ut8xUeEwdi} zF~60}np|!)qq=5z5VIv*Sg&fd`ITt)IUZm)`n!mcrm|s!m2q?p7GGUj?PBdP<=)wa zpi9`Jjg%T_t_FM~nl;`5$V5+fxt2MZv+p~)M6~dprgY`*HRXyWBmiEx!7nT#XG(R9 zLB_$p$KU-p2G`Q?vjatg4vy%TYyaY`C$Ina?TLI@A>T{(IPqKNva&KxnQYY81G~G- zy}i9uVm@wPviA0@Nk~YVoz|XbWMl}`czz#OMh2E^uyTuuiB-oUx5dIxu>P8snSd)2 z%h$A4fxiJezh*jWwGNq=21rXc{XB~(u^cPk4tayH#P)8Z^4If)J{UiPml zFodb5ujYwfr!O@_ET6`s@dBr1;tD9Tgi}8`o!VrN%DZ4GRg_ zuGdQ&GxG!P5-grB{z#T#L}hRpUKqpQP^Ew90jm9}zdHx>v>m<{60k__!2;4$g{t+!yQ{)1_v|_>G!|9|Y7ius)*r?FIUu$#coRn5aJCOG(b-$E1I9E-o6Xs2fhw_$%n&&-?|Di$xx~5Nay<$+$M^V;K2bQes-=G0_kui+$qem z74Z(TY@QQrJiPxPS9+W^eXTUy4im?S#lY%T=|5nxZvhgm|ME9pknn6S8Q0seP8e0Y ziNTP-9hW2bWQ1eZwzj2ZX4B3Ev@@^hrj4eyG&f_ZYf5-B?&THAB{SWU)F0ejnNsCG zcvjDZR*)oOdjpx+6yj2JDx&1o|7t$n<}(eQZ~yJa{_k&%;)YUSIP2!d#yuw$YkXA8 zdJ{P0Mrw5tU9Ae1;v|ATq@J$hD-J?C_n5m@@QKV!rIhpDi#!H6&6D~QcR1yjyN{O^ zkP#EF{3AJ~SWa8sn;Dy3M(G^3oxpICF0<~(Y1oYG5{C{qGzQyUAm52VoyLqr2VEF0 zF%?i%A@*su=&cmwS(b49zRJl*$hcyv)BM}?h!%^f{UU?k$}bdYnw+UN91ed6Rf*_q zG4d!f&g!_@G?P^YnqdlCg#F@!7b$BMo1xf3I07}nQn|lQM5(OR0CUWBhn1#!ut#f9#6{@g$lyc9xs-- z$8k>23dH&3p9F+TDUxHg5CI{#RK!Ys_{G@Ca~liwWpClK-9Dp&geZ31-YS&V#)b`N zL>-x_m1^j&MaoojABc`*ezN}7V=ai~-|zwch*BR+4>zL466x`HUPEyr>UNLu?4K;q zOWq_N*LqkCPC}`Yg8sM?6;2fjudfp{RcLuZM@sXj^>bc*dj^}>86Jh0l+-lly4nf( zR<{2HM!L;%LEitw#(%4A4S4!1bG8m%6ay_Tz{Ub8&DM9;)IXAzzYZ6vqdA z2py<=YixSfMD~sY5-fuBbwN3$+n0{<@8-RwCV(YhW_v#}6ikb3XKEiI(0B;(6*=D? z7xL$AOZ5|S5sl8!vnzFtOZ^s^n(1IrAIwTOoZFQ7(|j_HJZ{hP=+Cqi{*1wF{Lj}q z!X(%QdI-yrtE?Va_mwc^o{2`UE6Pi^rk)~Afy0I2OZ(RpIY0Q!Xd-tz#%*^xgFi7S zyB=K{Di7YNI$m8#hrEqIzq-zNYGhT@&bRuqm?fQiQ-&B_REKX)SSF{@@FgI2C(Hs6 zvw_JYC09Ock01he6ojzD{&n-VUvI%IFk1L%6=C+;G9fCmsqCxkC~yTU5d6cTL}%fv zTKDS|i1SL#%SVO(u_oLuQr`Ytmj9323j}5CjV-br6y`^tgRl(KZ=zrF*EQzsr8?D# z>HnBUp-Kn6Y^^toKsFV~U9HfM6y%*?61aDvgwLbi{(7_I_fKcwg29Hlz|q*^eo-)} zp%1c8#T$12SlS!~2>*v-G$xJFhEW=FyKYcy+2bu<4?VBhkl~p5$KGB8Q7!&lnwp4y zw&Qrov`Ork^YVoUKbOJ$Jc4+-Zfm4KgqzVPlrqSoe-okB6-rT9Df3T>cNufewEj9oy}0x|c_@1G z23*LlXUk#KHe?uv?|ePJ)|{7NCu;wM{_l#~K%EMB+h3jpF0{sB?T^mxjr)M9XWzo#-Yj3|g|#qv@8f>*JHqvsQzQD_ zm=tu*Lb#*1cB+mlJz_D<5E`DAMdLTf#_*DVcZ!bHdpGubnB6T=dD%HQY8P#};vIP=jesGaFEwRuB4Kq@WRv$$ zOWPd2C~cBqU@drTER+M15&9&2;Jtl8RSo-A9;h6Jcl~=qCRJI;|4VB(tBxpg$c2Iy z7@^bQ7FuIQFlFCLjFZ;E*tjr$WHG!ginPE4#|7NhHvfkNyV6pBo&E2{pKhRtlU{aT%5tZ4#Bv>S85TfZ={` z;C~2u$%&#`XCKm5FKxi@yS)jrT+p!|;3UU1q_j_Ezw%@OhEF-aRI(-0uU~+j9L&q3 z&Hm!rXvJ*R0!=h_E?v4>pvR7PZ7u9~!)a?5YVSn9uF)IV^!Bma>0OR=IMoR^?z`%b znV>>8&Gx4uKdU}kw7Qt@e}FY$X6h(O(l=EoiIVy<5d$>+I+HRmlTK49iHy0l;&pY1 zTI}yQ%^s`N9GZ`XZsgnyW4_O_m-9`)Sdm?^2Fs_+o^#%t+|QL>z@JAO?16Wmf+T(m zhR&v!X;-NsT`(NlrW?5rF{E;vxJGs4>qpyu11ob*PH&EyaUH!$57C*>u3TF1iER6j zh{!%?c}9<{eUW4nm-?ae4{4gig@1uU>N(b5_KxCxy%y#;)cC(LH(;A1w3!`X(SYfM zAoVipmS|@uv=&->Zh+!FR}9(2q-s23OqDU)+$&&&VwY&E>%9M8)TPj|moc4>#t4(0 z>YYHpu$SbM&b`-8?A^1=kug<05M!pOrwn=dsj6kk@sFV)I*12B%n(yVj+Pycb<-Em)^K zRthi4S>ZQWWuh!3r8=viXuR!LK-Bez~8DrCa6$6x5$CD&-{<(}M{9@Ayn zK<=Jiu8l7iH^9jO{RrZLL<=#3j)zwP0iRa*O56f>CU5s`O29n}x&ZzsKg!w3Gpdnn6+)n=iyANEVOi`#w%ss1ynpCxH z&3oZNXFgE4RNy;(ZAHR32MaV{4I+5)a>-*xSu8kRi zGtIKj0lSu77icq2YZg8Pm|!}g?U^uZSRp-#^e`r3bSz~OT>5s{3R93CG~PdeG6Ie~ zNaY_t(KTmDkx)>Rjr86Q@Z`U5-S|0r5tfOkQ`y@wL6u)xE9XIzmwEs{<^J7pf?7KIodGIrMgsA$$)nELft-A~uRxQA z;oTwTn8HzFIhjo=YD%1tv+_A}KF_Z)V8Mib4=Z~uxtkk#K0RAeC=Mr~p~%8)mrfB# zd?=zHYu`>w#HImmB(XAr#(mP)k9Y|EGR4w7U}_goz3a=G<>?0&HOKO=%JS6 zzqX#Ul(>VDwy^zgA)OA}w)v{OYg&=;Q2kDzH_MwLZ4Qtqf@2;;l^#1`W?)>6(=%ZM zc9wk`TxBi@easWT-M6~U%-|nR^z#WzP0w|*Jj9>}&nFR|$^=yshDI4?gsvAd|FNT6 z2!3r<(Uzuf=(;T77sApc(yA_DFG?^1_yOlKwRNh4e|*eTW~sxFx}Kq&JktLaG-lYG?R_UN;E?mrk?7zeOoAkyL+B%zF^4N!YEO z+h&)P(YL6nuI(Cnz8lk5V2Uj{P?g zO4{trX7~>H?ZPf4fvvTp&Y|uOEYD%#)LbNaR-pPwN9dRKP^$ZANQ~BE7P+8_v^H?$ zQD*A{GcTM&QTgD++gnfFQ5N3)5WG_mSr zLV@=WbOng*1vLe8nh&d!?3*~EPA)wXvVZsTXSKk$LjNQG!5PpbHf^g0JtXHi+G{H< zWsxf+R99E$RDmMz3mn)5da`tzW@A3+PR4uY&$EN8Cjvqdr-80yMx zju1&}8m~WNzq7SAf|q^!O9^(Nr26o&nINggQ{m)*t1Hc~yF7l=ZPldtrZ}d>RFCYW zhWrYj?hOK}&UEJBnuCuw)V+)4kC~WRDFJs&_kuJz^)e=!=S=ZC)`@C@@7pWVN|>Ko z4!Afi%U!wMMvCyC94S%oA<-@Y0icL8W(U_;1951-JgJb=D&0S2Ehtorx1tuFX#oA= z9t81^QJ{0;2F^ghT`mKFP|*JMGAP5E`HM>Eh>=2S!@_4(S&R_>BO)pF%g-V-Z3Ci6 zS6qT~<11E_lz~bTMjanqyz)@~{%39#+?( z9=z|Di_B=XAl)hSuZw3LLOq-~RzmPDK77{cV55!2{r-Pm0FFs7tsxjFb}>zClzZMe zhHGx0+!6>4Y7%=-Cc{h8lIn&>!OHY=xN7HM|IpOrHfEI{T z#lV_Wn4{1e$2b28Ey1?MGd8D10_}V0(h==@ijM1Z0f%!APF4S20W@>;21Tb*5oymL zJ-GVqmdxkTIcZQb$u}?f%yPLN8uA$s`U=xj?{hR!*Hx>twx9nIA#H?yE}imhnAv&% zL(#!^0W%%6!zt`y`JLt9<_N3>Wsl+TJ3x`SuUAZ~@m<^ws1H;h7Ovy-D#D+FXKE|| zfs52!5G-2_h|Jyd0@(8Y_hQAY9Qc9{-QEDKd3+Bh$MR!FKXK_5Kfr7;&^+0dlyq2ds_l%f zoSielJtMjW>f*UUWN?@))Gma-d!h$30KI3&v(OAYLwgR)?n>L;%(1j+&9ql-JxFtr zl^o-i68+%1fPX*c zC6$nf@xz1KivR9xi!A1z`lo~0yJzr~M}cNXVCq%CAM1d0Lr{@N8 zrNbP6O~@Bqf+J*tgy%R7@Yh2~c*|y3Q?!B17bMI;#n_>8vkyO_F)Gh`!_Z9`KjboD z_>pJ@HNML|lnIEA#p1c_c>BeNB2<9knaoC1S^%|n?*OjC?UP%-i{)jseBE+riek6G z3N5L5Z&cCJ`B(g~yhKfdG0D2;{W5M`g5t8gRUViH6hwlS-(ctla?^1wfGG;aMX1uz z*lO-}bf|?CSO_L<|Hkw8Yu4fqhr%`{`}U+ z7T%>-88z7W~%{J_WgTR4O@Km+Qt#5<+dP3Qqs@EeKi%rApktDg!UhZbSA5BvL zIA}In@Zy_KyPSBG&Ps*7enC^P?3@o?a247Cm1AqaO_TC{J$z=>C*4#@_jmOGf;Ua! zcL~f+y7k}(bjSN-^G&vw8w>IK{~!$Dtig`c{Y7+e4dje?z4z^b!OuZ#l+%d4qiYF{ z(*A19f2}UlD7N$2+4j>9PyZepUK0@01ZIZeBh8U!W^I0U(HK|9#pm;8qK`P8?>%kV z%8@T|Vymv2ShZfYR$v}~Ki{U>Hf{=h?`{{`uas2bKy1`Nx%s0$(~uh@#XVrb*xn+- zIjAEI^J$T(zd>xAY5zLp8y!(Npj!;rUS@Dfj&9(?MdPJxu6b*RNZnx%aB$iers4L~ z=#)tD)0H$TtG7)_~V2F^Z2KL|vef*vFPioq)Hb!SbCVAAZ4sy>*+B&pSVdBtj&#l!>M2vNY6?rt|@t@N9d8@;n zRNMtweb@H{Alg|$j9ljgD1(!w!=1rl#}6x!2_wI9uq#$}(gXOXe~s~rud_6uo!mq@ zJeF32Fa^fm=?}b#4& zzv$hfyp^9^O33B${u+eASTYnh`xly<(W?80Wdqlln6t$pI^xjT%{R+C5(Ih)oyXz> zpg>SzTwtawC7#j6k!R;nI|-_!*Pc@ys^V9q?(1_$j;%}N!=J(}rNRHwV|MCCHz7Cs zoW;JZ=Bt%@G;ZIg-x8MkcuXJD#Z98F1L%+=_Hu#ncIL%D`bR;I0d!6;21F_&q`soB zPl0|PXJ?QH^=qc?%M5Op!|Rq(OeVhCQrC^K@#bhoEri!lVidXitBt&3Tx7X!k!h`7 zjt#aS#22!4WSW)q(kSH9V@aIspd)enbYoU_T5nop6UD?*030@b_R^qoH_K8IEr_d!KpH5$NUik4v2P-_AvkAzA*u&_D`oR$+D0Zz zc=>C-}=o!%IZv;P`?t&CEH0XzPa< zF|g;;X7AjD6a|g`fvwx|E&B4!QecpCmEyxbz;FBN2}k0QvtwuydN1fG-A8KBtF{kBzBF^Q@ig=?iHP6a*jQLS(T&5ba@dfK_;29ISY9!~= z<8glAb(ry{5z-C#2jm~q{_u|%=b{2P5uJW{ZM|;cq_ql|BA$Ndx+laeg5o;Y626YoMyBdAu1c;+5?h$LxM|Hnl?MI@( z+g-GM7*=firQ<0<=L4k8Z@MkRCHny-;B4jTnX+M9k6eBbtD?&}+X`61XHdYY)~0>q z*Po5P_Tp5ve+bTARZ#)A4kbou1OHG3?!b@sT3wG@RyJf;u1sh$jEfV<*Skhny5Ns{ zaBnYR9G|St`-|+=CK~V9mO}S*huB;c&K*3*JEpdWHC;rGu=2^;)gyMG$0JWvnf`eu z_SDs*`|@pEXF`>j0jdG$@xt@uAv=Kj>F8l)z}b@2{&_ve$xi3Rv&YfF&c2Wa4%i4; zT^Q{;W#^Ue&BBA}#eb7lLKsNIXAw>U0uxbDyDK-;aw`{x#Mo&3%TEF6#mWg~TDkC# zJxE2|sZ8ZyF`(srdI&jxD^z7y>NUy|?xyGv;|E2NKC4OURB##$IgUiO$8uaor9(TT zh^BrvzQlS5<&U?D?N@=lu(S$_t0Kz2Weu5ecKxwB;G7`-iqF$m{F2AJJ<4#gJQ)*v zEE&CltQN{Yfnt{5bg4PncnN=R(Z%z4M+XDOg)eV}kO)$D$jpEo?k|ICWZQ3;CL+X3 zoue!A{Q&KIVW1(}h`ONMMyS+p)3P#_oh}41voW}SF~cNa>_axnvvJWXs~_u9MaJtA zDu-~I28mNXVZS)DsAwavMF~UM;}GQ85ln8%cimKnQ;`*OagO(YT=#9jq7Bpoxn89k za1SMLhtn_$>gXyT@nh<<1KEETRxU9Q3-xU4dGdhcmg+~U75GzDyrx34_9@$J#~Eul z%ItsDfS#<+T1+b;M_oE&8?teBXO%lLDoW_2{VT#eJrgyA$KLdVt(My!Udl%`HI3iW zAAC9x4cXg+0Xv;1Ap!xkRU9boh=|1ke#mzlVd7kkha}iS{`oknnM(-&t zX*+MB#)=Vc8XX_FQL;Rk+%;yd{r#5U|Bjs!QK&^!A|Bd~h1^*^LHK~Fgo5~2GO(ZQ zFYU<9no&_0dn>t9TTi~6Ec1QoZ6|9pue$P{^}>>GBb&wHZ+zy9hi=S{E93Hm${bd^ z{waN2EZ0|9L)3ljZdqcPsO*=ok4yqDB7i1otTbW2V1KVt**~x|dt0RVS9p@_%{c8N zmgCc5*8K!$AWA~L;C*43P}`yJb=us@hqt2r`GHTxZFlyLmVZ1S*;ZQArH;ws0**e* zt(@1jZN1H}4)_t*ey4m>VO+-b&uQ@D%->;P{X$a7w#9>Xv=a z)rlC)-Way92r23}tC6 z6n978%4j#3@qLAsnX`NH&(2RQtYQa3GN>f<-fQOlpXk}hK3O|3^MA}1Fm-M65Wany z87cy}erII0p!iVA6GM(Z(VPf0TT4&dmc>FUyot&W-O=NJI0Z76;>y1YtJlKCYVbxd zOuCmAZX%#}2Ipiay%doL;}H&QS?!D&Sm2Hun78gr_Cg`cGHSt@h{-hElr)#2X2S7u zqpZZ=kYd2=zQioeRGy-Dv;(8`pJmcDa+E$8t99^!NU1+HvY&K4lc{xi@39ZQh~9jY z3QGMU9*4%bkeC*OW|=vrPyj6YQ$ESXNV%7ST6a}CftOXHN;vVKPr zgT&$e= z`yGZY4;cHO`+A=?K_;)K?`}@k@$Ogk9nIjaqvuMXyi${L1l@7qikU*k{^9-Nm2nt` z6T&@F`JUM^5E|xvYirzV^bgfOpcPifWOnIuGbesV5I8I$;J8z~d`i^jBK1#`3u27J z(W}=O(SczEd3u=K74YO9S=KiKzpH!h&=FsgpS@*yd`BFI=IIob;lPP2{k^)sG(Qf} zW&A)+;8x_dWvGo8(-C<))3#Z*VaaKFJO8lwbi|2pyT(%x2HKch`W&?#0u5HTZT)Ec zA%Qzn*%JL_o=P&>ay;IUti=F%^5ziAwqoxSZSme9=);RS{7@<7R>(;W!C4AH&s^IL z=}k=)IrqwkatS#@(Z*rEdL8zES5ruUx$q}Jxmy#?A-}2VbH}Fut(6<4+3#QXXm4JY zXkdZ@DBf9|RrtRiyT8&wKEK7FnT!WeBq%&OX%i&JY3#$VVl4qNX}>b9EyOflS2yMjL_v>dD&jr@J^i_jN#Yo8bk1F9nMnoC#rhAzBAEXl z(r@~rmER!Nm@;d_*yB480eAo&13*B}!|mC^UH;r5k(I7;auN__M~ngWc;0$|8Y|$7 z90X)X`OHuF8%$!k;*epQBKH$vI?N4tHaYPywU4#E&)C4?Hf;-l9rE(CWaQ=Hj@s+$1K#aqa5~_T~sbr2{05D4&!T{0(cl-24dwgPIak;>H?b@8K3>j1tv>RHNXwQBhW ztwG24$yse{j*rQ*MmH6rkB@gO9rqP&rg|>bS|LHU<+dfX{+Uead=Aa-o$+b^fW0#NdkAncB#Y_Ff*_3I%PA3LjZgqw`TPdwkpPv<(MLzqK-rc zEu1=qFy+Z3HZe+86`og2!i{OA5^_UH_kXf!suD1B+phn}{_`rXfS_nn#RWMr%ABVC zlgwpeyq#@_ZpVy#O2n3e^43(47@7^Fd<6+8TQ+!_U&G?~$lmpgsg{r^`0%v`o;tsQ zHl0CJfjQ=m$&JBE1S4olj&f*!pbkDmLNjT5y{N#>ABQQR2Y7guSuqA4Erpo6&85Q6 zG-sbCU3ggZWU|~>;`jpMoDjNc3i5Uh@pD(& zQSi|YVJfRJ4L=+EoyGQha$PY$k(xf-{42ejuDNun?wAn>@KVAT1sL$2IjM+iW92S+ z&cp+3aaXYY`tNmJV-S&HsWgxWjeQ>b(pl0^8OXv zXL2PFD8PA>;dw^d+0+*wq>VU~a0R^O-LfH1$J5JjP}&@5yo^si(s#8LjKyJgld<}( z9Z2fv-7BBQN?aOiZwB}BE^$Mp^NRL-ItyGoKB!tB4A^FB_E}uplxRIQI_Op`pnRZI6iqyf*o##G;9nGyslU6ZG=yhsBi5RENhj%&yaLNeTrqy|_Z z=9xMMK*CHE0vA&NhZrRjpkX$;Ii|kYa=>c)c_F>r%d7Ss)NN2))n3tCi?DF;qpX^q z$B^&4j75cI*NO4cm^E)6&*M$*5WkOa_!Q+dUl*dJBF2xt)5=8_^0>ln*fRwKjmiFA zvi&aBE(21CCw;yFdXf$kVDHPO!Hi1)^= z&@-r#2bUjwSKEzO*ODA}4)PO$PP;gwwSQ_npG5IrI9!+~HX;8FC=#g8-{#~hl&q5I zC&KxOvT}9o-T%(wJ%CSQpuggllPo*Cm`1#hl=kqX79M(kVfMdoK{Jf4Sgf(WFxLqR zo0}857YIZtB-CT!<+_|qaT(>jf4)2v{wf_=PH5dN3YIcPF)dbphne;_e^O(8g0vne zY#oBsx(iUsx>CJRt||^V*gRg{sjPA zwQpj4Q;^t+KEg{<>mqY zb!R!i@^Le`l#vqvP~=5KK)igmS^GN^xR?POu+J^9MFs}Ggv25qEXAbHR;*K&fX`My zjxU#D3GYrb538_LY1Km$GnT%0QOz;{v^})83W4lA8_p8J4wzEH6XKSg6I@BE^eC z5?qVB7nf2XXmNLU_uv#O#frNXhXi+rAf-To;{N4%pZA<~*7rBb%F4=h&&=L?_RMv^ zF);Mq1Gp@#;IP2zIihtnz;IskU~GKX9S|b+FefJNw;=+(%G;z~zZAtjzT@@(n|CL` z*3MF%Fh3QO4~OHdjX|s0mw+WVzcxCv?-{$(jd(`xev(K374R9T?`^-$zD@Ug?l>*@JUv7MhUow4Nh`J9E#AuT(GCP8-LwsnX zmL~d}^i+_jb?wwjddHgB0s4TU)*`C3Vz&uH#O~LE7kXizXNr|F{4yiOz>)OkY(PH2 z`PKb* zr~HUBzJ5V<(qAm*__$#UfpeYL$-~S^N5*t#OZS;L0n#94*$KEtJy=*@l#atRvmbf| zJ@TQ7f~i9e@&$I<(7&Mg)_FkI>`?bQ-w&L+f*1KHv^98Hfql?(e~3V3g9|6fddaW- znn7rd^YeL>GS+aAHQ;w4Vm^>W5Yc~M;#J?8)i*)b`w!@1%iux4qQ{!ae34f}F)J<4 zLpAXB=k0SgUS))gpJ{gV!`}pdk_DoPO)ixf^QXgrL>-X}AK#m6 zF=%f0bHHC@v1_x+=U&vCwPxSIQfKD5!A1Yeb%VzjaJH489VN?COuH>wZ`&Zi)F>C;(*${1J-9=fa4OH zE%k(3hR5+dZ!c&szY3tbxY0S}Xt8GwD|kKQE1_O2NxM zn;BBga&cC^P_2trh~xF;UkW8)r{jg2gV|V9wi2ytXH$`u=hoy(CM9SYtkOmmnWy0y zNi+_(T-+F@R(UZ{5h5J;!mw7Mav8(Eq~#!i+fv4_*s@DSC1Bn#RA~e1`4k8}Qni#h zm&u*qlSrY@$x>Lf0#{fTf2F$*1qTS$t7?O1h7~M|Ex^VPyJ4Rqp_qg>Q$OZH^yh&1 z0)ivfCrWYgDRqH_&hLW0H_D(z;r>j`yL{D{Jk;njben;Sqxqij$A7q$v&B32xCv{M zR}Tk1Prfs+Hcr)gio~9k`o`0*TxMC zO$Zpx$Z2f2`nRsOKX!-@-m8Bc%ml&#ky9)dhyGFDJzw1c5LnC%)&7Q{d@g?>jVinl zDZdc(CCzm;Z4-me;4TczmZ!b^V_CW68{rQw!HJ~WT;Ya$svNqA_kNq54@Wwd_!dh$ znWNX|G51z?G&e!a5liUPvSWPmKZ^)XmvoYSrxtJu?i_{|7ep|T%Z0!pzKJQuF$yRY(M;U;Jm&@u;8v*1Hm zG@Z2>O2Ca8v5{`}6~fu7yjRC%J*j|kM(WRO5H;wxX)5R-z9&1Oe#L}D-FTc`w=un7trkiZFB5A#>AXb zoK+T#_d38oXzr5g-Aob&cNc3)>+T!>-^=7pIEAYP$j2L1}OV7}7r}M;*lSwsC zxX7)nwDg-6OB58rveFf!s)~X4_n(cUFe z3c?@zD&Kws?A~UT%lo7w?7-76Mb{y_vs==8ide=SRlVi|IYmoZcwHYkyYom%UzR1C zFsAExQ@TzG*H!E!S6pN~#7^fOPF7`mBKBnzhis38mRT$nifyJ$l7IGtqN?7J`tg{VovWJt&X)c(_-jJ#1r zjs8!9KxVOQN^0D!+Z3KmsM5IS#fD=c*%(yHaS&eSNfGvQLVPx8IkJw@I=)<)pJO5s>Edq&PW zrHuUTD^;-(+Zq-}*nsttslY<$7{$Zv##wI-+mF>#CmW!l+sgG>OpOmud*#Hx>Wyz+FOFnt7TiT|`N7Ejm1?0sa<`NQ_OHRucYvGjFyZt!90>x`*8+}~Lru(XtNC1wL`yt?q3yFY;i>E_~Xza`s%StCG*4@w)c3pdmon2gOfeSAa3C)l0v=VW2u{|kB#k?iI5m;jH zQv6b3rcGbDZWx=?4%br6((+c;rJ@*$P{y=P3#eUu&> zaWCAVCzgI+qTFpoL?$e?3j1<&m>F`bzy6Z%_zbbBk~v{h^fmYo{q{1&Lx;^gBWM3sA+JqW^x5k#RSC87O&IwlswidZ{=N<$D3+&x)*A{2h-iLJ z&l!e}Da`(AXV! zKG6N?SA#z<#ifNbDiY|zBr6)TQlDS}ex_)UB}931Cka_FGQeXg#K@mw7%{!~TZ6uGA z30d}Pn>1g7Lh$lu)-;u-r}d0X=O>Yn_JN+&ZhoXCiUjgIE%nx9i;Ih$h-mD3={rItb~ z&?$s8=LR*;P;5R43=iB)uIm41I{4C#PXBCH~abDg2r!Di|^uHkZKVii%^7jbvq?M&@cVwgHW8c#M{a$pz z@!)D;6{9Q#Lb*{!S;CiKzY=axQAk)Io)BBIjWexP>{?)mvJhrZvWlT^cf=K0W*LRK z_9%4n66?9?oC@^AY&C63!-mJ1b#=1y&~rMo&G)#{qz-!wU={G- z41l)Y#J%lO52HlD{DsDEIc=8ERK`NG<9gck)ZnjgssoJ+&`ZP_k!CO>GV+ueEaf(y zWcFnW?(G$G1-|HcL6pr0`?}U4$P&Q(@mNLJAhU86VUQV$S%-sDG>D3`mGLa+W=0o$ zx0e^6u_gxwOM?OH8Pm|qAHF!LcrP6k>S4#jfl6<`v0CA?!)&9#L1;1_-Ng7quV6Z8LXLgNw^Q^=~D4VaGxD6>JCbBaY~{b=_XiWyncldBL2?tU~ht z*z*>ST!28?Y&F#YZo#%K0U_#H8y=fRa&pgLyXp_}G>VJrIhcKNcw^_Rj>YPyllTc^f(Zu|-!tT=RBnn*X@6t$z?U*6r~ z6sb9_#ifA~ep#Qbv$)jwlR*&X4;U%4C~@&prG#qCnPvyl2kNpGO=RekL8;(}I|;fG zRk|gv&n4q;h^Ey3l@+bx3gbz$lKJy=p zf~LhUu`c9Pg9@YMm#>&WDRuQM3KrP%pRtJ`g{>spvV2^-U^t+EPW)YQPZIXnSQ%~# zfj~KWmd%$~efIqM)zf<{eI^mU#(5NlZBz_q%I|Y53Io@X{{<~Xte}5`@82?ZP$ZT% z@}4VQ^vjV0S8apW!IBFs>{ouP-UhViTD?n^#=Gl>8EW-D8HsNBJHrVyGL3+5(b6eU zdOGC3(~jc+rGS={U#nP38!uiG_5qBVvDSQ-wX7W}A|~HBy)Z%Z-RS-$@~~&aBfP=% z;`9@`u2tsY!M$ias9d?+R~VIL+7gTZdm)WRAxY>YzT>l|jD6L85?5@0M(UvYz0kXF zA!xKOd0rnAM-o$TesIJ3ULlE}b;gx4%ND1k&a8rW1p>_SYry#h(~_s@Inq?ZcI8GY zH~v^tFef6A0wN$7zNT8P0%xFO^4f)Xk>4QTG=NDEI47AqlNoW{(=AWID(=^MPesukN z2qY|j#8ol=Z^f!sd#wRsh=sC!`SRGR9J+bFB{8hT-#HoU1qQz^ma_VC$F5!ReooOm zYQCU@)^os+8b(SPzPUFrnC?iS)+?S5J>rT;J&4JOo;=>wQDbTLOr9lB!yHvZSz}0o z)Kbt2g1>*X;!TQstz@X%yEXv_io(afXiDBc(J5a~OGuLq zL7VBF|NGLuA7tB=0wcGC7*b^3@d;MF-sn>37-a=Na#YO(lfJQeGW;$gKx-XS!8|Iqg8)GGIAJEJ ziWT;-;}46x*_kr2(U$E5eq06ubw2a{Ng)fanbrcY^3@Yu!t8Ek0w<2z@n~LY3>$7c zTOSZMdgo4gqZhhWkK3{EEbzAU&7_vbcbUE`k$UfF6j46HoSuLw91&VyJ!xP69uZcKRNX4Ft52+?BBqa(P z)!I@*k-?>x0Oob5myb|5v8&5Pkh`*R^y+?pa;_&&nVzNftLAd6*mer5^$!ZhHJ_8L zkVgAJz?hwH`--rSmru+O-{~)XN)@q4$*;y0*HMeCsYV-bV}O5`bu7=RWd7uvUej}L z!&m$mY!}Mz!*BKuydvN$qc^*83?8>j02Nk!)#Own6)U8buV*QamY%T^wGt(|Jx)dn zC4eD)?05p1awXFIwccM%xgek58sP`BE@vyEDhU~3*u#Ucne?foGD2M`W92@S0RZuG ze$w+v7Sa`xMJW}|%v$Dyl#+`R4~q~q%v{(*v+MvSWoI+_EhrSsn`XUJs-OV1A^O@V z!8|a&j8ParoiWmt=~vmZS4H$&Zd9hMb1Pfqm-L{YBm{z&WeTEV0!E}|!>)!e>PM7b z`O^d1AsY25?KqB)LD_g3QYY5A0=8K)ML{oQewPHTl<2=L(xx`sopti|XA;a`YO!Mh zHpXjaAq_d5MN7-b4(Xa~=tk=Q7tWjc4~QI_VEzjZ|MOa{Vcw1g#6a1cxF=tvS;D0} zRdx85^sK>u+}PBGSJCpjlJ^OuoEeD!MtY%S-A?05MmH6^O+#9!oCyWr0KzU*F*DK^ z)tnEKij@W@ns8*d%!Z_-?7y0z2i0Si>oXLCEJP^_wz5=JipLiG&fde+fQ{>YN%&=l zo1Rn+`h>;(IFjGHR2xr(t39cPYG|H8VUfeJ_{o&fxM`qRNI&|__hjrCfiEAv zi0)bz-R%9~gz6N@5-@_v?Ob?t-sGqK2t34Cl^DLlqhov(T?-NT4lg1c07y^4$J?2_ z_LkhJgp13G2=hJv>Qx6!`D;y<{1W(OA9#pn$n01gAAZ|Vno`W;mWod_*r_1WF;pfe zUHI!TACj6!RQpYFpj5mFegSUkx6`nh?`MIhmQEw&=uyl@jiU<{tWqG*!Xm~#YkYR< z!u`eAmlV6W`vQAL5GtaxsQ|mm$T{ZZ%sz%JjSJ6z)u`v}+S)5#u<$jZR{?nU@qHET zETStZJbA7?Bg={-(ud8#0%WYPeoCU5YGkdH>KYq=ReNvO5dPm>$t7d8ocgah=idXY z7I!X(c8NkBLw9ilpsG?yoPG~N}jLP4ru%jfHVz;++4s#C{8 z-duRUr$HYrDc-3ZzoSw)p9(2wjV4Cgl50pc&mD%ij(Hi6mFz`pxRRAE2zERqNlRd{ zzoKNa`SeT9s>Fc#%?Ag^7%jj2s9g(k1e{;KZ4ZxNB?~z~mNd2z2it33HVd1-^;6-^ zK||M(%ptNoCol#bL zRmz0K-#&CfO>g#RYY~nP*xXz=D@@)3Ja+|3B)UA%KPZ`JwoL|2Z#fZPKLPaau z`G#A-qhX7|^^ON#sarMGbM8mJ&tD|~k|z87X9_x&BM%@56qGFaRciMs&#gI#BIu5v zu)ToQC{KVs^Teh&F?8xXOgn`DakY(zh;rH5=YxLqM11BF`2lY#k0U~~LOhN0rK0Ew zPKnlcrxbJmqjF}sptp+3@>=1%EIh2C%tCK3&tj?dyfOo-QzQxGmzK5wn&z*-GUkj3 zmqQnVT_Ge3iGg@K!Fq@M?^LOP5-{9P-})`&msGRa-zOpE)+PT!friJC{kvYCw zCHhHlJNG*@yKJ4eXJ{B2XK1zH^gi~+({}$v@-7ZPiv?5tqEXg0rx>#m72L5>vY9%g zk>R8kXJsgLH$=(^q+_9b%ha}A*U(1~-X5h5$}}U;gg}=i$VPA6n6&4Y?q`vl59nFe zW#}VqBqlk6uekjL%2tB)lDj@7NhIney{)?&@8TPI()8L zpqZSGHO0C9Os%Z11%bmLWHx9&K?ed#R)ZE`gtiRK7OlMhF&!jS(;aoOCUnCwt`eu2 zWd#u7VU@$%0DXq5=>U9PRSE=eVl=PZ&B&J3rhwwXJd3+V+hFTmk_A ziE^#zF7^O$_nW1FYj)zX)>3tc$;GurU2~bbR|z0(61+CD!h;lgw!Smm{aIqHJzNqm z!6ZnfX852R7NaOnZR0|8^U1<}>1@$~3-XwM3HQ%JCF>>95>W0seuZ~VwZZ0%1f?*} z*6k+0r^JtU3@zBg-_IQMi_~B0{Jd$I*fF0Fb^^fAy{Oh4c+15qmOR%Fc*%fHHn}T* z0#Hf{r>wsEepql(|3Ep#{#7zzT3+`ML&Ahs5W}U=<{K4?#xybo)5pjxU+4Oc z{aq~^?{$cWB5V{Ynbvjx~^Nme&F|s zUIrLq0idWJxNdt3kzcdaSqM^-vI3Lw$wXZ*7w+yuhNd1#zyI=Y;8k2ho*@>iP_d%I>BM zr$pysydcbUz?ONQvDYk}5oSpccD}c7U_Uqu$ucrv>Dh55;O@y-0*KQD6s!eyuR`YA zcyyaOt3R-em5}m+8T#qvUd&RI>m<6CPiY=Yr`Drpg=e);^7nvsAq& z#DGG_op(;R;yn8QZCJm-Uy(dV`X>Rj;ZTkS*~bFqY3nJ`9SV7RZ&!bO7USxg{x7_>r432IAS5xa>C(49HQro| zxUo!7?jKTn1-oxfV+qq!XEXBd-F$#l zH)Q%*)_1~m@CLxxABqsQOMboZH2dK$_b9&)WD8Eo)72V+)~(tarc7bta!GQ^L1gR1 zBQS$~5Dpcdul)y2=fcgb{!&7x8d$@2ZHl~=AT2Awcpn}C)& znE!6WA!x_p5)nyB&486wp(1C$W#U&>g*7}>j$cv6#g}b`*aW45xAAvYfwIo|Q+}kHbd_QbP2Ad5YQw$WvB3 z{DdfsNZ2ZEyMCbMv7To3!mK<`V_11Rjy@-3pL&w zE_WuXkuYDRC8huLef#q~`Y0#ga_iR=up{Na;_zq=!T{p>3-<%=508`eoeb%*!Tt?K zA?KY)cA!tZp1%HPJG-Ok4wCxoIkN|x9j}(0+J&3HL&os27}f)C(W^x>OE!A5 z`T6;A@3k>?Yy*V9eMXyizNFG4)9 zY*P^oe=I4O-}0m>#+dE&1z?*ND^S?QD4K#bZOp^K<|HW+`GP4*F@r9TNcGc}sMNJ$Z9()%qq(J*m=e6cPA)_)KdzhCpK^#z68(^=Nz7HR&4J4+`;~RBz%X zXPg{!+z1b<@H4(%ew9sfo9#dK&#Ilgcx{1Jgkj@20A5e2hzE+%yD;uq?4DANV2SHfmG2-QA_k+GK)cysQ?WZgU+yxjByIq z4?Nk>*zx@-H#!peD`ukbS}3#&wJt%eJ+NV@C+oD6n*Np`cB4L4388aJ zUemv5*jTs4a$2l@&W3?{l9^|xS!?WthJyRiVlQHEs@pvEwqAI>*-$*|m2vDz#SnGC zY8Mly<`GZV!#_{hPh(X?qHJxjQsT2ZvMHuLS7HFK?sGp;2=l7y@mhJ8JEHYrB&zCG zBgdIuYVLwBt!KPcoJQyqs=WiJ@kXUJl9M`6)~wGZXAC<9#kj zc1SNMC;jjiLRP|(B1K_c)3YdP@rn_XQzW~%9bivi{XkYW|JjQXV67^fX|2$61AL>E zZ&X0H+1BWWgcl`C^ffK&WI$BN5&8vpL;LJ-XlM;~w1T}+hD~uM9PhJU+MG9IRUm?M zjK!;`9OlMreWeCyGQ?{*{PHtBikh=YkYA)o9`)7UyEaYwBlDV7&>74~{|l~tMr66g z&Q7^<{3kO7mfvx@FPL~I)otbRN*~{Se6jK-5R>0sqQ2~7fpMVP>rE?)!yDMkp(^yg&xaMhV%{b$SpHORRY*286W~k#1tsX3QB`l)+SbKxb}w@YNW!Yb$UP zDhF%nx8u!S;sn86!9py+w;5TKYhK6;M){*kaWcQ4;kuavaKF2&{p?GS z`NHDmPYKG68iH`DAmN;qa2!m;@43Rd=k^k%Y2zOk+-<|tN7&yd-sbHIZ76wCsUj=b zY8AeSmwbnUPjIxVWySHK)!f^nZF-na6$OoK1okT@>WGf+%eGf5x_Rlf$O426i7tfj zT_;l8iX;Z9pLfvT-dBXDU(?)Dru)!%n?qM(5v!sTHAuH$>HzC?K5(04e! zg_Y0koD(@#a8#6OnZ2gX_ZSns-tx-K1&2m5+py3ouNM-OZxqD3J-m_W5||BzBZf3w zQcuTpq?zi&Gx@`EbO)E7qk7Zoy6?<+jqu*Jt5tx>S8O9jCBX>kXdwcpBYh^7_ zR`8|K+`zeT!~FYl$J7wGTxzQUvCerPF3+RomMh5H}OBz*woGA~{H{QcK{=$_P4apuQ{BK<6U zSeamHfJHYnbW6ge$hp4N6}*S{Mp-O2e6+)u)n94^nsocNimmo%)#!7~UhleK* zY;7lsY!LpC)+5LIDyF8NyV`=s5}ziPZnqS~Y90eJ^*F@-CxvV)wB=c{FxKzy+*aBm z^potVO#8`}T2nhHR}`7zpssOV(MfK25sj%X9p?U5<}vZiixH(YmG0MvfE*>3nS*Wq zGh(IX@%(jNsoXbI<#V35fB#N;F$@*{X=6isnU@hM9hTHYR4{E1OnhyKsux&Is5QxW zOME@*I^(3XrI~#iWeN5Akc-|l_=x1P%Uj=xROE*ID#+Fw<%%#>XTd3O%08o$ z^-zF`DYPdEUplgXRr_+dL=HNq^d#*Ww%LCgjR5K7Gcx;!#5LM)c=( zL@8+Eb(!<9oyQSM@Ai|GjjvI7XE=lpwckicwcKfPL26=tYRv8Du2HD!da%y|Y7)T& zmVN1okWtEj%rpUy%J^WiVxRz?GOkFbQHaj*3-u78eL-Q*hOvzFF~S(>)Kn|k!iEq! z27QIlbgS~ID1>#1{QhcK9$dYP)Wj2u+ zQ^<-T@*CHcLQn%&FgZm!0aBOn?zc55I;^!W0YM(%8Yz$jZiKZkr-x1fInT~VJ_msY9B%}OFJt>&)KAT6uhjY5{esR5k|3Xz&{8Xw<$ zCSKfhqk;u1HQ|_4aC)uwcdd^#QY`=x^YUKj0k?VX--z1;^4VXRkF9_YG9crlpGMMtDBD94;M{XsD- z9lwMK@Vf#crxYar23%^lw;AT~nnMQcJa4Zj zFoAu#;41(+%s!D5xPo0U6^0llIBLy1tnT)g`v#bgV&utAPqMHzMwGO({z@8{JPgYJ zxH3KEqnd9!pmbg|4Zv^!?oKRuY3^%%EAcvAW`!yZ>1q)RTO+#bNG#WnLrpt1?h zZ}dyNsFrf#@T9`?k7u#A+46P7)ig@V?#kYMW!hLJUI|v?AnGcYdGP}9jFamRTaPX& z1HJ)W7XDnCtA28tL=arPW`t9ndkPAhB>xHXPO$&U{IZj3flav2xctm8^OvhShF0?G z*pmT6Ije<~m&x7at58I%dKdgL2<*D(q);EhEao%oWF;7KJ+SoKrX~er`Ug+KZ%P_F zSZ2|$G^IGhdUN+Gb7*P?PoJ>#Cu**IB|L(Ot)u6Ktq~|qY|GwLIgt{&4&Emlf@|^j z@Je^yt!nN?b-rn3nJOkTQ}-e4MfDR$*UqsF9TuwvX@#&<>}hrWWqE~X*23iJdI>ah z^Cu}oZ|z~JJP7g2WqLAhi_|_`q!M(b6s@DHPfgzsE|m0kQrKbT>7~E=cP-s!ujh&Z zahtQl-{ikdvivS7R~j)R`r3!9)4U`?J{e^WUJLO{LDVt(V(m{-U{(%p)^~)rqV;Dr zT7q}^V1y;=5dfqE!Sf)UG$O#2mz5q{=5MOIcR3JH6k@ni_!j`=!^?3B1{yP|z$Bc% z)X@Stlx+u#cKPsg`VUhwvVJ0RA2BXs$3GXQQ+US-!Xz3O!%V#}KOfG~-gH$`cxYm^ z{kW#2q<>&tYxvY+sbN3wTwch3*K>cxX3m$Mu{>S%m=HW(GYoHp;s*7bWQB>*wEs+spA1HfBVG$aE+q# z{(0NYBLVXu*37H$^FI^Sty-t7qOpmLuHj^bz_jeQ>@}o`dUE(AWa)@5Nkk$plhxKF zpu!rb3VKq);yC<_Cfu2L?wA>y|4Uw?pi>kzuKj$>OBU!{pAifE!@!E=>&deYneCY?xHO4#AW) z9b9$i`tHwW`x2KKyDJ#2l|MV!`X4d_GN;)-74P*r3wX4c0Zs0l54|CbVW8tllN#ZYx5c1I)Cf$L zqfrC=u7%L^@@Oc!&Z_ZX+mUaT!RSKY&0ZiWb8W^<1RI zjX?)#HNg)?Y~1jDc?p3OU8-~_CUDontb7TE22=TTW!Cw|sBFB;w&t^)FU7_+qT}O7 zWOjZ4HJvjz3+gEBfsQDCQFuKx)LgiE)NNW1r^U;PKiuTUPP0j~FRO2SyYP95R$J8A zTbTL)k_#>=8*n~wzFqvb&P{dO9C23jC}^K8;$bp^r7AN1n6D3BU^V{vXWy%7{0Lci zU`p)X_|HRBw?%D{xVILQ`{~ye<8hD2o$Q-(hqh>d;G<05ykX`9D$n|keRb=+sn0R= z{!^#pRL;Ys>R&7e|MZ2Yxs6jzKD%A8mCKF`&Z+K}_8$>0dogG?ZUgn6-}7Svp3g>0 z(4=toVGH{H!T$KIbiMnjrtjZVdXr0@`*P!kM>M^M43D$+6EAemM^$aBJOhrTe37l9 zhf9Y?yb4XR@xwn17(;C37JBqyCzC@-tqFY{{bj} z;J_bSdHm#0gGSc$D?b!{_pw{dWD_PR4$fN5!it+qQ}MS#`J)!KWWe7+jR&zO654}$ zMn)E1TY9PUu{wq_6|b^j#Je_>L@4i!me&~9)TR$U@J+h_CPbC%m);#+eG{UoC{xjxW_MR5RTmILf?K$BRID}VSX5Z9_r{~h zN_X-AJ1y=Rz`Tg@BoA+t^CDo1#@ zZ2cbFA1>4tj{0<(((%mWLgF6r0ylR3p7o)xlVq!8f}s=jCcyZwi0{$LQ&zz30Es65 zBe^I1+^}=Qw6tj;Vcy4NFTvzlAF;$n6uLk~+HKw?QlKi*-|i3p@-QMRI@Y_(vHlDD zYU4`5__?CHzxhv>RF%fHz@K~g#vYE?sjBeBj*}o_#rXPPjjl&GhfOhG_{YBc3%edN zlObh(F)@!kv1}7~qG~rBF_d2~)`x7;nE$(^e#I+T`sp&@f&BS0z%Q1QN}u7qG9ciX z(s$@>KrY|9kRLb8h>?U9c|uo0^Yik_dGCbqcvQNi-yVy-_d&v`V)rew;Y7)8J=4Uk zK)B+ih?n5_gin*l`ubsZ30ug!&C{*EqosPwvBR$8FAi{J2k(@$f96Lk%X7b}3#k{? zt`=}_1mbfM!(x^39SghGrrao^4c&2{VynYTEinT2d*2#yS`uz1SvaHLmtG(xon914(ST zQzCWI&D5vovN5ZzqyBSCYQfa5QCU_%fVn8-B5Gf*VSQ6F#`xu&OOJf0~w3TdP!UymW>Tc-*w)Mh_IDtBU z(zxRX8GJp@idpU`Xwvq)sFjGJ$LX?4!!bK87jZKx&w(Zfxd79PwICz*o9~@mnj?`) z)9%d*=g;*X>t|nPZmIq0m+Er)<+EMs556|I&PXt;X%`-K>J-MBQBt&*4DrVsX^*kM z7vcQzA_*Imj*gCY{F{NSsZ+!Xipz}3SzG;0cJkd~^hg*1u z?$1A@vA6E!mE7MoNq$;RB+b_uxcwX#ta?dv0BB>Q8xyRfEgHrMHy_}#kpF1_z$A#; z4w4S-!xK)jY0!U4M=RGiV3 zlZ%sbPV%)3Ny^N?xj*#GM!Qx9ovw@uDcQ`OfDz}l+(jjm(9p>SW9FI#MJ`giYk800 zb-HXVC?jD^Ow8Hs!;SAene3ERsuhTKr>J1YHSMBdZ|YrfnfPRwr$!gL8!A}j#KeP; zX`?-O)X&o8&*coMF-4jCQig9!173Z?U5aRkaNwEH(J*${i}t#OAR&SqFZ>L#8ziv_ zQ6ZOy#R$Xp1&5aX?rycRTju-*lJ3p}6^ntw-6-0s-`2 zCI|{{4f5LVH0%+pwAjkyw~uvtzeV{tZgpNeZ)ake(j~rMzs1j6Z*9&fN+GTAy6b@7 z^aaQ`U$ic`9JBy*pMfR;#9zOeP>bAbJTSz>=MN~yC@!7%10iCT{I9k?`aT6DI9-?D zpAFt7oX>U~Gmnp{+S4D_@+=z;Q6BNk7X5ng+$hiSgtIgW-Xal?lS#Z%^Tak9e0`G#V*tU=_`9F( zSkF9#%TA%ZUyO_>`8Irk=X)>$4b6ABuF;{36r{8uE^~S1?nhMm&yM3ujw;bW)zp^Y zbs3E+R4zT9O~(-=c%bD@z(K$|`5Ttnyjj&!qSIX|}qD&h`3hm>|;PDmiRS zR9%Ogf+bWc9RAJ&`jPK7`frr6Br6vTRdc)bYJRN4X|9Uc#XH9ptZ11nB^y|5lnF3j z9i}O$dJ-oo-!Vi=aT#=#wJr-5e3Lf6_}2cSGSB%St%M>%=_plS32DtNWAP&wJ^Fx3 z>OgIBleVn%Pz`H8^Gggn2qao|ArYyZiL>H2PPKtDS*Pa5#;CSvL5x_gbrZwCQ@f^` zqhot;CbJ%WHi0FXNbN7~B9np(l%i5M=mbJxvt;tZftW%U%Go%nB@WD79^|o?zN_t7WQHz+_74ivGHJemg0&?4y&Iay2}3G~=Nn zicP`)-oh9e6`Phk4lGAUlZ0Xvmzan2;bihy1SEN~Hk@R#ddM^t#xAfMgm88aeO&QIf4;%zGx5QBzCK9MQVdzT3v_AO8-*{`q0K+L zaQNi^rD_-c`?~pPiF(9v=sCBYZ~xJgg0l;df%jtS84iVw2@oVkCv4Xac+`226e|dY z8jAlAAGAXwfHnO|g)PD#vX@LZKLwwC>}dUjtuxV;$=>ag8WN3V^L&j>t>rNC(7ie~ zE1pT@us&{~Lk-jGdm4Jfr1j34_2#B(f|(yaA?$G~{_Q@Csk$`>7|;CqD8S*H4u=@j5&e@r+zXvQ|kH0DMG~?R&&w#_(0C~{_nsU7t`ox+iyGQ2*o>&+LY1i?G z9JgBw3cjV^kHq&2VM+pC0wf*Jw3e6=M4X1Bt3FS#YgTPdYTh#(@}X*`r=-J)m9P&{ zCKiGl8%-pV4#=lHhRM;AZN`V+N0+(F(iy&lR!Gmwi>XXO95Ukk0D+)Dv5a*D8y- zDNl|>mM0qV5_jII=LUIuC!YSjZmW;mvTRiu2YkLv9KNSCDq1658%s7=HEX`ie4>vu zWAl>gyZrH;XJplxOF_>1#|$-Yn6(L$i(Xd(msMUC!!R;V+#zP0oNi$i7Sf}2a5Sy z$zsAi-6WirZj!O$1HG#c%efT#jE#TwY&u!v{vTCu9Tj!leG5ydfGA2Rprj1lLr6>4 z3@P0;q;#jWpmYu}G(&euw{#9Qq;z-po#(ml@80{aHQzP=pY=KC>=S#Rl&C*fyL;~I z8Naj$jz(t>cylB}v-EC1NmXq&y!vAFxW`;>>A$)H61eu>H1Oauf&Gw7U$xtgU{>hWBK%t&5!1pC_lLDh)f(skon`izdA-!4_jloMiqCopxr`nP7zx znhQLqi>^^-p?T0r9y(J#wRx@eqJZ!3>mq(|Q9vW(Rn+)oRTVPG@DcD~F&k*B$j7NW z9Q^_hGoS?TMaLFK+j4A5)x$*~JR;MkpjyYKFiHYS`;g5%0Mk4uZ!ta`fhplo+7vlvk<>dXV zX`HLkef3jFj|Gj-#lSH1AfRFp7 zZ%UyM7g@}XdNSOSn<7DXKFsMfjk$jtZkzB)oe-75hKGmw^oJudF0p#N0f2MgQ;wu# z23A(vYF&%QulT1jV^hwx=b((FS=;BS)Qh^-Sz&JJ?lA^0$^U}Q!Tnk*`8RQUP?t^< znNZy!mNtses(m8crW4ah?JINrCL>w^Omqe_Bx9F{D{7Fm{i(yqM^?wdd<@x#x{=syvGM~~l z>=BiB_NRF>4(&Cm(EG;(-N!k@5swpH+Ph4&udoiHa^0>&Gh9m9|JeobjgH3;alRV| z^U`_>D(L=FF}NV35b|N+ZF#xIwv;-KGYOAk)u>uC3Z=1CV7f^$ck_(IYy0Ii<~q$@ zX!;3wiG*2_8_sY{HUpv71`j_r2_kF!RvEt8Hc@zuGNVJP;-2L7MNYZ(bsSmd75iKJ zYj_lw^RMi?tnJqcMxFNFnN5qHRpButJRy3@)#%vOPbZ<$r*}`_)uBNO)4%N(-uK55 zx{i}7oqm4d2&p^~yTA+irL8y@fa z?gQ`XWiVg74mHDLH(z??uAtMd8v0wJ)Ic=B&%(OxUDF#g%p9J$oB6i%l?Vbl`*sKv zke(s}9p$K|=$X9|D{C{f9$1|!xv&#e;smmw zJ3AiV=$n%h_Per1r(5a~zl#@rsBf+~ezBSU-t>Gv2{@J7E4c8A;4VHt{U#=$<#46> zAY9e2PgW8naOZvLwU3+bb&A1?w8gty_lkS(;iPc0W=A|fKf)jDVGy|z|LbtIC`Oo6qn6$KCpxz&^2oiw;{-45?w|IXwjGB{m@(XjZ@)<6C|Po6Nl!IAaL8!z@BFQ_qirN@)b zW_Nn+7aM*4?8E718IK9r7O$m89wG)Rc%Rz8O_@IICE4$Llq&H=-N*l^P9)Jm68Xl#^w5jY1fW<~nE~{@`T2BL0fo*i3_7d@O6(E- zy-b?BXJ(1?8_iIF4o4K{I~~^IRj>WSl3(_S7kFIk8AXFu^@loibea_IA7!1Sn*Vtm z@X%#9wbmDMdu3h4y!_n4#fzOTrFq;`hu1 zV+{QH3vGjEM8yWBUh^6_B`1XO-7b`X8RYYhF^^FZPabNWbPyr_pS269IIXB zl&gAUDrpR9(i?3nj*_bUrVO?7`fCP=+jzv(2uj}cm)It1-S!u=dvZP7*x0a0k@~*q ziK|L&E7QHdZv5qq(QqyYPE4|89@*=rbsOpgO8{qWa-=QkH4*)(xboO9+QA?lZF)Ag z{O0B~ZtLlaGTYa_c!cg9N44GAML#qY<-p~7>eCE-d%8(7`fe@WT4r$ngl>>5BtT4u8rC8;Nhd*J)aVeM`-@S~B07*WX;*yE3%%6M^Xa67;tCQGXb{U_bfAOYFEbnAVaP)jeZY z_;Y|`jd&(q%12Lf9iTve;Sf+uS&+Aev_6bm@I~^tSKs4sd~E#}!&LUKb*9zW15AuU z#!^CGzpUP4%7~MUBrE$Gt4&RQL^AbX^HL0A?SPB+eluS|sLaw|c%P6k#UEUEk(=?2 zGVgykHV_=jc5D9hIEJBHR}UC*Mf$SWHj(u%UitN5^A7jjcBwFu4o3prjKW{eIuO=U z><_R!-Nz^9;8jKW$P}2Ra=0%1=}u*-D@k$j_1zFn2SbLbP%szWr6Zh<1;#-DzR7Xm7RWep)oN50{6Ew*b3InUyJH-CQ74;H5tdxm2I*2RmhZyyyaeU=?Kt9)Ec7t6doe zt<0qCS^!!)`tKDW#T=Y|I=7##BZNvMxXIxotwF#y^>v$iZp2~_w$f*_CKUD~ozTL) zaozdNQ^Kb!qxUTbD?dXiHi}wQ@{06Lg!FHH&9z)!zk6b@Tm1eKjvH$#dM#y;koaUMuLJ&Tl>Mc|IrT#jJmLhQa0(U9}K)!U0=j6q9;J7r6JZuUhqLV_T~l z>T|7t`|8;?v!CZXHm?1@!{x01W;+feAg`m`?p!^&B2a5H8v|O*n&+R*4Nko-=PniZ z9k(k#iQH*kLci_aDveeh-7t?nq;W+vc%-2QA%95OL4!J_;g&FPM88k)DpepoLgwEmhDHwc%h`GmLOwM zCvCwWpP1-4AxpQ>Y(V)Ii(f2cHYj&C2-JWr+kwqsW<@ltP{h~D*b8M-mbBC$Rd_xZ z2iExLRjeD`vuYU4Mi^{C`88`%pjs)Gp6j;+HWDH!dZT8dm-QxAf;q3o=7Zt-%{&+0 z^sRTi6yjBzwv1**8Z9(indjN!V?ya(U#YimzviCS3%LU$=6tZVuuCUdYuOIi4Dy7nEdwFZjc6`6Y%-YW-0ZIGL!@{KJ?ghg_29J6K(LIq z+zcnl<;p7(`Fp;tm$~x8)5Iz^575NTwV4;!wGPV_=xy1d77w2iig#7g_MGa0Z(=3{NYT2J=1WH}EHzFCx}rKs8gDY`!Wx%TL= z2_3gk)q{CHkgR|2eQMa24P2Kd2(kb=NZmncOYgq@B`y(akE;}jOV5^1o$TjABAFN_ zk>lR1FG!9TBNOyhv>iVzEZVH70JG{G0cwG%)bRi+ckrQQg61KAoRi?-W?D$LTT#vm zktEeytV~Po{3R1)pN4Rqv`Qx&K;acQ9c1oltdJ}4+{eF;M#x#g6OKoKb<4QO#04fm z^r;}0!1n^Jx+NU<$uN?bYhM=C1_bNx>8I|b<-gVGD)dBArf2@g`e3pbM0}eRX8o%a zKTGvHPfVV6wOL`wxmArzL5|pkrW&j;KQ6oUV`97S4#IvSPGZI8e=q4<7yleyq$2ZW z@-hwhx*gd)%!aCsOfhTI;J|gkD zuX#QWEA8k_m-3#aO3z&`?ySxEmnIAM44TL%D&>uWSp1Nir^FI%x3LMQ>O(O7g=ss; z$KaUIjNur?jNV}2)k>{Nr1>Sbbmd-OCpp1wY1x`6AMNhHR$^SMR;{6w*{g-hI|JERjswxXw*vAsRmcJG)A`I3jEX7NABsi`ktAk`yI-%&KW4}1(fge~r)NJ;_Sz1OAIaEGb|$#F@N+54jBLq72OG*Q zygptWB2ZzZVX6A7DUyk~`B7N^Zk4V`hcNu%-Rig_HEhn#)rMm8#J!-T1Qe5UkX2I? z|BJOzCO(x23v*Fw3`={8+e#)J+y|Qj<@duTw6&G+_|6+6k!d9qwi}HB&8iklL6TnJ%{P;N`i}Ml4SO84vDGs$GG#q(unT z_!H;aRnl!45I?FS4aNzp-`Q3Vyu-%(72E)Gy^?$|A!4=<*ly(8Ak z=J0riDcHZo76+P@WwqS-8`B#(c|Hr!hWV!K5Sb63`y<&|3bifsCF;v6~Oa0sn+%YU;7md58mo_fI6h&M(5&c0ec%MNaMZq%@t<@v0S!mBv5T67WNVg1OE6tfex_TWhNpH?sa@B| zz4k}ix92YstG3HNR}8$BiagHpdk{&qs8F5$90-Yw4V?CAXp8qec5km39g6R}?A=Le+Sd|+pHpllAN5WWwX>%>PFY3Ud=Nbg-gz^NXo1lF)Q7}62vKK@?t zWYqc47)n5q%-X$GBus1S~>+;2sFX3vk&?}gE%onFJw^-t7XhK+dhh7aXc@%mhOK5e1 zAlh*x{|TFsVBy7bwtZ=H8}OslO1`2)|pJ)%P5A)qvU_q+mWjvEu`?+-A{_`uqydT zM95M;oJ~c&=<$kz*pJMNEU^L@1`$?X9!0Nf9 zcLusujRtHr93>BX!h-@q@)coF?|%ST}cHHsxBE^p(mL}2}Q8J$o+fhdfT zl-?$WoHLW6>HQpWufRO0}BmK3@j`}J0SnBW2Km{?|-~o_C$52tzkh=?Gv7PV^?Y+ z9w`L>G!yWZn&lv>Ge?0KBaD!=@$pwC}hbRgy!jk!ceR`hL;L*nv!g0yON^MgU+=K~ebO z$=K|yOk1okgE~KStXnN$+C``(3XUq#P>$WUTkR(&7uF5Gt@t616ibhU* z=Tg(&*Jy+5C!d6bM>R9ro{6wi7!WVB20FIN-KEdS4b;Z2Bp-4UF>dPsk6N@|3M7 zdD1f+g#I3CXQ5NIZ0@|v;Bx3rzf^2b9QAlQ!^#8Fa@2^2bSnB&r0jkKE*q<$b-5<@ zm=`udruuy-(^GIc(uJzCKJlf{k!yA@J|QFI>scHh++Uycw74B#{as13-&a^K zv06Mvv-5dx@~<_nfA;x$rC=Z}$T0c@Z|Uw&OxG1iKpwc~+bhLlzPsX-?73~@D)jxu z2jOFM5pupz2O&t4?ypt--oGQ)7-&QA7o=d~)!&mVK9x2tQkzW7g^X(pg%3bH_GA;N zV8@{9QR{G(hbr-W%=dhI!KUv9Bc*t3CXx%I*RnY)tyeb|i#xa<*FC$_7ZmP>_|@r` z&0Y|!C*9oSPi<>m3Biz~wGUd_?3VJ3zmweWT0A3X+YDa5;oLa;^!4e{x;4zMzvn}G z`(c;qR}-~{mzJf~1<0j151X7Zx2r+ErE187ajQNR@FBqCQ;Ymt_CjJHC14}^q9wBX zhbRX7@@>q9a^H9raO?75AKNiZT2dL+U>N-)vO%dj=i{_FWT^&$9E8!l#lnP9T3}+6 zx{_$yh_3wvTX5GFN`31TomZdZR4r6|csmuZ-ikYWFps**qBYl?z#x|t7FI?{9H#UE zOH&F!UVm%!cWrC|H#nF+CE(C0;l@E^#bWaI)e8bME3d#!qpnQ&UsgG*mGjCW4+Ygv zOzB%chyLj8swn?N5kn8rrCBs1o5(fG=-vDDFqaV6Sz3j)YTB)9n0xbYQ$$3^C}@Z) z>c%^)`WTCMZ%J0-m0j2oDk+C{=-*|BFQZ*;>(zEO6fff@_f<+_zWm;PDZf`@Hg#~u zzt}l;bJnL0)hb8##Y=vSY~e*s8s6drV+EJYAfKhAh||07X!V0$d?I_;VpbG2VmEsl zBfG;$)X)Hq!|yR$j5z?SLfnfA>Pad4~*3K~69B#DY&$;F+? zp1x$39l56w3=int>)5J}e71e^g@r}ETZlB`ZKYz`v1f}!&O2jm6$>72o=Wc0F+5{^n_k*IH>t00&?ko3oI32O?q&+ng zDT(8lKWK-b?K9Eebm!;hZ4PKxdgssXj%t4d+CDOA=-XrCrPc9jgo3;!&2Y)9$x^S5 zdIJ4-r*%*7tCK(IbrEb)41njJ(SBD8SBzEmq`$%Uyz(6JP&X_0$ydoL*+7V}t?QWx zI$aU_qthc4Ru=TqHzqvV@>WlmIZX_OD1^8&+9vI~C`M+nDb_(%>)pUK7P|M!54+Td zv;Bsl0WKx6^()8Y@;7?|%6~zh<4te@*%6l(1g#OV~0fzOabZxL!Qp&D~&s@sMz2;gGMab=~wZu0lgT`LubC?Qb*I@w-7~&uwE0>#j8(zIXQQH!Ij^=6Fu``10KNit=L}> z)tXJ_%M2wP7Xzh)oO@U4j4q>`UZA!o%>2kZoTJya{~7l{l}mqDa6{{>eZ*g$qLSMW zQCCwBj%8Q-XePuOM#twFb{oN`fyiu$tjDbj;;<1gd0pR$K1JK;l8x)AxUP^LGmErB zcAIY3Dk(wnbUAYO5~e8GeMwG(Zw5h)Pwis1%Ng>+zu;~5|Vb~1TR3}m9WcX z9{e?xlNb{NSo`@BnZraAXp!y63{-yy`kMuKK~Y*0lAp^dn*eSwUA_rT3rmmDm_0t79HDIQ-^_F3g_Rb z)3KGSN@El_*rPVQ(@pSvU6`4fdhk5kLdrvn~R z6CBAvX%3GqlP{S)G=J7M1RrFuH5R>jw?64aXz@gnC^g=}uj9QQet+mF1bb6TVL06Z#V`m)0p=bL&ooHu{e8*k_)ZEwmW za73m(Vr^0~fT4AK=^y6kg`3`=r|8?6$M-jb=jmh0R#WKp>{Hh6I(9pt_sn?vI^HsV z-03FM!*;c$N&A6bBF|!j z)3jZ<@$PNk|3W7M@!jxTGH7+~T`doCi%j9i1mKd_zuH=K0^FG$VJzn*D^uh>Si9f) zGyQQhBYRgE?f*HS{cNJQ;krKV7##pC-#|*gbUa{Dv`5;#pIF`Lg*1(r4fv`ejjwJ5 z@3sv$mv3=3(p?3>5VXMF{ec?+^4{O4JQU>7^p;zKx&L^EJT_m$1>Ij>iTJ%dJ2a5) z)*1N8;KPexDo1#h}lyg(oySnn$I_?LU6)iKjg2=ZDL33eCKMV(o}La z?Eu=5*1EP6^nJ)FDX{D~zcRn>hVfnb|KhVV-=mkS^BMJTI#^hFNYfpkGq-#f%Nc;g zYnHKJxmfU-3BGsv{z}D&M4axhb0`LSv-3tIR9?}1oTa$oKJw;%vwSbC|2ZKZ@lZDg zQwc^FDnkDL?(>nQU}yerWB&+Qo$PePJ!#-VkwoT30S-wDh(-II*8CA;|3Rnh>bAYm z$zyvO@@RiYmUgCs(OCZz)i=mz29Rc+tgWEYbjd)vkQPUeR_yC(+aqlc0Ei?v627~<7B^xT?^F_ zyMC$p03h1BXWyFeeV|Di>IQ*r8y)mNZ(Z!lr~qRiHEe4Ng<2YNK;u`#_Ctx#k!#Bh z2uJC#cHML9c7?2IYtcq=aaSqfO;X(jk6*7Z+|+kVMVZ)~@c9eHe>0!0#dz z#DG++)iNLkA3+U0W7XJL+0h!68U+nPnm7wCF)q^D`uz1XocBa4S2tf{=~-Eg+JE?S zadVF^EW}#SeU7yYXbq8Zv%Nmg{W(+@b!cr?DWJW?qvxr8*0<0)pHgqQCPBJX-Z?q+ zMc(%@YdA*a>D~zrf(DfBeUD_4$)?9SYCd}tO6IC*$0m1Vct4!#K8MR(sllOJ=jy{8)0yH>#bZf-MsACV1DIB16A40y(|Q8 z=%lzIigNnoZ5#}BEUx4Mna3~_Xd3g_&jAG8uZ9xi5(<{3Wxf(Pkdhsd%W&lD&M zLR?Z~VFs{L(u6Ws?wrpINoiXFuT7%=MHMod*y!nnk1=<~LSb1r@|->f+H?JUE-X|C zn#r~{0QulRiTqa$mrjh%#Qk&6tO-&BOBF0{I}J_CsP;3@QW3RU8%=2i_i?U*@=@aE zje!K421me2BmX&^7KD)Cz}3m&GGV0ca;EP)Jt^a|D7ZYc{{^a`fuC6z{S)b+9M)8` z=H?8BixAU{bvQ4Hy12mPEP2q159EIo8AcbWCrX~_MCD0w&KTwTzNU-P>RKT%w5Uv& z2wK01i6jQmQ0XNh2b%Z`<+gkgpn<+_bwjvt$X(-SfD4d;J;wpda*Q~*k$tkRz37ES zq*m05j3Qk};m&kMk!C-r-1vdWlf|Kl>&xf8y-8J6&+w!XM{kCKkriH%`AqFXSw$rP zZb_17ijW_J8`yonN4Sh$G_tgCOJcacWeOJ$*Xdq)8*NcwF5U7% zA+N&^-3qE$Rzb-|pr0qtJLK&{0H{iK)TS%weBZ12;c{M2SW($rbnIc<`sQ}(<)1CY zMu|Q}2f`0nH@9b;-uBq}rXS>=`FVS4`K;%3qxoXnt6K{8SG`3mL<=>I!bC?knCN78 z%C(ZjZQyx4Q*$Ue&fFsGETkHzlMCI}xE>||n}tw}i6wY$j<6706WzfcO@ZoGq91vA zDHGXK(w6wX_zCV50BR&0rj>;~Dx@Sb3+y%O6Gd=R6IQ`p{(i5sdPCwG)jP%iTP~v< zHUj#9!Z@5Tl>?)H^FII%YnK)0!YQyA=vZZJwE`(nLdBH7h89TG)aOjBf(sBnW{CdI zLe~=yZj}Y!`O*~@_9dn{kJ4f!Z9A_Q*lNaBNmd7$QNu{;HyF!KYsz+TniGfO0mmLC zd%}3}FGXSO3`c>J4Y5%^ELsTCaSK9jd{I>UhY&AIsW0{?Tupo0$1<1+X7mg)eFADy zEFfF#CPjec?a);m3xr~YmcLEBgu-RT#f4VvAq3CsuO0H2g0=~5WNOq#Tunf3;S%d` zQ4KmF#8z#V%@O)5DlV^RaGhz2xizc%iHL6WMj7~$``tPFi>Y#hVTU9i3B&y*U57m= zh|o*K|10J8+wAp{uavWdSfq(V<5t`?{48$9)#Q_n3&!~Y66}?#UpTA#Sk2}qrEe|x zF-Pmx>=eklp%ytTA&7Y0Q>;**wJS zp&fxF)vl!%pV}L3EIYW?x@*6@-uCvdRbblR(RPNt2ULG5(}D0X;bhEn0w)GkcV(f_ z^uI%Y+T!LlEZgxT6`nw$=dAA;jH_WP2&kg}jDqzhV^uaj>z*H{V2eg6Hy)MH9p`~??s5!4!ml;KoGF} zvbiM;72!mT$?SA(8241M!2Vw=3g2=C7|xnm?1?E!hs@$mn%}q$3;wk>^JY;|0E~?mFG*M?3jrHcVJd0nt?ZS%`BHeC$LcX4!72G~`Jh+^7INu4VzH z5CyXr=2;+;bpOhcxzfV0OX;|r!a!SJo%w|`qBN1uOf@n(Y-(KX8%tangaJVsb@Q-T z*V%r{U&+JLd{q;m#5)(0XhJB13Iao4!w$>~7lPR)rAF+kKMmP`>g2Eprx_gNUn7MB znA89a5<-quesF3OLm*-L!CgoOrW5>;hCC`%hDMBC^Cx)}fd(O=o~Syi49CYta1me+ z|E_`47#UN`+rCSw>-i%IFUBgp!!=WtOBYN6IW(FZ;UkRoE*FAP(Zs@8je26vS5-21 zbZa!(_Fubb*xtAWm!v}Y)sM__gnBj!XC9p?+s6J-d96{vr4j1RbJ}&;9{-7Z^TE64 z0k%>$zuvF9%wE6Sb6=A#S&R4;;o# zw#&yR75%0v8OJ5&LKVO*`t5U5e6fmJ(YIQ0pVtIulwQk2*$P9|O|lF^A8|xw{L!8l zey*+gDlT+Qb8JqIc>q?C8%6J4>sDiOfhtj*e37807Y>YOr1Ny0ZtAU+Z9_@(hzjov z)#a}KNqm2NNANNM0f2;!O5f61a*+=6kD>rmDU+Tx+2mkic0VkZWU3se?L+)fCiy?q z%2O`KB0&?n$3aX=a;Sl#b8KP=ivST4KMsyPoN_Uf3otiKnZCzq?w_cKV^^V* z_s2rX`Jj_rAdBB({j~I49r)8=FjX`SVUM_OSk8czu@G%8Bryo?q?Z`GQHVgK+QvYa z#DQjf_$S&WL`m-##$1Fxb4-(AjWcXpHL6I!<62u>q^UVuy3IW&Sagm?Yz9#SKJL?m z^l7ub2W!|(Ee|NVJB}Wl@p5mbd@CnPcz62woMwywfEiDo?HMMWwzRf;F%>8A@RWH|%ppj+PstR&g`&(;02ar#E`zCar{6C4= z^V!rlWia=+Np({n07!OgOaYc*5-KhlBSas@IOKv5I#)k8D}6D7&Gc})@kB|+!nel? zn8#7rL9T|p==@r}%5R7tMR)7KXht4i05D$QM92zx^a4{Iy+f2?Mm{zP6IeniQDSJR}uY1Q9A&B=;sqZDUM~vT#<*foHzg5823D@gqog z*5;$iQ`GT+wzB}!*Gw>yNG)_#M5$wS#JDNHYGR3{83ThTct5%fHsTyqFly4Ts)gQD zA2tUn!7m%Pz~s)(_EaO3>`=mG%7fAqq~>JM;#aM2Z=0e}I#_Fg8ko0WGv^ONyek-4 zFATQh0LLZr;Lk0Fy@jQce{B4@&p5|;g48}zux%-Ybl7Whb zM$$}8US1w*SEtI;#X9ujKTcwI>cK8!mVPI|tI$}+eQSl(s*oEE+1<@-{v@e7R;xWW zfG>;U;R|uJW4ylfp9^^*Z@c-Kr>PwiAiCtMy5=1Ax45Z5oJw-J^TodeG!_U#IACP#1yLG-5in# zoxv?P0Pkz&O4|7rK_UhQ&czu(KcmUVebfGbugPR%e@b)~2u~g66=n`tRlS7F-AZ&a zaxmr5^2Qkh-Q*A;f&}a#kmzCScVgNY*PjpRVn0V(L*K>-$+xx$z~*;$6wJwywy!Wh zqQhd+4~lhkkG@fAxtaLQ!XP9m$`+)6JYs&H7!E($c>4rSGk zT*~|wTN0-+U^9-G#8ki^XYVU`iJdrUBWZK%Eo`lwG&VF{&tz`bXGm;ZC~2s})YMp1 z^4sD&iy2GAb{S!=3Vlwo1NHcq3l?ScTBsW4gqJzG#1DGPV#X!Iw@9uqmmV})@E#`= z3FsE1ubM)r)CFh6H4ctbar6jp#HJ+o<5xs8pqA?y<{yV-Z9TtN?p|qkbzVL+!rITh zI?Nk4P1wp+Yrn;+NWz^88Py%DYodo__Gv?kih@4B;{uDTsnL`?rc^QK$EUf_z)kZx zP?2&^!JOX|&ebs#J^x;1fLlH>c2)^jkB&oMfQM$+fbq3mafUo>S%H$I2}MN1T;injtp&B# znd|0Ge3*+|4{2R87YM7jl(QxT*a{T6;+#8+-`^qR^C;|_K8wjSMX$9YNP@lzXGTmc z{GQp96r(1O$|)*RGgePzj)9^Mm5j@4bRfS~?))}RDh0>K3~O7CaF4}6M}**EWqUO> zHAyXQMSZVfFpg!^qzD^hL?#rgrS7Zm@@d~0y3qnVGkq8-c8N1b~R}-rD{w>wv^A9Qe z{jae)a^^BNamXgYo>*Jkum^`d#pqcH{pIRGIXh=I0=MSJ)cbGGtq`=MobuvoC(i@> ze3ZU6?cj*&lm0Nyfc>Tc%I?@kC9mX7&_-*=q~NI$u}DhvA|jimt)^(?&{*1`I@BWYX*n;3yS2##W8||MUCZf^=3H01d__J zoyn%!z|t-=?LZJiL+Kyh_x&|94s$h@eRYD|U;+YyF7}^$4$b;@fB$@Z)k4J|{FCt9 zOrlYhvM*O4&%DfN(I3DykCaBSO#jClQG4Kf>pHG=;dOTqc#;Yk$*+^MA85EJg5l@9 z*>^X|{LE*D5_K2@6;;C+A5O6M#&_kJ-;q4G@(Z3&AqA624uZZwdKjel9EhMem?~^K zpEbhAW0F?vPkA%}j-TJjYohxPx0Eo%YhlEfO=sewZ)5r?q{4hGXZQ5#5s~mBZOcwp zIOnME^vwj37|<|eo1$Vp!}e;_**-Sjwh;O&sB4IdJBE2YnUx1zvXms#kAhBGEx=Th z=|7E&iU?S{bLdOhd{{hO529X&fQI>|aNf^hnc|V!Ibx!trKFIaxXW>Hg!Qh`hsU<; znRTktZa{WvV^GkpEH0i&3u4_Eo9Sw#Bal^ic|w5G7-*A?c*i%g6$Ee$*IVON($z(E zZveZy4@?rdh9=py&U7{ZDY#WDIO59Bo&6%8p)7?};amQNNVDq6bz6{J=3$lg|1qwA z=Gq^>e=|J1bOR%BdD)CnMWVMaU%+uk_c5TXEWPLez(c{s4KcTs^J;rnks6Z#Ger|d zEp@oIazq`^kGi46C>NIsR@@GM6mR_dy9hDs!3?nd3-tc^$dZy$a0~3fAuS!6N87=o zIwQGQ#z?3&6Szd#@t3j)o@GTkT*uDg^)=18c4(im8RuH?pK`1id+j}4zi-NS8E|Nc zO5CJ}fHA`UVm$u)PT@y&Y9L(v7bIfscappX|KN7~u!||&W)M`)@`YQHhKuZ*8Y&3} zy9`v^vr=03nUt4U`?n?`f9IUoMg@53?B7~P|**FO*C zBzSmuXoP_!7~{fLRz)ZplI*9tocMj`Sev-boG#cZfTAp*>f--x+Y04aJLJ@5yt&I$pMlOql!M3}iS*J87)6<7t7R6tc)Wwjb|1wWV2v3;8- z(2ou)EVW9Nh>;UGPabXAJA^H5kXgzQh)pzA#++Y>2R2c2nPH@vXAToi4NqhKwfB3? z7i9j289CYm;1;SII%gasM)O!JHJ!7??V^1DE34OqP_nWcZbUig7&0=nQB6h$nY}Jz!^3(f=ltD&l~P>9562naI&){Im1eSyC#X!ckiq@Z zo%gGAz3Qy-@ROzHKmWgm8%uSr_Xd%rFMWV>^rCG{`~B0fA|-G$F# zK@_M=b^C{^6|2+AVJT#^nhYE-NgaHUdL25hjlQj{lRh=2<(HS&fJpJcT1u;tHs#_L zRVB#l=_$~+Q-P>IOCU1{DuXkb!Pm^27}a)FOGNlm)12vcbX9#jdC#yI+|XSR zq-H$bjc*+JgX=rAa2|bw`b z|7wUV>laZsO>+l_u?c7aAy?i06@)umoPP$6-*!}l*YSLnQI2)X{WY8(4UNGPjS4lK zyDW_uzx~EZP~r>`7OUp=fJTfqWiBvlW3}}vSPa~NV`a1^AYXz0h_#6rqY(}kYyqUX zb`)Ex>k~*C61$ID>5g<6l)t)%_1e<`RTD|9g%c7IzNx=?_cFm>h9fH#ju+YjEo<&*qD-pW`PM_`9ecj)=(>N(oc9 z&k`v83l9F&L+8zygSyP`LGcUh$H|?L|NAUDUIa!*eI(C#8Tf6^%v^v8SbbA#gEByg zc5Uo`{Nv4!h7Cx4QZ6_uXYP@?V6PxV6lDQRa78YJ`9;=S!uQ&K2Y{rpnnA^)74{Cp zw`#FpXMj?Ozpl*KNW_;aP74D&d42eqmPQ}QP$q23Zept+WM1dG-Os{(29UIBw3Q!e z{=IJBrC<@@l3^?l-m25Fpq)suWEeg-yI>s4j-fJN;DocqdmLh?jglg`P#{4|T}h2X zQd08T+ZQJ=n7%5YcTKczc}=*DO-n+(3THs7wt#y`ABtb-6PgnvGyT{(!cIVsg{kBt z%nq(emID16ADjK^axrJ?{T%a#`ac$OkZ1&oplow}l6(ID0}e@J!e*;Zzij_rS9$Lc?Ijd0@H0ev|0g-b2Y>smQUZm`^%gIKs&AA4qA zPb_qATI=)lBn=%Sb5KCQ`h{*bf(7{b-8Z%1kQV{uQMI~7R>QsQ{7(y>(Uq=GLy-f6 zHqNsA1oW>ov&WdWG=$&BhUR=yo}G3+?9yO(;6=oy#Oe)Cw&SNL?5cF4I$wNz>35tQ zb{bGEjd=y!5=)_~gj?NOD}dIX4!6#Yz=smA1caE6NVW5 zJ8_Rri?4^hpIYMwXGOX)y+7wc^%gPp#O-ngg5wFDKTu~OOS#Ub2wvS-pHpsTa1trn zu|@u$EdMV0z_4_5tKQSHmLr{8w@+Gh^h|-hPqS&?M%tn{!Nf~71!-S*;M}l7zGNh3 zS^w)R(*VnBwWya`Fr{(&NW_7jujqtf_EIZm_#pSCI%YPT1O}myjw^;wRk-YU0 z!(J85#>2*0#E6U=e_K}?=Ra=lDUzra%G}&cpnK5ZzSM6|VNV;htJf(*++r3um;tsu zxkkvB&Gp95nhqYH_F-<`gf5Sv8Bk;*4$WIi%t95hgzTR`f3z6Ri0`-jN_$XxD?qP~ zI>T|{cIjGo)*hh8oS#idgoh;*ZK~>84LrL;-TpOqGnbEDoIF~46$h)UMj|YJTOz4Y$|wp#1snWV$rsQ;5cxY!Eidf=B{%nUnZakSMGlSBZG=Pcq0gOaG9 z{%4hx{s|~;1WFJ!#{3&Ul(CNmaR=J>J6u05!%9G_-@`cweGeApTOzli$ku&BCdZzM!oO1ev=q`SKt zq;u$QNonbh8M+4P?iT3=>6C7y8@}Uv?|r`K;csSUpMBQe>sJfD)ebdGvY1u;geZ8K zt%>E0UTL0XHmDVy-&WCjx>>>;1VR9yxZV?wEswU6^7350CQ5R$oC!P}7ToeLQxC#* zhet;V)C7|%I0!IfQ0h#m?cgBmOyI`lkML}G`iQAyTqMr-3J&tQU%cIHM2AXj`F56@ zxS!r2Em8k_R5cvb^aO&!53URs0wXx!?8!!7AtQya%dyjK>XW`a<#-aHkk*Fw*KYfj z%i~Ai1{#F$LOrC;9~Xf%o)=s#RuJ#mP(&Lz9y! z*jPKgT zupM8=vQugMn5cd!-06F5;vnn*>uC7Q=Qyz}Kj}$;1oR>~^bz?E-o|g{j2YwE$RAK5GRz_E|URka1O7iuDIg zt^6G*y*}3P7(CN)yIBWLauJ;-L`hd3W6ZRxOR!@> z2xk^SorBY}3WfqbBfC3Nrd0Z84JeoQBXmXMdm=a$bO>ARZ)+iBnQtUL_vX*^^dAwg z$tF(Jm$yMhIW)3jsUT`34n;I57g7-mO`+4`VmjTpuEiWBcR`=!6*j_PqMgj_#*O>x zei?xZT;8uABJY9wV$g1(yZq27=9&B3UxmCe^M;)#w&SObxn`S#`f!TE>#NROmz^Is z+vn3<<~u|GoYhI^U>Pc`kEdabnT&T-ozJ%N@nJxQN6Q?|PS1m5e(RqUGxGfT^m5*7 zGiP7&>+D$WKy=S#ZJVV1^6JR9)|Bc+Nu0-Nu0z0S(GW*2M!)#Q)q!urjr7T%WXB&v zFzjT4w`sLkbEcIFE|83D_e!30^D-r%)>@iL%@Cw=0#qGN*5)SlCpP*X@uqTKz-UND$nwq7rcL#6_qj`)e7N zMb{&l!(y5GK99gfND{B<=?ii5hMfbSF>jOdRrS_E!Mk;9y~_lsiLHP+Thj;0_N5$L!mnpG!Xyc(4jkH z)N2b7hGMq-3tZXP=iiiOWU%WltLYtI9hUf?TyD()1xP~f&iVORP+jw%4B%Utx!R4_ zk!I-h+~y+QAE!0gmHrseS;#>OH|oCT&KwD;{KEr%OE}S7W zP`(SvNp+GnB6wH*M>wk4Vy&S4vX{0#ACuT?N#CL4(DPj9Fy)i2{YCBQkvbHDc_#<%-xBzN!7@eZT>3v2Q$3{oS0@n55?u zw#}ely8KB}QgD-#ub_p6P4SQ*EopHzh@h-smUZIZbs`>4F}VNXAu=^%PLxhtbD4ix zmX;%)7)m-jAw*KTh~NK_GGF!_+R&)f>zY z$!gojljiCHSOE*uiEA8Z*zb@O4GyyLV%wV zJtOhvpy!oWS>T#IJ2+>f)jHI+&By=lwwZ8tCrpG_bScG|Y)=bj~w#Da!B|hS7QDy1%){Q`41HI$Kq@;h5&y zKAl5G#+xosdVkz{@U$ZuL$xa~IJVWMa=vA|;u>|2IJfxpWT#}L-yi*GxK)$TqVej8 zc1W&z%u#SByj2qLBzBXh?7Z_!DPbTlc_L(_YV)tO<;4m%nRD+iIQ4~GrM^T49`k8&szGO>;VYNK z?1+3Nx|eS-R(%N&{o1l<$P7pD3(N(_YqZZF)|GRM;awb9T4i#jRvT_-YG}MWdt| z18uHKoBe0SHT(B{3j2q}$Mc^a@Q}a&^v=gzRMachg>t`ep`oP}CO4UyijGZVg-{U* zy_!~#()qi$ap9u%0f!^%H%CNjhDGFPa>kD4&fR5D+@g!3qphr1qg20l2LS_>9h6aWb<;%=^)%pLe70&Pu}VW)#1~MgqUcL&k=8_1%3m)VUMs*g``|pGompS5Y?(< ztflb^L#r(QT7FfWA+=POP2RK{XKM%)>88Yq=}NzE)6xA9tG1O*?J}PMKoOM`}j>@e4>3g6v{tlzp(nD-hg<@ z=-o_c=ZUtKL&Bo_ii2CMr*-zN-)ZZmt+!!wh-D@C8=~2@Vqk2MPEPS=3JgcS^q z_i3ZB`t3LzZG6WQ0=k!{z(AG<^%hBYHrB|?nN_M^JE40H=EoQvX=aOwg4lPb3#8E} zS5z!Y-VqbpZ_|a=OcYYvSY4LhD4a0#CJ*6M+;h*o+;gqpVOZF#wynG_5WXFIiKeR} zj=?t0F|i9cvaWn9Qeb*%aajAncK&TB`h=o)-}&IR7lm@Nslt5c16XuxAkop2csIu6 z>3yiS*7Gp!i3lIv>V}kI`xbp8*~7b$T{7iB_-Hb*o#JQS*U;NV|7mMC*!VW~n~h-b z^t?b4iqpPHaLd7;fOFaFDdI%9HI}sVU^7^{vlQ<%3DHN<^~OleW%IXjL}meYyQlrl zdB`kP`a`rQ#@=rRC8L*K$!~z)^81EEJ zXOgW*e{4R3w^~D&kGQD2C#?0l5l;3+AfQanxnD-hc4#F=RhV4{n40U*#DBoiv~1Qk zTDoaUivwhT$a9FG+~6%s1kB;qM3_iQ4e8LY;j3UzURPVyXCNks>y=t4Xg4?oC`1^G zVND+-dO5(ofq}Jwan!WH8ix(UrQpjTqo6^cTwQc_mM}_4WgfjiR||5i z4P5X1vmD8M%xRCBSGA}{@p#Nnct5t|+of(<&+g3>y1CXTv&>beh@#|zGgZfM%(x!$ z_LtUxa%ZgmIf?Rk#z61cOE=lFjkBkVT8V|K9^P5imT~vS%ixaQB2;YS_Vq$t@r}=^ zrranq;;qR>^ho;s6*!aK0%MKmo^hOSPvV9sKBG~MG3d+amP)E!&4lfGl$OB?A$-&_;e(Ls&g#ZMn%`IkLLdz>;(4oe@m?D4rl3NzFIqAx>2jHy}p{d z+PgQ={416Dk`selT|!HVn36K3I4d@D3=f^ys{?;fDqKWc0(e-h;sf2%Xr3aYB?Pap&l`Uf93giodk4+4kcIZLNxOI!IR??G z5x5Aj53CKJurTJ?tdlnk; zFStoUl^9J)MCVz8L9~B!*0#Iow}Od)sDx+*L6*Te^&uMZ2M_5f3;)`DR5D#T!EW9; z;Z7I)bo0sSKX_M{P&G(vl2A5z`gk$@WKn_P##{WNzK9hu^%Dcb&;;xFsGOYl7LYl5 z$^QA{6i(@POz@OgP1*01r^K){i_qlvZNAXO?Cu;=*q}=ou^? z_%16C6Q%2bmEhFAxN8EZx!|3&2qW_jknZr;70afw3+wi@1&^ay`$iCZlN2LIJG&rZI2yG+_+kQjN8TN09lO|$%qJo zh*_^RS*;vt&k)ry`>qF1YwY%5mrysRCGq(ER_t!)K-YG@&rL*<8s0yCICls0-^mgo zK|cmfhQwGnedS%h+E3_b;zGC0uc@uYa*4?S+WL6J0@^dFzBB&7tCXK1{ny|1mtC2% zE?XrOqCP~@&~&ID2Cq!MSrF(V8y>lzvM|i7whA89Fl}lZi4uQPOspg>m6=-DvYJpC zu4kKWrb{s;vwgDP7DF#egbpXQCVl0t_Qmv2Bn4y8;la#W_V`yMZ+u2hPL5inx+W)% zypl84Q6Q6K$L--pbSMKD_r zJ^NSHUd})Q_Jcb4e6{tgwy!RKOsF>5FC!L`-sBABoqA?&=NrTu-fOmCKi(hgB8R`$tDqkp8P@kZO5r zE z(9(+EmJGlT|0Ds!e!YjFN8@nDKu5}JZ*nG0M70GJ*S z>eqa0LV&swpL)5Jr+^|L%74kKOv=4P5}t!f1bOT)Wr9X1oQw5T#!_rX>ZtSTSPaGb* zwk_mv9^XpxOOVWO^wg7adn(Owvz@<5m0@SP;8li}L>QrJ*K`}V>cV1fc2vjrjkNw* z!OS<=r=7Di*WS^!u?0QBoCB0>(Au%B*A*aCIGc6e!&T$3@_g$idtKI?p{m{4^B8)W z?z!%()Z&Ta9_#CUMKJKPIk1oLpm)A?jny5x7={&XFS zkHTsUDe^aoG`O?fj@o896T(|#t6{T3XI#kjIt-YmZ)1G%g=mb!7G9`^Z4;|oq; z60pE62_d~B#%}u~joF9w;f@KWjUXRx^}S=cu2m@O(R~2h=H}@xlDr~DHC*cl_o709mC+6WtnS7yZNPOHso~)l_!G2Z8>vLD*?T2-2{& zQJZdu89cd}tkA;*U>~<5EeU^re;GSSPB8#CQ&UHZ#J>TE#7h~!R-7ZDifp5~O1aj* zh-ALqz)bK()Ha&;=Ezx06Ql!wUA3K$g$*sI5}pTbb_IVt&4Z%_gu4yag@;rTC?{xT z&|ZBkXZF%(^)F|zm=72fngO@Om-d?_+vBEnf2Iy1Bru=5jr>_qZo-!&2xSGm71G7i zOZ?!W>{iI`Cp8A?*4kWh2jQ{)WQztpT2;#0Va>k&PS9PLUWf^9_3+1or7fbz>E?dp zI93?*P9^qh@9Pgf=O28qnUnNV22 z(%=ZKBwD@%;xS&|VmX<=PzKH^07C(;udmH5Et8Uxf~kkxPFDzuXJidKr?mRcs){?; z$Ub4%M#{@3GDax;6q{jN&vZD+q#siIW#61P;o^Cj0FdJ3M*2CW+&2zI;~9X8tV{f_ zAWx^$kBRyR6qrMoPx>*M3!swBTW@mJ4M~X}ZxE){A+uj-uO$kFCzL^=G+*H2g~Xbk zzxpp<6B~vuEy0g7t!f$@R`^Dlh9?`uNH@r)Sk{PJY#L33 zG?--+C8Cfl;5OFb7HjVZJ3d>=O=Y=HqUP;s#gvx*GbdYqmyk^?sl3@d_xfx8)rrr* z?JzTLAS-nJtd~q+oPmKM2FN1%Yn4EmOfzT1sVJU6RaZT?XQYZltrKe%RQgt9uJW0k z9Yc5Wbf_@dZs?+dX%<+y_l02%ujM2W(^>uA_R3ydZxP?+(C-_6q#~yY`rK?swemqS%!_n8`V5h^Xiu*Rn7AwO zs0z(CXmRtv6-S>o0%Q|%2ucL1?qv^^;( z(W%_y#!j1xn0(hmK*Gv7G7jq=`1aV1AJDMji?OCdlR-$-NX0V|O{K`;GqUatct}W1 z12Ss)@>iee0f%?HrY3pdIv(RS@hl&^l*QYT8OSZ&kW`}jPlb6x<-}OhOQDbZU+T5# z!AJmY^KR+wkhdqHa0R{HBX#4~#j_6?zgPOQcrS(TphVhq7xbN^5rK5=zXBzLkMaVmzMM7lMwpG1ZhKA!AOyryfnyO;#hoW~;&&|Vxjmo>Zw)HCL-6+Z;~%RNtmdP+F6fNVF0)+c3N6+($b3CeZ@m941b@b#zSOB#$&J5 zR8pId?I_D#>Iw_FVX$cx088)g8^OSsb39pTzB*oWS`KAC?A)>$`A(utmnZq%0`+7x zlJ9bjkwrVERyt)9wer^eM>*D6qy`RiPYrtL@elG1t5}KPMB;X)fcS`UK3!|jm)Pu5+#>`D=p10{j}QZPDMv2 zO&lI%LHB?k9nQlABv43G&)&)e=5iV%CjeK+e0Y1ReHltNgX9){-=vI@W*m?Xn){v#_4p@ZdlTFM%1KI0n2sP=WrQZApG6vGM%l z$XHP+I9t8Rw2<9E4q1>RL!|z9a<#Jcq3VU|EG5X|V^PTfvn{hR_FvTJ8Mk*NDpixDLqexu($-$1Y~JR#c>Rf+6rVd*4EWJwFx$_ zlD4`t{5&}af=(mW&x#ux!ww$~)2CNp;eZ~CDe#~CY?)TI6+01S!Yyeku`Yw#kycnZ z=lIxe>yT0PR5aei%|Lo4+7L;v(Js%Z%YTyxhl9amz*yi{%|CRW5bbLm&O2Qc=pZ8# z$()JYBr@h=E#t&k(ZZRsu6JX3;BrF}n097lvKGBb%)cSR?UWfCQTo|QaSTy(@(@a+~wEMQZdRF&NkR3o>tuMfqlAEGW; zecA_!d>@?a&Oks=U}14o5gs$Ap3#F^*`k+#+XT`sHt(+ap8Nd2WVbvvC1rHnu7#qF zUyJ-cH?+Igd0Ux+_Ihe~e9Jy}r1;>wn?O3Xn8{XG*^iRdeezii5|L>UU5Kh=^}Rhh zZTdG;*t9O27AH-iq+EY8gr@U%^cu7>7}`TNOVSnqo$8%V{3#)w8dEABxhay0Yp#Pd zq_Fk+XB$-BO)X|8_pQ|WW37T;E@|-$K{hmf$DSsh`ss!5@X+S`(eHSrC3p#<3m{Pm z)DcflzO)MI??Q}jD?~%y(bK>GB@NUNk@)Y?$cmDx5}PuUlro7ppZ?GT3nTx(3)6{d zs_}nXfd9WpG3KT4_GS`=1_U|gl+*a+wR%#hUX4lOYi@mWzw#=~Y{b z4H<);gjEedZOfaNt6#CR8d@~%$~i#WkFt^el#h+N*uuu$q!S9&Ka)%#A0SY$8cqds zrPi_zjd&G-P9}pu=A2Q3^>QQohLF}i2Pt{Q)mDFv0LAcYXAhCl#LBr#9a-2ox)C-5 zIyaF8_|MoWSuwQU1hT2|xbi7f2GSV1wu&+dh*&=j7SsFXKv zv|5E$#%{yJnt_xrX$c;gEoZ~d<(YV|qobrkt%o;Du6CX}2nVp|IE&vTH9%ErT(^<%v_jPI{UAXr^VT&@ziI2TK zlUg=6xU?ki8TrQ>pq>tnkE5@~XojT!oP1Aq4-m-xCm!lLIskiwr)wx98<=ZP{!f=! zGuS){7>E8>6&^Gv(TbHIqGIC^gAdP%NvSrep>|RXMvm~dl2SjuRs;U#My-VzUQC4n z9wZn$A*P3)S&NUbN`0%rW_XMuD?C5NiXQD7w9VY_wx9yzn_t(WI%AolnYGAC|d~$g#<|d+`Sz!%L8h-xpY{96Zkr8=WIY`co zLlYfGN4*=qP!5aRlo|*`!=UwV>(|r#R~_hL`3ZggT;nTPE@d6u2-Wh#T6QMGlz=F^yzwGy@G!1-4Hu z@NjeL9-MTIU-i{>W9Ab#ga2#A&@&rRFHj_tTi9~U8+s|kvxzd1WzhB+z_EsqIeV+r z7+JN#ZyN4IH4!v^*7tboE?jcvrI7QexydX?ONqkI3GpB>a8lLQ)j78qe2f&+s?MSP zw!DQjqDcSovtk~7;QC(N`%Iw#l3gzl2qgJe8_CFk#Ly8JaQ&4ZGZqj7*~FKRlkOxr z6XB($){l&_Yq6SM3hAF&O$jLY;DZ|fQ)j!B@12~F?`>&v$>IH%1n|G6lJ69_Lkp>r z=B+w4o0b4judd6-n7(Ag|Hy3h47}rDPGaInE_p&h*vpS2TJ3mFnx! zzzKl(VXgnc@7Ls5Z0mvks!oh^cL;MefFwwFDqPun;kkjxaOb(I+@(_N{ zBL0@_UE=d(sj4X(Kqt8k>DV6VxEg*%<6-AFoTsGzG-Wfs-2RoFIV~eIQ@;SdoG~dY zC5CQ9TubW%x@6CRTc}(Om1xQiyJ{kJ>5pGz3alBXzjdKnvu?z-#I5#EpZya+yc&&K ziPQi*iy>6f3RTbUi9*izabAi*PuK*S20TDxx5R!fbi0K#?Dbqwk>d$IC1MJJ{@>kk zGS*57(^^<%-Wjy^aD5b{=IZsah5mgZN?p!wzXdbMh6V`bU9PQDHD@b2U_wXUjQNI# zj}x%C;%G|dn7yuCGcP|5Rs8+th;_|~8}^>2<3491O||7-(SdnH@5ew%1f{467Z2g2 zI6Favgk`NEuZ0YSs+#ZV9@FYol6Ss%_AR)tVwJ*F4t7hod(Z+wZKxJhb0fkZD~PM_lBrp8^n3N za4Mo)ci5pHON7(&jg|+uoUaD`)kiw%boO%20v{U93m-SyXFCR#H@7 z7Bovie&{nASz&oe3C+KRbizdHf}{A9lu#Y-IaB5nkVR0=uA>$O;nXL2} z3u7>k1XOohTVU)VnzzCMwXVyToiLqz-9jFOpqnT_cH@>9nOm>q7!_2TSMMd}3Yc;M zKXkSIa%1Wmt|(Ei?R=%%!E`8vcv$^a<^Pt05E-uKj}qns;>o$V=8-8eKoHsoh_TEV zy&(%juv!JH%b4cw&y+p6rGe?3y#f9O`O{%kly-PEcF(U4VhPsLY%L|)T3m-M)@W?l z-9_IRMPp}VY`FOH2%>fRsoCwJ(i7~C;U6MB$2v*rS;wqX7iHzgAxGAP8V}^$vJ!Qi z@Qm7&^()bQ>j53G7LoO%ifTzDLENjt0^*h)E#-gAkx!bX4AddR>}M_Y)lW+7W|g z-vVk$#!y@8d5I^Zfjl%EmUQo(! zIDez$c5o|6s9>{i!uEUB8X!=2`4Cn9I|3kNFf{McAg%Nsz`KgD!))D@xb5w_Xe1x`Y6&avL%djEaEpjI~zI{^g)?g z@PjbBQFdlV=z-H>NNt9HfoQFgM6_hFO$9VD>tE}yv5+nGm6(b9*#TpgCTkE0-Kj4r zqe9jK7AS@cVUwd*MHfSAkXCmn={Xf;0zuYvw30c5`*<1*eP4ls50_-GBggdtxa3pI zQq+7LVFTXyTIxSeJgAEV$`*-Ya_o0)ltYbdGx`du(a?(4$^yh31x3x0iZaDzGkYbh zA|AyAis9eDmO!E4(~EU`CwCc?{MU`uFrHag!^&Z~k?E>WoZRZ&ST^-^)8Cg0?Q^xB zA_1qhAE{NdjKF@d&IKZ(>D8h2R}SxVDbSw%xhzKUh3>bLP)}k+CWZb9M8M>@C5u=O zBcHe^oOyM%Q&6-uFKX*(*J=!rhY8n}>wLXiOiZ@kjjd^Z@^QGDSuP7g8HB?__6uY~ zjmS`nzJ=X-CKnbaYm#&T$bV!%A(&zPN-Y}pH<9{=IE(vv*0*DMq6Z_?Aj_l*OByjH z)51^g{9x<4RS6nPILNvBtg+=ztOl9Rf7>?{>a3R--3gBDH6cohQ8IuC7a994e$Pn- z<@#K_z8*-pt=WEDm;jTs8yHcS0u|L24t{^}uE61V`0ED_*J5VHJ3N0HneD-^)TP z!&w4G!U`p(s3krLZ)pMAYmLR^V1yS??Xwjf+iw)o9fx~R?G zEIhSij-8s>@Dvq}?l#fq3*n>Ao352Sj!$o`r^{v9`zwnKb0fJ> zrA6mRy{LrH-5!{lgiP57biG(p2@=Iwxe6Qx?}l$AqfivztEo?Z{%MpwBZ=l{^f^*J zdr`z)MB2k*ro<;eN>M0?{lyz;0DZ(UPpJ-_FXebR*T5&C%KRo9HbZ#peEDqiAi%Gm zh3L~euhwbLI|+25N8L){Je)sj|KbhIC^|Ec;KBdZ)gTEq_9PlhqMu)?xa(OoKh)@? zW<-^JQbJl3__;f2s@$>Fz7e{TQX;Q-_NA)EFn-M;Mey}%#-|Np%%tLr94YK8mMVSc z2muBjo1!Z?urL;O8U@G&Z{`=VgjJ(|_;6r&vAQu_KdC@dMDVLM-6#fSb0CouTz3q?>THAkbKsx9Iwu;~U)fY=!sDZTeuikQj$CkrUNA-Q&4^~ibjD)(DEyra+N)Z8mT z!lTOtC;Wj#fy=U%ulU5?Vq0ACm$E$H)!X*#ZjwwccX}cr-#B^1reFRo1g-}ozFyWA zXGQ*FNttdNeeDHjqA;#kPO-4FwMv6FuG>>! zFJjffpM)S`1J^e?*1?ZfZ0Cg*&`=A_$XgQw+KDr|2}f$0vFB${MdYE@VvB)^rO7sK zmW_%+$;;o9C1%4$^vlEsX1w_&;^U8Mhf@Ndbo@SjyqKp9c#{I)lqc(tDr{W<)DC%onvI>S$a=;Io6;N#E$PVy z?@}Y_Bu>6jL|F!L#@}s*#EQ5@&M1BPZ{5F*Mvi-}vxns36b665gUr2MBf(2S%jp=k z&DnJ+f)WW*!jHu!ujia&-{dx-eohI#JsXDh>|Or4Tz8$io}yzUJOU=U7gYIWaAtH& zmazB``*3=$c~cfGe(Q|!tmMziFns+dQtl;kQg1$5%jCFK+9jF7zg(u=sc$LU)D$X z`pkO?W@$ECXY}pr>ZzCq{+GPJNq8Mz8;J`u_lgT^QlxqJ0Kes}SwETU-N7?TOjku` zR&ng{M0w`G!re-Z`N6W6=PAL;-62M~VOJL6#XhI+C-$o5gtA!tZ-{>b+~Jbqe4|M( z)Bav8!VNkZH=IyK@Eoymnhtiineq=rGMUN`*<`;ZSq@Y+; z$+Rja_xI+If&SVZ4hl-YXy4r^0AGwxOaNJdP__T*;0a;uz9-i^@cWWY!9~>se{YyI zeE1MN09-j^EI+6p6o9S3lkL<*k=aGkuk{Mw6Ef&iGdbzXNI3EiZXbiEr|;JG&b?HK zk%9jCb7GP+i1u6!QZo`7&|ow1*H97LZWXwYK;6qW%?u?-YxV zZ@a&}fpczK1pvxV!Gc|vmzO|#{~Zu}15ZZa}VvLkKE^ygdcyu(OCSDWNw-V3gJBKAHLPG4M?ojv6K;&qbp)HjtwM#}f` z=F=WdV~OJ<9v+z&Y?ZrEpew;Z)%K16031`dhF5>=UWf5FrmS{IASGHR}xxp1o^$JUiTc*z6BljO%|;eL6#SL3fx{{(4WFsAI_#+dy{7e zi{a4=mx89nAOsYn%O)R}>ZjW5)--e)~kQ;tzC-s)dt&@rZpw zu0=A5LkbKQOEVc8tWsO(k>p4A0fs`$C@2T^m%O%gG))exV*dVgD-PTM&n+Gt3=q;d zH4d#wXt5hPobw2gcJ#c2mj{;8Rgc71{ylX*d(WrQve=Ks#87Ts{-5MSB2t~)Y639F z-!D7bip{J#U;tt%yVX_%029H~+SSZ6=dqeU2FlwgK1A`>kB>_6hj-dQ4Y@k!rt2 z0J>%k{qB0W+dQj1eCGCN=Sl%@^j^=L0X(9;qV|d#S;Uj;L!VIWwu$YXR-Q}2fxr5T zzTPK%-whsdHf;KZgC35kMtx|zkzU&)k(>pKBwp46wx9(@yY*k=Tu&d&6iVp-?NCu22WCxXJaxvi^@>3A%^A&|{8ZiU_#i{P?T!BUMCQMH z--xP978AoM6k?Li1)JTX3d_-6Dq#}1Y^XNIf<>3DakS_cS-h4NY+F}rv>$cSZts0D zFB|bbgoSX>)6x{sBi$@~-x05lNXEs!OR@l`@c;_sce24v{VY zaL1`VD#QkR-U>i(oyDpp_7okTMjeS7RY{!Gd7QB5#tnY0^!64>BcSzm9V3VB8kwxb z6K~%xXUveb1QQkSNab9@UYGlvp;zkNQRY-I76b{@YLY!H=suk*Rr8^l8)GuA)}oCy z>O}>=34WkBTNrM{sk)0^v7qaG-KD$wg(ave?(3v8pow zm&#tdaw~jCxBw$#VmtruTYd53YR`$#_Bvmhz%5UCd*S>9pX#%~g7`|qaSYAR?XDM^ zOTr}e^JT7V^PSg1dp0mB?cA2Mw>Efxeb@nE5C7N8eg@uqr~Q{}l4JYzX}QT^)~<{SAd{@)W@*F&oWh{3281AG z0xFq$pMTv8Ux`c07ef^ofav$nxHD-zEe;%^EB#BjT2+g>z_}KdUzJfBw7J2-{#CZV zupHK|vvt;-|IRS9M>E9cl7|>MbWLn!m5`R^O{AZ@6oIZ0iEY8|G80CRKbRl1(9-MS zU;2dxN>2mOFXBB@lR@k;6^ph)TtB0)y-t?RLRx#2;+`WvgBub|GAg{>+$ z6(b`uth8E2su4DH%ik@EiKLLJ8PnZ9YIdsPLAATo>yl{>fc5s$?m;G$yLyz!$0>e~ z1uJS%EiccdsKVt)Hmq>7kK<;it8e9@L9I;Mg8X}SZqeT;)#r5Rz?IBI%bBjQ!sy~J z=e+Y=d7$s>u=Q18q-KuaPl#dK`F@s9QnADC$Ux=Z9Q%0TvgNDATyEq0S8G2W&x|qQ zGyYgpPp+)CG!?aEqFaRg%FY~hrRLznEl0C+d+hz4%Iy=|*llRY?MC+j43nr4V&4H^ zQ@xo#nG`uDk}1IYF(X9SB7XRKixipv;p zB%^4q(2z%dLOd>kEH~n;WQk4^`uvgR>F}g%5&?3q<6ayrlFn+1Ae}Pp_Edv#v*n?c z1K9;umvFN%^U;=WS4`=*@a%%PjUe{0%g3^b^Km?eMJaA6Qh&~%A&X8C&F z#_DaHh(dkXwVTJEwgJ9JRyOp*zwh>k8@czsVp_Rg;#AnZ`# z7z{uG6t6U_&peEbjDK{^#vJU&Stg^rr~{)7MhU19u3u&QxubtBxtb8Af~=kjiqC`p zA=RHzL=~DJ9Ld_HBz-`;bBz+CA#i}V<2dR2D zQUOBD+I$!+ho*t|kjQgBw&8BtRB(hEL}EM-0IrTR?O;nIXE0NtEILPO#)VMR@sT5}ct!O{@9D3r0-_(=|xlB%i!ydS0`XX_Vp2A5yUpm=m7 z0$AaddW5rx^5Y()eLd3Y%lP7F!t2{(dR-e`*TvJlCX8o?ZHR~p8PO=|?Rq3e=vjUI z^4!i-m3{Qd7`PHtJIS9-N7HjZ)`D3Gj5gfKt2g=mQKL=QI4xa7$O!(PcbU?u<=6d= zPU_3ed!8gS=-=aKHctDwZM027758W1kEiqUW~i^%sDXm5%B3gX)XMd5n9vg7Abkw4Z?r^Q4mdXD`DLzWp2<1YJKmBmV~~-7a=~2%VA)))XetYT&z) zc^eHSMcq}2};{5{CTgs@522g3(v zuFe^R``n7Ku0kxnV5U#%>3fX}TJnlk7Z$wFRYJ+LUmfLI4b`oQt(c{88;J1g&|SrgRw5m_o26=9Jk7<)7ib&&}X^xY(bLlX`!Ilqn=ZLER- z3r9s$+}~=}pIWv63gz=aH2^DwiWclx zn7w^hcb-4Ep&{b%=goI7BfTflbzCaeibS?GVakBF99zZk$3H^tGHgltVX-y<=_PeM z2&pfRd`Qg{EV~H+NmwxEd*nLsnCIQ|fC!ZI#$wx$fbSw~Fn}yJ}{Pc;RnvB>(wZwg8L0stGHt+gBEx==}I>8Cdt2X!N`@w;I)&L}y9Kmj=nUZw=*0r}iW2I1A z-MUbc6Ke!(XPnphws5+DD{c5qn#tkEPip}QmqPcM6%X%_yS%>h3!n6rE0bI_ zwym}#1}OVPq;+PD+#w|tCI7y*)saS$PJxe;>PThEU`X(Z1-;U59h`+snU&k}d^}Kx zm47wPLT+>8i}wjw%3MRtl)B;rKatvgX}*Q2JhzDemqRcMYz^ zHMr~l^z*%c_jM)YNr1Cw_ss6hyk_3>RgPSu7?L&br&c?F0G%EDKSOJu^zMNxU#QGV z$x?5)%9Lvqc3t`Qv`2S!_kvP$U>av8>k#Y8US~T{;=5jMz;9BSHiby;T)%*>ybQm+ z(xErj6w6Z5QpFQ0H5+LyP0{bFtB{>a4CQ()*wSq^VGi9#w6_-oxFseq(&tcy_FDPcC|r<3fOq z!1JKf<0rKA<}0%v)m|3CZxxqfsRhR6n?p^pj@;Th|kM zL|1-v)V#%TxvwZMA=cJ|RCG(im~3YEr6?X|J3NXcFXYPnPl{uL+ZZ9P99J#UTFQ9S z8tPDl)hxpYmW_us{2yAQA=?h$c8?+jcK0}v3#gKIROFwgCvk|%S+Xm zf)ss}XG&kO|HRzzRiQS-dr4DEEAFjrSPk~mT*FBbwRwv0Qf(d=UGgM=pgLiH;4d(!N{afA7xkL)Z9GqPs97~;qxgil} z522!uUaC3=BJ8t)80nGB>2yYKq2Hu=Pf; zH?Sp~vzm}PnUL+sOt#DXL$hLY$ee#GE%$Giot0Lui(~%Co3FD|$ROPU0r%acOvc9E zR^LC}wqduWiF-~{Od4rst8VPaeJ^T`xt>D7c2rQ;3SHr6a%YBLnGWEA9@q-Qr*E&e zQ3caFMBMoOBOAmEusy7Hk?L!z-9$oCjYUIJH|<4Iv%{aYUVSwC+!&xuzPzS|*z!N$ znntH%a{|-5EgwD^1)uSUN;ff5!Ekt&&?I!m*YrU7NB>#cD+^GNXaFv|l3_@tv zTlc!VyQBH`bP02F@x01qMJEQU-($PfGsjq8e7=5*Z%qanzI^-XMBRx~s43g`ltcNp z*wHfsh4(%eq~B8j^L20!#?z0XFSxtGOkTg`9Ph% zpD0nCVUvFbDb!Gh&=zk$_N3ur#m|P%X8wyV&ds?L7KJXOUDNPkAX30XQzE*5Fb5Bh z4DG!Bk_t3BHkD;YFD_nJi9(U8D@;(M%|$cCuzu95*Js#Qa9CX*zhXp16eEo3iYMXl<_OIcO-FGTOX~gX*Bk7v z)F=P_%D+7HXZxjn4Nn5>h0$p+<7lBEV^{Pxs2CfK23^J9xa+4_^}O$xWsm;nrJl#~ zvsrzIfA0lW=XsM*N8rt~YmWp+3Y1*cZ^kQrqCJ>_J9~FQk@Dy5Dr^e8zSO|gR)p&Sf^Y%8>k-)P^9=kNfU%COHYFoghlU-Gh~@Pk;FN)8pl3toqfvW{hI|N>%I9e zOnMpJM?0DAe3&o!qkeMgwqP zA8Kj>;_;#t_%fPjp1x(rBXFi5Z)L?foyEosNWK6bB*we!LbF>@1zUR$`-79?qN`uJ zue9x+ahN#6B`uZ9HMttAC0f{k?gMXjR#Ek)BffK=4gUFG8g$3ghdIYdDLysdeG!Ao zJ$q|ca^1iP0wI`q1diNDX-!Cen~4Rgo!D>x6Sk$$Dsz?!(e{+NInByC2}+ACoa={P zYT<8Z%$$NohI@yXb9xnR-yN=X)A$~cSVbK$;Zw)WSI1=NHP=o(Er~jWt><`i(s;dS zxgyHVw?o&mPa3d^luCWh*?e@l6OaPixcx3z4-asWkuS;u?R`nfON-67(!fzFou#^C zp5Eyr=j1EHLg>$lo%>mbiv;Ga-wwNNNos^P-#u6{duP%mFONY?J^O0m zIJPTy*6_00_xcAo&Gp&ual2vn>_VO;t!9l|)} z?lLbNtz2}4EXANTCmlcev_kNo-}|(di{tOpv3VXKar+@{u>!rLMJ@%_SBigjFE+2FW8$bjIkoU`8!ll- zg7tdRi--k9E7c<|S*98X{k)supK&||?J>jYxSs?0!N(J&eYbEg zH);Zxq}x3uKDTw)k}oRLT}&*|6?hS(OSy(m;Wo>Pm0vAxdeEMhSUk(mYHB-;Z*X~h zQ{ZY3;7}CW*HYGlyF2UOY$tcTZS53(eB^8wcxgDw(>Pm+uxPa2-%gwhI>V1L-@$bM z_TnkD`TCO0=*z-l{78qdppj)8{qF{23(gc*jimOm6?f+KVRFbRDx=Aq!0!374eo#B#3%g5}7X;-3)yHf1~qTr|w$u^M~r|K9sr zkV3d-j1jadL1pQkc^%a6fa`w>za_<4Z3Sh(5fy-G3kTbqY%^gBRLuU zg9thGbuM7Zq;7s!Gupb1-%O^6D+&vNa!VUzk@{ zQF}p^3%@?j3Nizw&=3fx)foaJ`jTv|Rs%W+r%2j7hN5z&>L)t=)N47V+bY}gJVnTOY~eY$t2Z(0Z-QnAi%RFRyvY;b;l zJ}@@acX+koe0&HC2Xox$=UT#o#}4I)iHHGAxQ|`2HRh|i67Iiz zQMn0hkEe9M_R}7cMIImGyg7T5Ulr3WUEzdB6y5%)gyPQL11D$=Y}6h z&5&L%)Z6K^MM-18QHt!b)ftHhWsYk03y(PBuk|5S7YRp!h;YCwCkG_A0n!#uAC#Uh9u=)Ucy*tpMZA-&!%u9BKS!-^M&59!auDM)% zjoZLkvg=#!0BKbf&0j~Q%o;MihZi|TF@dRYJlreCOfkF-D|A9aLee`y2#N2wW zgC9ymWkyK;*6wc9l4?9@YJ7`yUJ-rwd&D0un<*@}=H_El*1{UKMM-h7;1M*_*#9%d zNixNy-MvzoHce(=MMtcHJzIBy&;ft%mmG_z%?YOGo|B#0vP>Gp{ZAJDHroxl7;2x5 zBeQT-t4~xgi|{QYQ_~&oXr;WXY9OGT!z@jUx4L_Q#gtQGdNt7BXds_NW{k2r0g^-M zFj8^_(%XZd?iPYtn@X9n&96V0Ey5*woj)dJE#QjOKsl<@mt*!D?c{NU6qc@279lZ9 zmCCiW7D@P)$040Jlz>VHS?s%txya(;Tds{~=cWkom~ZY(d{IwrALi~nDzWT{yn?2B z*?=xI3h!jXHGL=l_4N-#c}y|#Zza9p7>>FXN|nX2=~*{03X)_oMt04Ed{LIJipqBr z0{RH5sv1c(KttYy08ppO^P&%+aETYyJ>+@)sq?kLH@g;*fmAo5IpxDLOxKv4oE!~w ziI3`n)GCV|qkuA?2tSns#?MYs4nEKGu{JCTrkt%m-TR9pNWlY0b$TRACBX}1-3bQT zC1d>qmVHr$^2Cn(!qlebBVRV)M@hbv(5OZKanhNUxp$KtDZRMMp>kMxh0SRfA$)}c z(!JtpIhZ&qEnHZ5w!0V97?VC8xz6}wR9jbt9k@qFAAb)P%^Y`1x zT&r0F4h9bB?+_4{DQLu?whnVcZ56dUtEtKA!6=XYi`96qfX(btifH}!zqBfghs|{@ zqrarHaNsC0sa)I?KaoNJv=Vf6F!K*Hl$WVNmXwfq4Hy8$3}Jy8DNHzty$i)OexUeD zZPn*O38+O;7a^gH_5f^s!m+-N&PSj^B7s)vpc4}*V1g_>sDEPvD_<#9!qbzm^NaHc zCov{XFpN?(#?1#mT`l~p#S&iAr7e+i<2SBbQdT8@gE{Fsj?J{RuS{Hbf?Gyg2GS#L z?3B>AWHeRI)%q@DjnVEqUXMFTAC2YSkBmu=$qMn;zmBV%k`@En@6&j#IaxAVLg?x< zXc{BHjp5+47$tm;8ki)Y4pGUY++B;cdUOv4`WKs-T^4Q7ND@uPV+hzU$J{>oJwBs8 z`ia%m@xTR2ysEdo>ztWnxS)~1XeP!K)IZA`O1I)NA4;4?6UX?@V>7Fwu1-yN8H~oj z$VdgOg>Y=TeBeQtUo=Qu< zN;47SlcJOj!YYa^`#>y&lErJ0^`!FMs5)9O>kYwrK40_9WK_>sfF>Uo##D~75(Xw0 zA%yr)^_@f#x{4a!?F>Qz!43|q(#G+Ju(W7@N;~O^f%73*ik23+%yu2g97#7fUP$rA zjeb{labb+|tXNe*-o^y?uM|Ii`{iIo3R)Iv2~FnLGeMSvjNR-L-6k5Eu1f*z;_}=U z2T_4odhnlYjx`Q-=CIK994_eD*le(MnGgdeh*71pxqipj1jM10>ee81v(LM)`KpWH zWXD!o&oF*cP>?~yf|CF?h!B@(hbI6&CArGJUzI4CxX~fRgx8iOi$x43Aj9Ncksc#i zbhfIOVPX86xw~e^`ul};@BQK@32mIs^?(fd0z1 z(o%Fb<=_f0TyX1A*d?zc;^$H%ZmJfxD@Ir$ zL*7q< zT6ldos}!}I*tfJG1<76dfglnwGf(ehfkl2+p>I}#=)2f6!?xH0k<|7{aEGamRE0=N zEEr+#&)4bzN}{ScQw%OD@(_+eU#SCuyiM~r+Kra)wJegYArW;kR*=aHzI>Bv^Wh}c z4$*AykFr@V^-@O$Oe=6h#}s~kGZ~Rwh5%EDfxzo4zjt}L$VHRY+M1T1PYX<+;4w3* z$G-#9(2(iYK>SLxAtpDx1VPxpbqwWRaGf&*)-hnl0PR+R^8sq1y0M{OOm`cRY|_&> z&A6B}-XV*NKzCt(_}*SX^)LC~?ZG{ZuecIrBlfMoaZ3Cu&&yH3rPp`HKwDZxb`Usp z`hGF!`ii2qS~v^U@=fy&9IoCV_hIg_B2RR8J4C;9DM+mPsqEQA7e zs*%&<8k+E05;dY&UGtW{I3nL_@*XTM#KV^0X#F#h3;HR`Ze;-#s+RJ;n}P%_ zx7bQ+pe}h2?Z;G%keyxfg>6w@4XZM^1}Pve<^z^sOG2XT*zD|V1e5X}*2)ukw{GZv zrUF6mJazsBLsji#5O6zv2@M_mo7BBAo8MJ{X2xyV?CV=x#$vcfZs}@Q5|t&r45F24 zu4b>;7OP;e0fR5E4ABd%h(s&UHPx4zDX_!AgUwvnVc;YW!Up*kKU&%l6W@V+E1=Hy zthZO=Mspc%=z38m3y2c$Zlv+pxG9o5IwA+KtDNQu@~bMRAnkuggaEfKU?O74AqTd{ z0w^59))6tK6LGw-!n1Iv!c_*E?W{S08F^B&HvSXE_I=OKr@)FWer5yAIR%i8(%*&^OnxOzP+6c$_*5- z7Df<7|G2Wu7zj-*#LJ(0eoAS9@pFCUC3dtFJOm#I>VkFCy;a)=t`QSwH|tr2G3nv| zT{%VGe6|&}st6=Mh46QQLQnGtqFk@W1t zSvUR6elMfU@p}}wEj|`ZqZublkXPxz!u2cCX36-E8&k+ZKbueYy&bJ(5FzjK&o=gEc8azc2|$cHYF5C#|Sj5&*pgeuNqtI`%W z=ldG%VbTwfj&N~?Jcu;w*z&T$zhO?WorMKaBiaY#ODrTfIrYKwCGup!02&2ppnk}3;=QPC z@Z30YIN!fV;Osx@w&GhG-hLM=juodiJX@v@6pqMrZP!?g=T}x%;>*>h6KMKeZmfCw z#;SW)aXP(qFsP*K@8FHKSZ#@K;}SH=Etn|zETj)U(}ba%v2-neHl z#+=bfjISt-Qnr1yyVmVImFaOmKzhnMjWypJdp5{u^guBZ(2}v$SjDKn+7cRBe)s<5 zv(PW5a=$Pp=Z3TEbo}buT7~&5XvaC!r|F*W*SzUAuyrjm>sNk}fP{miTOWsu(k}N9 zh`MNt`664hzk-`j)*l}mIzW?&%Q(IX-UCdl;b%h89~$s+?oW65DsPSs=NtDJMEy?L z^26_aV_<(q2eNx)o1~*~zUB{#cOB*1X(%VV zGp?>%QDP}INz0?=PT8^@)J7w4g~i3DXB)j#*0kc%^4T2}n6;7)CIZx4R561)DdrH+ z-wV4}6gWgH+q)CBv{{&$4or}w{i*J^+!%;KuQ9o?*HtO|Et@v6bt9kv<=s~B|rg%~KNMEPv4by2ZI{jXTfdhqS0k!IWsYkYyAh0()#r7;JcnryG72Et1r79 zwZ-P{F^lD*)&n6`0}(s36Sq35+5fQsYxPT>-(|Bx#J|u>nj)D0`D;5tm1E?SXk*bc zax6M!&7s$DSn6xV*Q}`%{Sv6-4vHFv*1}Ew5U2myUrNsyQ7j!StFKdwCpf2KI+9E( zz=Pd8BbfZ~RECdt`$TAFZjl|@O>a%sT)sKk8bvPlBc3`k_bH9)H{fE&rATwsP0aQ( zyeXhS+z;u8u;$ro&ECK%m9)0`pX#`gKnS@op`5je9e7ZVrDFvLn_{aL@_Lo)Av*)l zVYStl>Huq}L9}-q7Ti;|DO-Z9hc4w7f-zo-P&iit>d+Ey+Xcx4tBtpbo!tbAJo*j$ zDImyF4FY1l$G?*vr~mUKHx8G+s#8Cw^DLnL`s(2#p-ILT@{RZ9W~)EVS(Bo?0kF&+ ze+!b^Mzm88%l5Wp@STB`ZsN+XYT57nv7VA)gMq{gB-kL$S*nqnXgj1JM!v z-NXvg{l!iyo-&kXl;{O|k9be!n01|axh`-iMnaQ6@BT4I@qFsGNy&C?Rr)@0!&Y-U zMeMOPGhS#pF+j)}fg~mvy+kGwx{vg%Pp~F7mOlQ#|9tjtpv~j8f3pL;#cDk|$J{)h zSigKQVHzC0XMdH;Q{af3oqw38pXK8QVR_RfT88%t3NKgG3ck^wpQ*~`4F>QKt;*Zm zNY9Og3i>5Ag#rD6E%MAx!Uv6uihiTN={&?p+sa#ijn_IwI?|PXyy0%^|B21EQKfmP z)A1XP+8Gi1+Nj4<+jz{eWvliX)EKo($Ma$`=)k)E?C(E@&hk1$i9hP_#g9!6eW@pb z4%Y3UMLwwRzy?x&>|cfvXb8K$?(h?gRDXWte`nxIHQvwc@ORC>H8nQUe^B3FG+Ed0 zO9y4*lRKJ!>!Isd+e^RM)g?wSS^qQIKb9`xfDTdeMTW{Uc=gXN3KrtDigD&U)He~8 zX+LRVjC3&u117j%SuE$RH)fJIT$R=SCPEY>Na{kHZG*{;MI~ zH~;zYdYY2Dykg()dYGNLYU-8C+pg=)M#M#HE_ZPs^bko(A5rp!1>M4ki1A%1C<*@0 z=H8KJ78l`axKA|49JgGYIU<@e;ms1S13I%bhoL8Q%Toa+5bo(&475Yw#Qr-!A5vX? zwe;WwZ}JD45m^Jo1(ugv2~5RIeca5{1uhW+2r>MPqttTw8Tq zR|(RY1o>yQO+xMYuKUlkm1WOin)R92m1*{ZI~S2w-0mc17A1eh?dS!9s+v_+0w?o6Kx+HSK1#4GMd+ zJ-f_gdY)Cc2^9Z1Q#um+PHd##70V?tdCqVq!7tHLWyN4Y>P1WbyobJgc|;Py`xHs+ z?@ksOg<1LtrQ8Z`-wm7-quL#jyIl(C3>r?VxeYkHyJ~s(c8nV-U}JzlY~1_t1_5c#!6MB9)kTc7?#?QzQgR3tGEs#5GjgVL5KZ~(_+flX-aOACgtj2D7PI#+x zew%)zI^We-=Nd(L)cCmayt?nG5#fLKW-To{CrE53qGSUvRKfG6jS=}eb=c*hYd?yR zq;{O**z3fw9rYz;WlJww^t16j84g^wdmN%(b^#KcHDd5%kR+ivsa>vdAcpgT#*a&p_ z;)g7rnz|$J_Gh)uj$t%7jfB34y2PZ=0n+xjLfiK$T@oYAnZY4sPtKd z9#6=%w%dnO6;_BiT-K7T87e%)?GH(Hw<&^danr())CTt)nZX54cQOT>?(c9B)p(a9 zs~KjfvAuo*3%$S#QT_{{M~)P1bGLq|$=m|7PU4B$>X9ng_}!{>G9vG+%gu29WoK;q z@xf}$!yngG(M%<8fe(ZiO43L!`lW+V;#ug~;Q6i(7-0cZ55jHjAc zpl{CVKN<=~WXB%Ldv%4puMS!5QI z=+aol+s3^kSJgT~t6U7)YUL zs((#Ok|+=-v_`-Q=meD*R^4kL1Xgj>^2Vg-!YedJRb(S24Oqbp%E*IEQfycB7NRQ* zFoI(CN+Z>G!U8BRamZvk+lai_CT~a}vm^CZ-Mc=O*>$>uB%O(dp9ZwPM`cGbtoH40 zubnCdN@&t&I&NN^rO?W*&@vwQpr&$$Wa}bhP}pc-&)5{t>lrJ$xxFXmo82T91d~R- z<+$3(Ef+6HA;RXcX<)K!SLEb^4Q5Gf?jYB5q#$jEB$T`{o1b@V0P3z+Roj1VdXVI6sW zT8%m4hg-eP#tOt_6%mdi7DI4d!edMjF2HFS6&8IybskBt_?s0!=y#T4t9y;XMsVTr zw~}(U>Lg2w|0#uX#Q&CKHqDbk$o;-J3MBfHmpBnJuEYPhJ`18&{CK6VYB$Bpr}@?6 z>|5f{sBN8$(D;`S^borxYxX}^8yGbHXrV{@oBNt2^Iwjjv~Z&b8Uq0reuGc_hFwSC z3df_O0>^v#P)-^#5jMW6_U6UdWUPDM^M?$chlk(NFqfe(Nu?&jBV(lDzG1`LK~FUVh;6d#0y*AP@Bfv%E0t zvupQEn4Xi-UOekYK8uRp&4r+CgN+KH_Pc)Lsfr74h_0@`6g9s4mImwX;Wbk4%$Q4A z?*5+5n*6OUQL%{iPoQ>sH)vHc@{i{ z6r2b~U_LV=yl2Vx`1oCVKjXgHn}2^^-;)2H_PE^&kNT;u)n`z$+~$T**h6Gi!jy`; z0$=ehwG~~4b1K4m+YAdbui%5xQD&lZkL539o9j1hUj~iN=vr*Cp8kLd z5Io#OlYgUUj-V;qX%2$+FyIfC%C|2b#OEeqhT>-%XN)PZcEanKzS$GG98vgp%YXc} z$h#F{m&ZliGkeGyjyhf0MEmd5xv)wTsA?%u$&rWo?Ay__tc7x?YO_5YSM0F9V!r(M`8lVyV~tP;aSNl0V83;2Nu8amVkQu? z*3;7iV%0rx!ba$j;z=*JA;Xam+20B2^xQs;XsG0%DN%c&(U71)r>CVsAW#kqkal|0 z)5`!XmA_TA8ER5oR#aa#+W>#dyjS^8+8eXNm_-o060~SdPpL8sF|(~1>&sT&WDl_e zcV@tXEjd9-t*8ECnEBXllsj@?Gva&f#M0~|Za4Pl6p0cK5!P{@Kb9@zAoep01&?P7 zg=Md=J0zpnsWCC-Hk%@+tnI5&xgvqV=nkgYZ5c?YLcezTG8aLH*(Y&B<4S|9pyYih zgFFQh12RF#52TtJ~@)lw3~F%oj!j_SC_68+~|*P zZ{*l~1LU6hQZ~uGBAg5O%~UqM$pm0<8Fz;^232#}w_IFjDViE{5FIy9l$&eC{3Ag! zqlcgqCpS}jNA%)vN+mi~j31xof8paTXB}KZK8l{l(#fCEV74Mq@JmxI1&zt)3A6sICE$%0L>gxHvvO z-efQa79RI$MCr&kIImYx#__~9`K0zB8!}r}Zk1!T4>qGllw=M!NOLo5sEPd-D*1>Y z0Gr27k*N34mi2W*N|3ERh2fj~G|+^mBRrge%s0%5qbbq5FAP`x{PDngDfi8?QLRTQ z1G+fqMr=V4$mF2g%P9^n?ps55GERD66^ScRORxkA3xI5-GO=fa6Mxn}L_j{P{*c^m z@!t)eX>pprWl&Tx+B&0&W#8XgA+VLBE5Jcl|128m`}`=x%D;mi#H#m4=ZVS4s2RfU zY;Yf{bJe4sh$a-JACuNOO+dYaqbz7De6#jI&!+tpBqMav_o00Av-T5tZY;SHv|$G4 z<+9)?Da&_}VVCRW8QNq!37&qrrU#+ksA<^7A*Zbh6LHuDrv$c0^a+)LhZ3Unl) zleI$L@xr=-EshirvRYzx=}tF{Z>ue7v4Jimk2{95GfX%*JGlh7!k$y2kD#k~;j3%Y zoxEk~xY1>2|K9R(=JhD!n2nH4h1=&zg~kdWqU1Dh@d9CN$$L}(@zGR1bSU>y802Di zJeDgv|#!&7$XgofoB&%K9;s|f{)!caQ~3)g3SRG&qDH3_LEiUei z@hy&)E0Pjkm)@FH>YYWk8+6X6_Z!0Q{bUrL-t;SY0VAh(=2CYE?PcFPU$aFRIm~p` zzzck)1&am_MQ>{q2c8~CL`6v=;G&sNps%U6AM0P5+?jdfUaDX>cZ79`uQgXIDMOGY z-y~9!I8Mq4`(GPAUe7j-c<+&rHC6cf5Y;?y3!0)Y!!b?C_kTRx*v_&G8SQ+2jxu>1fgK3zzfg)j+F42zT-G*6w3A0C*YbicAIUOr@Z44=z@P$V{5H4$g~QkF4rA!T%xgL zG$TIrc>EcvYgoi?mP~#T`a81e3*_nzcf;9HlaK&Y#(MGZWOXC#K4bPIu-tOz(=IG3 zG`dW6!;nn{U&Ez6YX&17-aX1#6PU%I{c=iSvR|2l ztG$B)@hvuxv1v}7-_tF~T3DFZB`y%E2^XiZ@DAbods-NG_|9Q#=)lbXE@6L8ImK#f z9P>qZ42nRTMSSd7eI4i#3i9bdZ-C1wA088=7rVDKueCeFV@V@tMtErT=wmC248 zBR4^?G8yGFcALMO(Ka?;?+TW8XayQEb#+M!lidvVg_8RXOc$wh{ABA%@*2@=f3YR~ z#ftCE_`@_m>Y~PRw1FEPJP>38p8YUVSkQW@P+Z;5>3K;Z>_z>HnD*>aJ@>0=Ud|X=?Q`&+KnrXo7}eCUogb-h2I-+r_{@3#sAekD z?FYBZ({B|NThqr$l@=0;jdNNuS%`XX=WB97 z!w}O#kTO~^ORsd$hNXcPsgAQrH63i~Pi;-EhgyA6gpq3}bU+_0CFo*bOLG|b`i=W~ z4Jik760gA%N9G^BP|V1}!V=_K;s)~c6sY5=%KSlr_=H~&34LCz4OYVBho@}w+>GXH z_DxNag2oX+1{1o`S`;lp-Mpd}&G`;vVEL+m74jsahVk+wbp0e{U=G*kIA}kM6EVZt z3~gk&`N?NW;>o)Ea&)7Lg8FrN%fv2;;n_zvX0)Miz1qk)yQN?M|2Vds{0T}R0YZQP z4-a2N_9?Ts)Gw1R^FFCH{3`+EYc-I%alQ~SX20JO0xu@_DoX6_HX?z6yy@BW>})k? z)cW6p7d*SCTWJl*HItrI)z}B)rkjFFgw%UR%HwmVx#*!-H{8tY<8Bf_un-cC*LZa} z;*^ZLTpzz3Y!{8@p1)Avu~h$FZi~6LnOAJ_i^^};iupYpj&vSAbs7M=nB%gXpx{=b zq3({&p+~~5#?c?KwUaMy2Q9ttz5ccoLc^%PYmK^s!J&?UKmZ^Bk&y}mqoa|SyfRxj zLC;Db6Lk#yVYlA@tvclFp?9xRebj_sk=tFIBwFUvikEkv&65BlV7FuC44Gi9r;o}fi% z@fnNFeo-~DjlrHVLek!zHKJG+UZEw#+=i|sv-m@!3Vm#Vt_!4?eV8;p4muD(UaKA^q9)0&?f(z8!LKHIbLf8^)mS&>CWkSZl+aX zG}=$@uLvY3i$kgN1b}G<&^#{B^DPbZ-{Uzj7f$pxU#H^GNvLL+&0MPXb z;DEV*bG`WuGhkz8KKc)2!v_GaT%b|+#>UT-k1`T_%>Q2qLNcFxID3IiS)zsz`20vQ zff@!Nng~T#W}eiGYTmll>HPg>wCjHrllDMV5qYqM8PZQ?8l^6dHuXih`xv51T-E7~ zTQeD5cZP&)_SakCo*df~9VfFTl+J`Oh->%ym0^TH@{k7Poj!bWJL%$ z3lU^|o%!X`H}ZzQ{&bi9sF$QFmDTRkvK^hrq0qcV?N`?t&7j(T^EK@sm9ViWQZ>a= z+e^PR9pbV&B#miFW#sae2z%JQ@erVn*o<=i(0Qx8_Lj zHDg5GrVI&WbaWKxJCtXf0PJtc*9K|-j>h}ctjZF5LV4UyycmcdVUpSE^Q;&G7`Si} z^L_w``O(UX!J++*7Elo?B8!QWZM{Dzk-uSKX8r*1dGZOgSAWBRg>wk7A(d@7IFEnw zm6Vh7x!ib-u#kj~wcq}_^+BMJ{3zVi)D-jJSq_VSf0feLe=Ab8VK(Axk!X{)x~)}G zE%mhk3zN+~%&`83K6J0JC*tRIU>2GZixz6xuzJI4bI&M|W5qBY4_CFCKU&xGaqZ_J zq25v0lk)pu$*t{e!1|HJ;%mB257hR4lyMv+Qr_xvsqLQ`j9SQlD4vZj&iUd?OH1pe zgWm}tXsrJYnA~G=%kGkDVPOI2_sQ;a`>n{3{zI?{f$egm>Hz>7Qn+AKNg@GOsVx}D zvo9$~bb^YDA6-agiLfzl zMTHMa$yw?Td9CWp{{WLqX&>!&eSN@X9UPR=)Z~uEW3mhY_KQ1-+QTv17WREEzfY+` zs_;wfsK8;>tl2aCSEm2xu271ENsr~rDl#)Lm^|HICI0%QYHXbOlLOHg$c2al zUJwlpO%YiHfUNxYN|y#!Dy%aiql^+~C>tnhh)s8$9j{#m`wFc0Gf3;jUM$wSYh{&| z!IocIvfI5h@Yk`J&!!j*Xb+@(T86FYgL|$7u+!@zAUxDNor@H;dn!$R%`?-DV~%IF zF*}xlhIG?mkHPP!+{m3GQtP3GOdBA6lD^%W>|$iH(ZK%As-Y+PiHQkuWr}xUFC-fz zN%ooA7AxSGI!#S@8Hv~r&4eKm^f;dmuKvvrd_g3(=~rtz?N-1sqr~%8CK8i#9sB$r zDfH&3vKy_4?0>`h_j4lhAHAdrL>T0zc->}uM$?h@sb#PJmCT475S=J}L5T581+YY!8mMug|LLX|oEoV2ZJe2bHylwu`JIg_ipS4bH^%LeS$m{K}hf z4@c-i1|kq&&$k9rv6Iy*5vIK(fVyDN*l>LnFp z(shYb#Ov(a*6bGB?G}nS`y@L_sQ)7%gQlwht{LF#@E8$IFwO_nd} zza26HS0wfA*2eJ^oFy%W<*}oMp)n4<&~O$iQ(v7-Ly#y1>(^PVbgccP2Gq)lbPS;hYn7C6X8$?VTbF=iWemA9`b;PVYolEa{R>oReBIefX z2i`i!molsC{n(~-Y$rX>Z9tORgPhT9Qs!{;^u_l}OJ9byAQoT8Z4OSXoRlh1R`8_<>%2Zxotec$c{4G$VH534yM_!a>0+0^98(Ul*T zCh0Jt-8*E4@xN(8py%P?83A+A&2(c-VbmlnSVIdw@4_0^j`w#mIE}zN-uKwd)qGHZ*&=^IP8}Mwbgg#y)ulzeP ztBC+h@17sX1$>C~-Jqq;1>bwI#YL}WA_cvQ#-#3wBCK(*?LbUi>R0ji-58i}ye8=6 zF!DWm9S?geO zvwR?dqqRYjfIkHAsYqLJSRWIrYS{vsrwyVHcbomT0w-P5^Yio3{_Rwff>XNq1tUM+ zvBC_4cHPd>+Z^gdk6$hp5|JS`v!$KvsMjm*YZ_=$dtXcS>LUKt4=6zrfeHa&Y?2*$ zHE2q}JRVC@S-zuP+RS>qgt7!l1|lK^!UbxI!;_6C+paQqn6CtDD^Zv-aPWx)O2NVQ zu%p>diuj*xaeAF>H!SDUuXqgY`4GJ8(QFujo+|}eS>OBmWN~D&tEztF6nISaI{*a0I$>bR0W{qvVUvO~`|yuaJQBk6i^kFU-6I`9BQGTVOxC`pW21u8~X z$zp)V5F0!ZzKJ!9C%S>-r;t#XUFb9FQwl++=6gJw2fM1qfYva38ksWY^ax zH7^PRnVx4XR?Y9Ti`9PGN0C6M5I$3c4OKvF~{QA5&iy z6lWJLn_$7+eQ8P+V9Hl-JTIO0e6LH1-zC= zy*##`zIECN?bjMLwel_$V}v!)1P1Ep>yPaW#eQzP>PLbwRSNyY0+4G~Ly6IJJn+9V zOWMs2M$C!-BozLt0sp4@mLP-QjRxc2{Z;y{@-$O4vb<8$9gh8ZJ@_3FcGZHzx05mL$sXeO+Mb!>*&xRr(uPZpbq8`3MQPdnN;A@Xcz zh+FvJ;26-SOs=&;k{YRNKEw}V?ua9wbpkmozz2( zObRr&NDEhaUyLxzncjdD>vTt8EA>WwGu+>{uc`?_j-=CFUX*^;?z8;sikZr#T{mYF zd*DWf{i98xJ`A^-5x@H&?}vHyC~JLm1DC032lmFwO2qFF{q&0VJT?SHB2b02Sj%A_ zW}!kjH+Yypa_q{X!xSu^4KVt*ZfY2s%ylpQ?6b)jgqN(Opfel2PVn(^n2s0FUthKy z-@s-^O$)RxXvk#@K>A;}F*LN| zAys{52*JyXGE&NgobIhiR7Njke^-)6`u)iE8uFLid z&v(8hMR~h@yyg3d)?l#nfTApSIeJ!XyRQZzYkWUQTlZU}7P^}{^zUv*bsctHExvT2 z7rS-K(aQWS|2^4@SaC^oP$=leq5%#E2#)u|Y{TfAm^{_FIe-<^xZY!ZI<{iHT{!Ul zPt_XBa?nDQXzi+FY23QRF`>ANH|-2A^}1U3Gpz%eo%LKnrLVg)bnY``a!>Got4+~= zFZS~}{GHFiw~<=;x$m|-Veg3-b@#IBa(C-&+3FyDM33v)q37R%p}+G&lgXR;;}6{~ ztX(fc`5L$;cQ8M(;+Km(r!G3(F~a`k>)mVQF$KYE?)}WPF26OOw}UAw{}%4h+1H%G zW-rs}B9edSHMnn{^oNt8X}o9w;oOQ3gU6wc?>DyZ&QobZFBgw6NM8F_o9j~~htp+h zf8UyD{(nU!; zo!#DE&<7b=FE?EazU8qwmGX!evP~;Sud0uMsl&zRA!M+_K~<2^#3hRmsK}Pg z?UsoQrg!iD@b7@z%|D?=n=k(N+?JO+k@4R@J~??iB8X1GNKcdxE`_rfvR~ZEdf%Ky z+t68}5XDC*%sP!Y&kECOY8;im%*nRz^qaZ>uaW=0{TmFc9a?TmZG=P$q6OW3aq{&< zmCk-2itiOnlpcoHc|S(4uJvu)BptnnMHK*kmY!zW9b0lb*c!;8tybb;_RHc8?R19F zG3WbLT5n3CAK$us0{6@&9=_G{lFXgd4Ycnr40L)G|APwsb}1SRn^9virH$r#KDEHq z`5>VpgdFjHiTg0-sbBnnWYTd7S7UU8ov)o8C&E=D`7hh3Ys(U{^(%Bpb(->V&aeGw zXMm62i<&iu=3?1>uxHufe6{(kdC8ElWS(%g8r}Z@ff?mx ziG#f{&6fZeM^*S#jcnN?#88T*r53NhKN&q(u^wWH-6M9Tm8=~VWlcA}r;p}4iK(>? zg3IbWsu)lYO9#R$mZ3QerIpK5IA8e2Tppz%W5gLDQY~dkZEb&(Z{;XonmwAsB$zX| zY2K1<2?KlcAu0THN51&AvXp{~l5-S|?@a$(+te8x_2igfiEnKyQ7s)-X&W{1)@`-=D5)m;Nvse9;b z=^niUPf*3x>p+G1j5o!%`N{I)c>*uz=F|} zGhRHL`7gm-X>WGYmgZ8#1Lou3H~!)3`XsD7Lh8l?POtnNIo1@%PYNtUDP?R!()evL>un*xy+F=RmOX{g{J!v9u#0C?slM7mg9}by6#SHYRaAqox!|!k5+32H?zp!6*AGI z-&gcOJWvHu0h3}(nA~fMT8uQFv{l?utC0PDar{##NYGck6NF|ohV%Fq`VqMFz1Bu3 z(zrKOl!P-Gg@ivfcV1|$NZq=4U3a)~2cPp>`T{IrLReIWb2d6AG@lp-73Q_L5HQtt(cUR23D*ZB1k2B<1!BXeSzsjr3Hy0gMndiw+ICz)^NB;*M|7qo zjDodbQ?~&z#GmS$&vX);hx4G+$JW<}$p+@bn2S4&weVSXZ{tvgKVAGq>f*qF=uHut z*uhE3XKyVdtzw?B<^+D1p~I%&WImOCvKVRRLMm z3sk=Pf_IR#-N=KB49EUy0fQN7vEh4fIKu{(2aq8KQ`i(A_5WIwp#MXwZhXc$4?=%D zT{?#5%Th)`a@FoF{6h+m%3zhZZhD?WG5-SiR@u%pvix@yPSBaH^DdMpVzrvj-|kk)qKC9_bzR(^+Ipneg!S*L5APv zAPVhY@SGRv5NfmzCxgDrw3X50QFb-Unv5dwlU4R_ROR1JPofL&nZr~@wc!o>tvr$M5h?;1DwRjiB4wQe;^Vscwyk%) zuind_t@kMs66vgly@C#pAGseocj;uR#C42=U}V>KxA%z1_Fkgl@`bz+@baB0+8Isu zyRHqg!C7gnu2X=K+dp-4Z7&~AUzKiUg3%yOrHW3G**O$4b?U;7;@ml|++fHoXseEE?xtbLuLnnSjLM67p4D~q^ ze3Z3O{k7E{567u6SPXJ1X;cF-?MyfhN55JhII!Rub%K;(ahBDf1sL|5 z>+1g+9H}E~X_o@38%jTODL2CX6caztArz*mEW+eb!UoHFl7F9Gioa*`1*NCsLVS`} z-a@mzQw*)Y58$*UemVhkW5hknBFy2$uXksKEcbxuh_Kf_OfFbaV;r6ryfKwu@o72= zEzFB5UAz8cv5ZSXV>M575RHdoa+J3}`AW=iWwkVv@}x*Q`Q~k2ZUgjgp-=w?#I3R9 zPEX{EjCDSeDtKBFIy__+R5ZR!&GB6>)HA)Wy`B;I=WUYsvEWTF=Uc1d=WGeL0uTQh zPj4r!B^2BX^@iwheXa%Vr0hN1W+y!|r&$~$yh(xrpZJwT-yEhs){f8oZupJ5rauc^ zQ?`Ck(q!igWmB>m4}AV_2f>#(>X|zhUJ|#@rme43xo?~4UzJoz_IpSIHiUEB&zQN0 z1F*a{xnh&v4NQ700ZaUO8|TgcpVVk-7lt~!P*Q4A39U;>^jNBF!-G;)s5i+%l`J@B zDnl{fO#4CA+h&_BwFB>ISJ*z&<++@Rza0kWX|{%@DxGZT4_?nf4$|<5qo(zS>9+$= zR{Q$>;&~32(r47Kn-BqV_mc5?&zTXaXk~cN`pqZPOJDjAzoXVOY&UAjWvd*=+<}9@ z+hx($%Qv^@RwF(u*`_N4oi5X%QRqkO5x)3lNiWxd-uB%GG=l4iCGVYaGK`>jCqLAE zp=5E3!8plGLE0~w17~rn-us#ROdmT8zzdBbQK$BNR$7z3m|m07DU6foUHy2ZrQ-4AF)02PQrk?|*{@{q% zh&OJsqr-rk<8uPupASJij;p>-X)?vf%D#djEmaBF%xZQ#9+l?@+#Q^q*-L2?su=L` z#t3P7h+I;&YNKc5b{gb%FAV6Mx9W|yv`-UzMp^WDxV-&0K)o~`xpvGE4>n~Y$q)cc z9s$511b`CuJ@ZnXRC&icro=iFnF1J-D{w_p^1Yf2VBDD;#vz`Wxs6XSQtUgk<*@-K zB3|-92$zr>5R=1zZ}V9tX`7Ory%iFo4W;MEJ@_m|2`7!w=wU*W0+59J9427~Q9YBg z0#cNBJ4o~Jq^4FQWEVMC`)$$^FJDVED&h@<7Hzn)-ltYO_?FzKe+?-pf15ETi$@4B z-0}~A&rr7>tK@$r`o)P?NhP3Y~#wrD=HxsN2GI#I)>m{hE5kmSc<}^MDs}cPS1-?@Z|2{+{3;M%l1NV&HxM&2ierN z;g@yrsOd-Smqfxis)_#IzmrzH!c(Fm263A8+?yijzD^>3UJP!h6HMND{27~llB_{W z@urhkBr1o0%UOYMShDOd4%vP^fvLq7pGZGCtxP@JE&Sf^#?H?uv9~xbhmGlbF#62P zPyg;LAhq;e>D;{3vh0@kf`^Qp>d|P8-H`Ce<^v|JK8C)I_Po|NU&Gw?8illarNYY- z5|ONSTpNo7Jq8#I`zPK`XORw43xFMPvFy>g0NBxAD+LI%um^n^a6N6X%!)8W$~}W}t02wx1-YJTJ;2Sn6@jOE66^qaWKDQYftgk-VP+dKmuZKu6k}7519P2}WFA z24Ecu-|^k53M9I`zY4t^MQVZD5<(|nASLD2OfyTBbv33T_V!Z4J7HvtdgA(+&Zf-g zBR5e!rN*olYI`-W4$Ti0G=OyJAK~X6^U;%SHh=BM=}Yp#8Qbv9YgQY!Cr_$ib@Al*q^1t-;K^4Uc(8Y{5B7a6{dc%9VjvoedB3 zr2mR!y=T8wxs?_RBF|;tq#%>Fp^<~hkwXBJvsp_|V#`Rs&x+CcWG&%Wmz1rva2gbX z6g{~ub9E|%s7P#2b@)vt^O*v1F6IwRQ(y8vP7Cn0uE3UE+cpM`0cbP+Dt|MFg8i! z=@%xrdOiH@Qh7mRFzHyJwed8Ib|{@E-tEmVJoeX;iRF_}KM?Mhpq0+9H`7SlR?)#n znQX40YbIXPcPwC*MW)|~IJTETAvodnJi%w^()(;8V0pi}s?F>8vyso>DEA2I&Ta>b zQ=mA29XYy=*7<;ybvHZ!==MPJNQ0aC;Z$hlpLE9 zhfEkQ9?{NAG8QMul#n05x)iVLf7M5Am;bwP2^AiCZ2VXyMB@7l8B#jzVX zj1gVyz1zxr9m5-PdC893SsCsaS?ayjQsvXMhh9n8LnXY8@40LzROt6HdhT^OPSA-$ zy%X5qFrOv(UDD8T1SW9YSXZ>~r+%>!Ye*ci z1*5*@S-)3n=3x2`KWWdimlB_uVC5Nd{!&x>k>vu9vBN~YlJK=r+2$=Yi|C0~*0g9A zNu(ZC&wJ)e{RpRh1|M=*$9L$@{HX-xq{9oGLDK_dIYZLQXxGbcVE*^u?kK1l4LbGy zyZjHbr3-z&5bdvN@qJJnxjL1uitaR-ySt1FjZcM68T8xVL6I5{xwJkfw~bHlYeBmk zJOB8-G8X2=j)!yKUat{*vl9(cBzW`XVDl<9ukm3;o!;y=^&!Fw-frSEn5K z`?dx~+4T481S~5UZ@z>H@;UGrMd0vfYHS(u{2>gpRR}|e#|A|Vr5X^FjFQ7}t(FGc z*rWANIL@sb{EP}zo-~$8!5B#aFpUgdk<^JN4c0praVQGeZGYYNMO0)eXQB3ft%9Sd z=aZ)1|2J-S#D_{ukvv{vqRp<)sJ%>$~nn=PAQV92h8H#6_wp|lc1_fN-$ zZX#ZAKvJWO{3NsLZien#cz3OEC|N-2cH%_2+a^||dbq^%g!j&3U>nKQVw zAn%B^%cn3Bmh1`TkuU5SDXn?nPS(H-+WddXoKu@Q?@YG9cvl8ZRhN%_ug0%dNDLZN zqrV>a`(C_cS)3G(mOb+T2pnjieq1bLpZK7-$G+a3;f`JAl~G3X1; zuc#h2nOauK+=p??UDwXg_)e`wfy0zB4d8N@^NYxO3zsz$1Gnt0E&GeDIBLAx(-hS0 zgHg5?(R-(awgaY%ZZ~?ob$9+vpSpxFZPA&Uap!LV1-)D@0Q?=89 zA^Xh9$=il5gUyj{lPuTk56FF)+wni1g3DdkkN!Px2&=L;9J|<+LqrQ}FdszVdPB~N zpCB(0+pmLY4^*oQh+zfVB~+7(YpP*}CcF1pjrja@e1E3OYp-jq%vx3gk(4-xTCkK! z-(NSM$JYuqr5yY^JW-*s&;2+WWk~ucdxPQ&O}oVR3SL(@7gLy>E>&>nT7Xr0(BHZq zM2?|pTHS1GW(#u$9tB%!A~Sb0>)N+wdH-5Q_X<;f`H4y!c{yG~NkZy#?19Nyu8!<| zYr35Vi{aVh(8*%dY~ODlDeOse_p0k{6+D8PO(cd@+xuqP#ZKFB`r~s4C9mtvBg22{ zKFH4~V?qg%N(%{K@#3!)okB?D&B0NGy${>WIiLXt3GMx5MMCK$Y=%XFi|wNv zjKoDej6;ndNO)|m#t;joK@iz+!}n(b9U+`Rc-it5Oc_@VfBj<3-NmyxkOF{1A)H5& zJ;+W<%x+dfqNk2P4*M%1Kzdo7BK<;NGTWnD(6l_r5#Rh0vL|gvAKL0;f-aanwm6fC zGic5pfkvcK#2*X=DeQ1>1safYyn^nIO)J*pKA# z;~W}yz6?H1kE#p4qG{7)%igi}cP3aCaH^|u%AgfVY7#W&mJ>7&>JoY|Vp)ABa0=QF zVsdc4N*n8Jo!UvCDSf+`bQBz_XXsnKASP`OfD66hg~T~yNJ^a^CJ6518QmNjx9_QE zOJ4k=oMUh!NnBC&zTCx*Z!TwR*@<*?bZKO>WuNHA&VPTToG+lxCLo3JCkx!5MnkfSxLupQ0Hh#-H2F;-3)mz zPXZ0icY6o!RsADRJAKza{Y2vXTIaQwI_J4Rr5H3A9vCLOlAHVgv;dN1St~k^T!C!X z`2x}3HeLdHh}u)hE<_VB%Oj?Hn^s;U=LoF7Gu0NX2SxV^y|AK+IHI)(U5^lZOuM}8 z`QdGw_|p~7x%pgW<=?J^akl@PaShWV4tRH27uhW*4$w%C1uC1hZA|XG*r5uTzdkr} z%6c3ViyVAc$A3uh|M-&9emgwZ#(@Pq8SvAbA3FFon@hZzLzXKN9qPoN4mz-U`k=Wk~iNctkqLAeJrVY{n3?bQ}`d76aTdVFwfC-=&_~Wpvmp=pyzga`xT2=WwRr%ytXIlQ6?ZC53N?}NzvQ;0DJ1?$R_V) z`q$agLa;+*f$y+S4xb+bZ_blNWlfICTd1kqb}<1Yc$ogI;Facu`q?kccCM^2^@Pe! zC10f5gLI|>t6H>)Y2`)YSG?ra*wpp#0ezf(kQldhcj6lNsTaecu%u!qrQb6*RBT_#zeHn@R?D9 zsWA@SRtX^jCIcD4jSVVl$UIesx?(@yB8A2(b$g61T_LG~A;ht{%BB!=LJ{`0PdSNa zTr4<6Q>z$M%Ey5$!Za}`CJjg%zQ&%wWLC;G8v6;j+C^a3VSY~bvh)9I$Wl>xL1e}W zQqnLn5g$c0%L2Z(tt>*Xzsq&j#8NkquSImr4#3q%&DyRA*~b7LTtk42?cr$|B^6_( zg@vSomb>jNDKw7pW^%%6(#5kjI+9-IR@+S%#b5mn1KaCxt$R?Or%f^patM`mGoe0L zUJ4X0W=LYGPZhQb{SFtDg~v%_cOPU^kSsdqT*H;A0KR)(d+|Mf(70~L_nNIfxvu+o zT@PrQI^nS0I!<;9h?`~7(2-3~BL(>wctaLeUjOib`*`{%t;D5bb%V3HJxGaKe$0#a zBk*W*R56lLTXw{-jueAvxa7R$Qcbe)ob#>p+s(^ivbTB+Z@zDmv@94A24#PjF?5^r zay#0s)rmGe#+jP3bjQLpSPg!m9!y>$w)MjUF>KNZDNTb8;OYzX6W4qeQ5XchH1@jQ zlaiRTC`MX%uByb#G+xNbkh<~6uDf$3rsa<%qOveQ72=!S;7s2*tn3@PeWc|)#rm$x zaqoJH;R$OTWC* zdqYlGE4TLps;-Zvum(j~E3h7(__1ua8D^1-gyc@WGjj$uz(u;=eVS@NVH4?k=Ky}( ztG&Aqu3q?w-^ZC=9}bopy;NoMZ9jS}7kdJ80OEwO(7ZekZh60#=Sa7Y4;uMDT6Qx# z7zRg$RrkDlJA=DpKZ)`>+c-|0552nGx}llp_~0Cp1%6o>|HW_lF!#2TI{VMJZ=y_T zyIbF+jfogXmk#J@)YA(ok1KERqUV3+Z92+2eN%`G3XJe@S>FX{?VN^ohD=XUvEqRw zqf66GOA?c3M`e?-=?Q51yyz^PPAY@$fOEJ+5F`oPll)lh54gKl?dWd%+zO5WEyh7y zyIx+4CW-(xkS+l+!-5bl;uC~1YKC2zQ6uhyg^8iyc+!cZldoQdT?V3?W0mHW*#6+C z3c*D3$u86Q3P<@=IghE(EkGy@OJ4<5iVaXjzaiS0jxu6e!qVTZl~ zH%c`cMW-`rbvdrxb+y7#R2o`A$kq0UX2fgRsGq7KWuXoUN`eU3GT|gzNH!LWv?v~p z#JG?(mg!1=^wV0t;1Bbj%HA2i=D6l(YKkcqk$T8q`6oi8dxV69Pn*{H?7&IAPi0CF zAnso?eM%S))GXZXeq4=C3(z$$Ebc=Hu0ycFD=!<_do8%hM29zmMm1AscM@#btjQLD z4X=;=yVUg|&Rn`Fkb;!B4o!5`Z$Y2LTDxZbTBUcTjg`dQ3T%R9i-Hb(;Zns7O1vZ$ zPB~z=zV2_j@u)IBVcz=f#Pp~+Ic6H@ z6}$sX*FN)@E$#|X*S(E!IV0;iSg!eN?KIL%R@UAUtG~@`?YgGOd3i^-2mMc&?@6zi*_UNAJ)Sc@ejLrTeMkBZZHb?B3^@Jg_4V( zu&Fcm=Xz~$vMUON$Cjencm7#bdP> z7!LNM`MJZCLqxrBj?Wm9fz)Oo3Hi}42B1J_xDedNxXj7{tQlhF5o~=2T6vD$t7zTd zQ(t=n*Uxe38yUoCJL zm(mgrPbKgUAx-gT$Nn;0K5mjBrB7JYNA`^JwSrtoc^<7!>IN$9ABe0%@2sdV+dOfb z94h}lAWAY-Sw3ei4FBe+11K+`pRD!PjDrWAr@LSb;H-XAiletJoX7-!q}>lqk~x1Q z0|k*0bI9a@aJol(TE#Ejv+Z)C9gy9#CtCXv3LDO81#756pn9;4)P8V&-hOg#v0}~{ zxi;;W2VK+tBNFuqnl=u3-yG8a-jj7_sGoV*`3}bn|1Q)#)s#)||8_ z!9ni?qH6;dk9y3eL@CdI12=ajHEB4`A2j)B-mn7eF7vi`9z zsua(<%VrIcfhhqbB5&u?>l(Mlfo>;*DlTh8NGoW0dwGw{G;t&fpp`-Epzd6v5ACTY zS+8QEc%5PbHutXRv&(Aby|dvKM?&R1W#s}RU3ZvYGL24BT84WLUcNvP^F?h(bKu`# z5I8#dI7biU9x?*_V$i@3m#6hUdWMdCD4D&4nOHtYXWC`EO3 zW+&AaWnLScDQ70nCYO{H5~m-AO`(g&X=5GT8&hY8#k(t$6~>*6=mv>aY`y|WiAx-wHS>4r0daF*3jN%gtyiy`tID4o~kV+m*ow_@q{GB24bo3 z#gK>Y^TSgrck4<~Rn|44P-V}}`=X|4pVCGwx)>WRl@>K5JM+a^>B9rq<8y?yVO7S& z<|ogI$xX2PIsCC{^5#NZHx`0s40e3T-9(XZjqO^t3*W?M+>JA&C}xiX4AvnJ6cN%X zg%0jwm`CdrcjOjgsg z&ydIWBt7+f3~l5)oA!1PC)wU3ZHAYoDeOcmlhK67q+M9BQgL8O8Y+J%(m!y*0W@tR z^I}mnPfY|yLMLt~1pUkVBb#tgml8CnQ!6q$D~UAl0F^ZVeC4E)73lQ}7g_H5XxQlO z@o*X^@~RDtN2jxDT+m-~yV;F&{ipiT5AaeccKFElKG^QpqecStQd;aI8d+%+# zVU>44iV9yQdv^})GMOPy6iMg| zmdD!5d$Yqi@`qDidS@|~r~f+bvy3k@ZD?6qefM^}kl%9vK;GE9FTNCNxGIrTx#VnD zu;U)$vJeDruPWlo!;0uB-D6Ugfkkm^4lMk!lH=&-FexTUn8o?~wgju1Qa2e>@;lYf z-u61_KRYX0QbJLBVJI1y4!`Rtqh{bK$_9UgG`N#6U=Q&Y4|W-t+u{7tmtL)*a72@U z{~@sz^_wdO@N46%1iXY;Ke2LY`zKCXyNI9S|Xh1d+(lv#Rl`>kKYExaq>2q!TeG`C7J>TjyF zpSE4$&@Q_1SKS5Tt@I(bFHD3;b|ozO&<>CUeCp*c5(YAHhGWgV9+wQ0U14hCC1aPn zfZlty00m-r{us4zr{8DfwxJ>fWS#fOtU&8pqigkXE zQ$$(*^J=uz57)pE>}3PvX-N{B^` zq5;2uKuN-vP(9MoO|wu{8?eFPV-hz;m5sJRsp~YwfXrCMeO}yPejuYsexAL<{A5?n z9;HW}ce=09sbycyL>JN2B|980V1X|0hhDa(Re=`Y|A^BQA~zvnP^}xlqOman(BholEJ+e-`sy++N>CvVl}Qc)v%p!%GyQC{8q$!x(Sr-C zi6|SQ0H{-9D?x@)F4dd;`eR(etq6oB`u1JT=DgAQW(O!_QZ%Ft>0VUaX?1W_7_k|V zOz~9as>u-TT|C`gPHqD}wjrk9^Lvjh(!l)186z@OWpJNNkSW)HT`wIY<8H3kU$YSs zud~xKn->Cmtjmj6k=p}sF(l<$xW(pNKYLU^9_R>Zj~jLq9fT$P28t+cxU=%i#>Kvd zO}RAsu+M1*^R2KOwD8jcc}hZ-mpibz%AmF}q3Y|kN0I-isEavjsO@H;qgc_m7-^Nn zm?x3bzy?~@eG~8ZKQaJR2(7|#+uwygmVd#)G zMmuL5F^!5C?U?-Pz0nan@=eP_B^vZ^YB@i!G<>>2~kTxHqWs3O{pkZvWH57Ansn3gx4LnZlP%UT0&kTt!qI z6YEGcve_5BoZIGX+2WP?mH87LR4E-uL@*Divm~sO#F%0uP8m@?Q*cczA&U49*sdG|-1-qeEkrA-zTj zvZ>#F7y;YibP3|~3FMMyJ5+Vb(PSjSj)bXY8Rq(5$EgSmO_4>-s1Nqn~$W{3HyM!&5_j~-=@K^qL^P#Ya4 zj67lA&jB;eBJ)|AN{(QN>Ku^i1T`oBwF4a11*|h)ju=dle$qgnsHWPZPLkMu+A=^q zE_G4Dqi1R;ilI=CHG@lEvNXrhQy-t0KnM?M(@UkmMug1h{;)ZdWH>L2DHJdGobaJ4 zJbUMfDI_{9j@T8iPhDPQ4H0Xq zNZR;=#jf$0gVu<_Xz4c(jDoz;(ooAeX|=`u-t_hJ)P=uMAq4zp1xMpD@+vR3;FkA1 zZhEtS_FwQys$DHyaf5KCI)Xu(D~Z|To_1V>;E00WXf?`ks7Vv_MSHx&mkR8hvV1lbKp)eZNCY9P9YbMLM=1ZZifyRiKpcaAXa0%c@ zFY5`lJV9{@oy0N^f`0PPQS%k{NjYLkcqPd#*-B?v%Q!lWq!NZ#c6vW_TR*uP7E5#s zhH!EVlmUzo=M&BG?$R(JaW`*10)?>y9)`crH7=__20)v{RtUuLVvUi4WKQi!(!;# zpU;;3(ce$0BBUK#5-w02RW_)ASq72RBjg<>c^HXp^P%RQLVVtNhueI?hI5ZN8ZiYI zrI%Z;q`(@UQpYuX;FkYgvb@7YKN~XgTpt^hSGiTz`9_AZdvBAQ-9t#(an)Bq>1bLB z2V~%!m6gTa>%%`gZdrPwZLRc_+7u2Lzp-%*v8Xuf8$2 zarNJng)cm{vGbL34!zJcG5NcsJPVm?L56yXvk>KVkN2B+*s0w@CHVFl7~{5CZvJV{ntgR-qX{}=pGbK2Xgt7jQ+4i}ycY3)Y8b1Z}* z>Tb!7vzOZZ0)e=z*h<6-FH;;@l9K8Q z7|N^z$!ZtOvOY7GsAaRnQuQ_fi+#JeRqYb6f|9-@ou*=VYg`x;B}PK%I3~Ev#XlT_ zwTQkxj#X5(em^W4vg(O=WNcwP^(l~rhbO2SzC~Yd_M+W!O)ni5QD|BcN`K_H7K5|( zc%Q=-W4I#$)f~%goqS~}pp^yDp~I2EYO79LnA75*JMWw+pS(0$W6Ou-Q_FFof3Txr ziH5NL<*Fu~(*cQ)X4zR8q)e5e=^NWA{KqkpO9ceg>SKkosMwHcRvG6Ip&fLT&OV}A zSy}l*QXU|2w1K&9*WB5-J(stT(1$zU+!~+TgW2WfpOkpq_9YDs$&klxIFW4ge@_r> zH#0T)uIS}bV^A=EM=cPdQ*1juvokg4Pv-wQ4^1DffnU6lnn-H0B?crv`oa%pU4onc5KZ(y$)t*Vau%v({-CEziAS#(tSkNsk1Y8$s&Ah@T&%*q@Dmrh@SavlYM z1*%M9^NVwy(bCDY(wy-oMJ4o&^79RtU;%0|MwmWRM_J=y+z^Hy$d6`H(~(mu+8`%> zf^AugI^@||q0W3hSC845TpE-PiOTPD2QbFc-=-JEt@AMayvo3G%Vw|sN~A{KMTkCw z`D<85eo`r!qL&l^AW48b*TP7>i+U2_J_Zt=oZ<^O@Fh8iDG9_|zdYqd+8XGkFRa!s zLjGD&GaRiCfHzVDar05m`pp6y&_NUqv9H$hx(^G&$>h z9yh$6A;8gHs~jj+@WtJxhu*g!QU)nCD>&r8+O#@4G9f`e-Y(LH7>0Oa1~iuI_%SJ= zNfVU;js?|h{!Rm7ao+Zqjf1^whx>Vpv?3Za3;8IpOVjFk2D&-<7 zlb|RnVV79?IZ%}5`hH;2ORGt0Mi~jqiF@2w^qXj^kXx*Ar965X#!?nV!mqR;djdK9^68TKSeo+D^i+Zi4#h81qFpD84GxTvoPM~qoK5- z_!qCTPk?wGmv-$xli7&a^tJhJHl12LUC$ zomtAskk|0Gi_coo#uGx}G!!@iEogp%^z#uZY;O^oeg0K3n%$-t*r=P07k>>?sAmer zoQ*Q%f}%8%TK`3D8DjOFIvGAWtQ!VrerJs0q+vg^P(1hty)6!BIC?67EWL&@Ji(EJ zZ6^%p;S{!yI;U35VLG}e(O70_b)4hcsxa`t@K>Mb=~#xHhS;dB!!LFMS0ZQtXKTI# zG~pQre?3;rpIF1g(s*-@qD*XQD$4q=1f_KMn0$d4e{5+^82_58O{hqk1(Kvj(e#^O z#W!S1QK?i@|E{FYPfCT= z!5np@WhsLs@T}kca(ijqaWt!>r|wnrWv1-7{sphDu6iWM&N_71@MZ(aA!&f4BClt2 zOW>J1$;F3`w*O8Kveu`zd=c^;31iS9l}jF~Ak09O0_P|`0+ukEvXs;C9l5!*RjTdA z;anOC&^Y+LfE3X2$9;vzFC=#y3(a-Uvxq=3bbhs(9 zOU7#Yxy_MLsH4j4*s1Zkuz0g)MWx?qs%1nfk@X=N)dCu;!;6M*Au)$=zFqCtDg}{-`Ybdx6Be@rpTi zu-KDF)d;lT5s^6~5cTn8)F7u2Q+9;Uq=}i1x#?DCi5_k*#|ER-rWj?7$&_wE7c+z& zvM2x{5t3(TOU#QA{7{hP1H=X$Z5%BW-H|24+}ydO!EP0!f#CpvMLLG-DMBa8r(dX( zT|cwEK*Mh`cCjszpTSA$%PP=-+F*)Sge5v@dg*2R!5?ZDOofyB5{q#b{L0?APde;9 zm%EHtVEmqtYFY^9(zE4OQKz;7A}J>GP#DV!ZG4vw(FXD}R{$LZ!{`_&h@)Xb=%9l8 z*_18+W>sQq5T2t-%YJUWx-U3kph#jkoP1ImOXE9vTfBJ?l zycS?v1*I6Sk=a7RCl`P-co4HCSmd85C3^984nYy8)wV90bDMg2^&G5yFtzvJpySJE ztAE96n$q?V{HKj{wNclc)0Fz1C<2ix5JVl>??jYWp~68l$6WtH{mYvnRmMRf4AD0> zHKZCX4-Y=;I7hG%fEAUc&srYqfz2_dJkk9=Z7!c{sw6#HDAlqhC4;FOzYK@Y5|Wc? zO9-V&=(tk`7z>L?@uV(Yp@!sarlM;oE8T1s>o~?+qyePRMb`{OG4a#AMSp@b6AQQ% z*)7@TnkV#)dPIu}P0Bc4aV&f79DTpi$K%b0I+QOSlAvhSZjL})rN@^}% z%MS^Fq+AG&dWjZ82cP3jeCx*`&(7F+EqD`y_r&4C4_zv;eENS{0QsNzCJf?=Iv6_4 z_$tYn4CT}sDvL}*E5Q(Qa&G_tya}}Tq~^FL^h~~h*O!W-2PV$Vs&?KetzU8?grUu0qN8UiHMfE z>VPTbB_&}tT;p|S11}x)|LHqtk@Y$-xo#8wj*pL{Y~;iz|3#-m5+n&(>ev-8K4yeK zaG>Tus(2U^dfS)(<|y7ESExuJ2a+;~g}Xsz=U|3HFORvXdXxf_=lTrag^)hk_D_9M zs(R#kXoO<>H+Z&@+o%x~Il%Y$pHN=)3 z`%)dv_3Ip=2H@?)4m|irSSWWETX}>X)71^TC_hCX;VWp|+QKIWO8~HvI;}AWHIe8% zaEp+Cy5j=E`*h+ni7Y#kbu;27VvJDX}pxdzw{ONN)O`|2RH!i zwlldM;W&yE*yzBabAUw*eSYYSG#p#T={{Hx>!wUY+6_^oLaR3>=N#erRHA22fK2e` zkTSSqMm@(Ux3l4POU9T`qWJim4W9MoY6zb(+d?Z#`z33#K)JU*+zfn_2M#x|Xv5LDCiCrHX2*Szo&-~XV-1HaE* zXXzOlPUP^reQG-@iWP{?T5U(gqE-0=RVu^TH~U{D3fF2NXgyDsS)AbK(njaab}8gX zLOE{vxuOG^%=^Dpe=_-4B`0l;4rfMQ8*D;}(ZT{P%k=|NlC@9(pK*VXCwv<;3|AUJ z$LH{72`48YiFQOBZ;1T?Wyu27GR9HQjFe;`uJ8Lz#_Gza3bXX_l&pj}j!L)^cN#*Aa>6DubF2j(3bJY3)1}14>qK2#U@v{Ph z|M48QVE+{B(=PgF-4rOSrQ*&b4&k62ClIaz$fjLZGRM3OSWVtMGIq*(JZ;=J6yjx| zH#+%loj&@l?)ox$UYc0>KuR38e@c2BLV%Q(5Xk2L=5lG$^`)nv!sczFl-M&J+^y(o ztyNvSR=jd_b(;CQPUd5kk<#%i>JRO?^P%fMk-6vdFhQT&JW2YctQeU&Mr;!-2;XjO zO!A3$!(B6@=%7r6?Zw24#fB>z3es3{&ahj(E+N_|NzNng01aJHl}Enm(lmB&*NQD0 zEtBhI)`)*Be*XMG!>G+ODq?E(+*);)#Non<0!m_j0#!nhn8X*$Vr2ojFgmC_kHJX7NeG&lfGM zi6E^Rh5sl%HPsB7IoX(O+qP}H zCcC~f&#P~>?jK>T*4g*QwfD6FWWf$e-?Hh1ixyHT#-oUk6qi7=)sl>4)mjK;mg)Yo zq(JJ8-DQe-vdbw}vmMg8$L-b`ViO`5DmZ`6?9uohQzS_qI9yWUt^qi66{KNKMffVj zX+n}1fhM#u$*#omBvJg+Ghyo^*|@W@FrilMVzQ0JhW3+d+v8IFX!1N-7|3$fp9(7a zEk;|=+amFTY8Nx+i;2S6RP`|>3k;L&`K~q+rdy~LeNLg>>>aAPl>we)$R2 z_3C^V)77C~4cuYD=ObrwmNO>uyA?@lxDDrdd$!|ROPf+|j80c!ehRFjhQ<)c5!4s2 zfYFB~q(4h!0zvPh| zub||JbZ3GPMSbFPQZ*46n4sPkZ}MRi<;+=*n+7J96@*~ifgieoqY~qX0hW%FYGA@) z>R>FN9mB@^>#07uIjMCzPgxSoJU<-WZkAx_ZEf4lYn1b6tR=bOGN!Rq+GyH1Cps@B z2$n(WVVq0l4{#aa;eGFZZ|@pN)b-r|N&aojpe=K(sx2FPQ%9PTH#BVM-QUc1Io&s# zJvbGfVK1O7s|N>-ucN`+jIvjuNnPQVpNt*cWDvj~(j~kRH>K zFvS0sAx4XC5``vy!5l#JY0W+)ABoz(LoF#Vah-@74K4bHlcni~ye8mEvEN2{_UX5>;f&)Q&Li@0n?dB_e%QMQU7L9p2z%-BH(-a)g_ZHw39!% zsn4KSM1V8$_LDEkn6Wv^ic(GR-RrP3HRq3P<@38$08=#jH@8G|57&xxPJZ}Ht|SqL z*9`B<&g`>B%k7mjhTUcn!)YA>&`>AArq5gvukX>luJf(2*K!UHTSMR1i7MbV3Dw`W z8zSbTYm0t?=74NnFKVj(t)0~060>7laB_^XU6ei`lI$Erdg(FTg4nYAKF2>GkYmmH z56I-q&g8-hA%eWrspVm|h#yHEzTs_zc>1mz4Q4HvDI%pj*?~UaM9noM8#83s^kKym zA`OjWa%?2RP=k#BuoemR1Vi4sR-kcW?RVX*h;&3Fb4V26SwLFaS7vowrmn0&Yjjpf z#nktB-FQn{kOz&#Y-Z9(IvG!Gu4Z z5RlG0ObLZY=%cXJ?$<1*YS)1^2gg)=1v&KVE1DzK=VS;k|t*f4>NVb2cmK`-1l`bBws@cNzKimy|#|d z3ZgO#ky%^OIpB+|*y;4beiNQ)x`24DF{%GD2AI}}5=ia6iQC>oK}^m7Lj~`3^hz&-h+I_CjvW*+2^7KZT_Y^^fZ@J zx2oVoZx@VSs_yKr@8>$IbCQ~mV`*7KG6__uUfFq?^46zogiN@Oox5msA*_~=3i5r@({8L)0 z1at;;WEcP{kC0H<79;fO1zvC{u0MST0aE)&=D;sBkXmgSY4MrEnnDTZ(}ztZ!(L%1 z(>NO$we#3Gkf0VFsWeyL!;#=onwvB2ri$URduZ)sf34K~5=u<+cB^gWmgr6^Qj+Nor@UfQ?H45! z9$HAqv_Iy~dW{#w>ow|rw=9`){k=ActfsatIbHx_%fs)MjgDrAua2q=eBTwQAM=^w z0!5AHxI-*~j(woi z!%>&Q##p8+e>cmmY$MM(M5X7)BLR~sj^P(yNZ-~2{X;uPn$ONFU3SJdy&UG2SBFF$ zMo5m+()&6L|LJBv(8b2f`WMplF9_nYsgoa`7zdOdUff6OSxnIfF^_uP{7SzFp9Xf9 zmUjxN+}_Q$j7s4)Ih=+9mVK{Cf7%rWZXVeUwGT&R>yOCm;I}-b8wJ~N`N2XWmE~Dz z+z!`AXx&^1TyJ+g0tcGxcH+ZA9A;0797iKFae2Gt(m@;iBxn}o=Xv*IMYM|^InoR| z(zh5^lFJlUq5>=x0B_%r9kv9f4jQ*3{7Su}iM(HNvMP9!#xC1~rLM7(Doc$b8(!Q$ zZm76yPD|Q5ZVT3*`uSBEK5030?U5v&Mcj`n?Nn-mT^jlbCa{bVKkk}B+&&U2IK5=e zC~BXhXj5GJH=aUt$1AWE3P<%OYlq!-h0!)IJ>T}1sD9-2=IsQ8G~eX0a5+Ckw?FJ% zmot^Kx5a*TGAAD+`}7&lmE~NDe9QF4EFw}FPqKSY7-277J$jxIVtIBvk!9$*#cQN> zP5oS^$hhy<@w(`F=*e0)VT}6N<#5{r@fR9bVm^_jOtu0K_}-9R??<{BTF}!U^~AV7 zqb%c}xuVTiz=H+CrQ;uu*`E|F4;T+bzXkj$&Yia>abh}p(#O192y*$--lIO;KK|L@ zMYA2xO?)6Y7<4H6>aLeCVIl-hHMtvQjijoQlO)iO&jPvGo2u9oU&gqdVT`b2+9NX(enkzVdo zOAHa;i#zFwEI~{xYCGx0M#y(h^F9_7!m{f9w31p3MyAs8NKBfwioEd6vbKg>S;lbM z3esBC<{c`$nwNd`2b6tQp+GdSz~^F6Q;nQ#-IM9CF# zbwwWbI@DgDm}5$mR!y;Cs!!``!+u4pO&{U&&81?--H7=;fbFkSuEyCJOY^Q`sQ0T7 z-YEwKS0=v$LL-3Ej7$l76z@;@LbEuJsa_w7*elp^zVuE_nt#h18<|4GBmY#Y@eu?7kJ($yn}>ui%m6L zfzi!WMoU$C&88VfNt=06DvnNP9H*Fe_~2b353~Z?dGK95r>RHR0Y!G(`z3MGB3Gmp z?Z3?QsadYeMN+TBV?Qgz3RFPOXHb)LUZFaj6P5k57NPLM2 z_L@FNOV6fLab8DuP4<(=t78fwraCE&$3+}BIX>*j>pr+4Lth^Dk{V*_9+xf;M$$Kj zYSzfC&m5n|FSZUAZW)fsTaasqUgeM0k$nSMqSJ(aAKDDrbzYOHpF3ktt@~KKgM$e= zEh=U3j`=!vf4loQN%lHFIXcM?IJ7zYV*KbyBtf|Ng<$Y)Scdl{SHjKY`qQCX-yhRe ze~K^j>M&TqJ3Gcbb$VR$!KU7`qeX1)bMH`t-F67-N}CgZsZNP<#Y%M||5+Q~cokyf zj6?Y=ls91@yVr8Sc#WY5qtU#k_`-2gjZ(fv!kA{$3?zm5rTuwqmmge^a7{oysX1mF zoQT-uAzd{AHP1GJW11qZb+iq;`6>NOIoGgcd~7Z%rr5b8J}P3Kd^9JQ(F0WN44_KB zurdxgN@A(LH7&KrQ$W9wksVS6yFNjmLQh*t$DINn@3F|b@axQM1Uuvg1&#)*_Hnd) z5svip`#oI{H%bHZ7jpQq&{jSD4q0EGx*A+_$v<}xotw)DMlRSVyAG{+b9_uHg3aq< zt@XVT`H-Z+8m{#PccmKGb4|=}VMT;5uUf5}lMy3yAlq$?0)8}lGRSLy_jLtrOPf2j z9<#0id+EvH^fX-{PFPXcWnW&~fJUITpSj!MKt1ffAJoZX{YEuuQ=@Ai_M8NwH@$yL zG<$ewBYsa{6ZVpLcRP8V6^Xh*vIpEs%fSRvLJdp=k6Q{54sF^DjQb&AaM0e*x^Mth z7AjP!F~T{99$GnF@Ak$*X))W{b1WWfcJ9@zk57w!wwa5kn3Ik)kNlPtBkw4~uKB~6 z)gp(gCSnbN`@}cTGkG+KKjw86;(A>Z@HwiWz@HtXYmYT_xp6vRCJjAgXFw(SVlE8c z7RgpFQkQ3jgvDw#q;Uw;{c)%}&exRP7r4q89?DdTX=3}=Yxc)95NPe|op_BvVXkfM zbK?4T-bi%&`!y@ib$7<=WiW!POJu9zli!N-vUv|g212m_#+MD7R=n@O+4}MhSak9D zLw&KKqcX6kTk_ebTJJhE;b-q0j+koq&v2x_vcPvkZ9TB4YOfEyS^c!{P2sUrH&tAZd^48NCESwq?Tr(tEocwf?&)uC+2v=f*Ao! z-k;>VPNun{hqJXzVEvgAn&`DB})mME!J`Bsg`wA!cev zMe~{H#!As?X`v=SIajxe+H38Gn!%yE{Gv^WrP53*fx%W0&36ud-f=~0x8XJCGK0KW zUpeZ6KXpT^H`1;umUaxt7#yzRu5yqR>i=XoTup&})b4(Jwt8+pgiB%GS&^_(Z;e{@ zAZAX;k$SBJeva0{4=q;(yh3z3p%Kp-c0=O&93ybp{H#75@oc#WX}Vb{P$%On#e67+spe#_qNLj|`jL_K!wA6CeRC3G<()J}QTO!t~~NekyeZ z^|LW)LGdg8>OcTo=u1saRt{(8=5i!)IJ>2(ISm~Jz|`_oe9=y1Q+5V3Q>aIBChMIF zvd?o%S5vt6MEE6IhK5*AH=4F2I=}S#U{~b?-qRnz&Yie;?%1pxzm$FgC)W$4C|r<} zc`&V-q|7R|X!~h_;`|ai>T?4CZw%dQj#mVtN4W&+f|$nmK~$w(7sfRH$#jPDR^$<2 zDX=bj523Pv>o0i497{+RRL&W9(ghmQLQ#X8Gpje%cYX2-<251tv2WV-^)~5a`zFTJy7b|ETm^?xTKKB1m-C@--|ioDy$7mkjjgt?zdL!d{!-DZG@Z8SNBv zil|Ir>*rRypHoX;rqhb5?IiG_*x&KhYb-^u-%YXkW<}4MVe5aeTCM&Khe6&?xdnOLT9vbGOQ)I%dB!WS2s{y zGk40kw}@`J*z>v;!JhLSks2qoneGChZm3M|-np_pVZ+_uj)3h6*F6>noHgHn`i%A? z%l-MQ&3Hl8ncaAaa5G)@d{0OPwD+tBWHp&IUB)9|_uMurdxf|8bY^H}cV_EMSyus_ z*4~bEiX5&tT(8#)K8L<}PW?*%97RtT80lAPc#pINu4Z%qy0Z;-8-h;=*b>Yy-LRi4 z&pLBM7<*MV$8uF5W2!WTnp}82r`Vfc?R@vx%nS+_-b_~D4YmayHg#{eh<1*{@Dz;} zG|_81R}Uxd!1Kma*l?fosh-Uq2V^BLf=A!gr&p18bHcC$e9vKDsD3B-IhdJa7vp2ZV{vfM z-nk{};6bDL<{xZwKhNZj^Rziw4kJ?2zUYA$kpf<>>q=(fSiw4KEbayi-RIt@IQQhS zTMxsmfJG7d6Ui5WAZw5wpM;33N~HC#JVt;6tgay!V#}mYGLqPug|6Yw4isc*(v;?z zntst-vnMvL{S=lSPN$cAYfVWLY~3aeYun&t2)y`~7G71u-iqwb5_Z@oz~+aY7?CV} zCT&hvHN+k}rmOfcVlvHW- zfVoi1SA1(8M`HWem$C0QJ04B?FBrnHw5x$4+_Tw~maFdaYIavR!LYqmMO@nek!DJV zMont#ld}RG!2?be;NpCEy4?|z88y)1C!tJ^c!sJ`fpD(kxi!MkZuuXG`9^mtqGDX2 zkYloVV3;_BUPt-~!=QN>g7=(y|IiE*TM#S^UtlzOmo03>q9$updVge~e2W=hSu!K2 zAK%1-8=_X!zz(Ux<~vI~YUGfswP<~8 zmNt4eR-s6~Afh+*J*y?3^ef*7ANE5?xxI(&_a%ZU3uyA`;FPKd0~(6edibZq21dt` zrpo<#Px29q&jD;C?r2B(mMo%$bLfFZ+_3lg&!?$o-*-9&-be58OqI{+-f_q1gS1zl zkxT{MiHRe<(d#Ckz%hO|rj2Bu*bW`GR)Z;)gZ;hKx&e!9(4;Qo_UUf{`Oku)>Mfk# zoeS{CSsqAc-E4a^E`jZQEgkGI|suSV8nIGn6_z5Zcz9DSC`%nJISUI0@D0mp9_xju_d zO+b?&0zi03%WE)Cs`Dj(eXum$nr=l^?=PPW&WMZtpI45I`>{km(Nn(1zSIUlUX_f4 zw~tQ9Wdf*Q=iY%b!E22hV7Ffw-o|s+T}~*x+>!>wJY8lq5w(#-VhoS* z5M8jjyc3QF=g-cJEqihxtDNO>;2E{me`lrOI(t-$fp*3X-3vre{uJ43qYhE-#GiBg zSqL{KiJ9zvU56qIezh~4wJeL~t*~$}fSxJkY7MBy#6xM|mcmX}9~J)^5HvnIjhQJ= zlaNQnPDn!mNfsnJo1zn~qATQT@}1sY;a4JFmNsnv`nPa#M>$mnO#nI{fM@2-(uqT6 z{z8QDZYQv4&l0Fh7Hx4P$(gw+`u4T98=;=rI$hGgAYu0F-J4Ko0q~!t}MT+On z#oK$YU)@!yudf{uS&8=O>id^jql9!xN%Qy@O)zNc8nUuXmF3% z={z&FqKPoK2qazVdenR%XbZB~lRs*daO%7XN^d@xd#oiXnwjr}rt&#~nB=TRC0XUX z4ySedn|Eb@!V$BZk}j<3(+5QwfjUc{-hA7Ze0{ORf>94)qw;ugY~?$_mV6!sOCIwA z<@>ZiUHLMq(0o_VIA=$m`m$M;`g*HylYdpU=eT4rQ6g}WS3YE6N!XV~6@BkR27R}; zy!Nns>0P4mOzpa9uz$n&ky0jb$M=HHvh3>xlWW1&mEPuv&^$R0?>iaro_#oLmpQ?d z`U4%AD~9K)Jz50)my9C5Tl%9?pC@RmNOqp@E*=Blj%UK0r^B1ZxBbHydG(=E!i3eW zmT>#$wyk#tHv?J#l6BR;*w<4GqdhR`54dNMb%qIVq(XY3AoY8>3G<8zHef`YnJ&&x zR|>=>6Ap8r4l+DF#&fP5_q%6i+ks8>8>S(~&y@A`f=fdIDY)`8=CLq}W9xC49DKhl zsHurVC$)*xd@vK?q!+E#h)`{(THKR1#lmBkaMRr)3MZL?;ePe`a>PG1kA)kZT(n%C zR6RH}k;Ja>IBH7tcRVOX*QJrdN{XkWlaT%~rU#Q#QeUf`J%=szyI_g>xKm_~2;13% z#xiOdK&Hfl-~Z08eMS>hgdWHB=kK7H30TP2#yPRdX~{gMNE#Arx0y;zj>0o;&d!(P z<;J5F2+)8%%16=i)>7R9=MESqC+G}FEfEh-bI>CgXJZt=!?oSI?9gxlPj`tzV}6>| z+4(P^1e1dn=+^)`GcjIPs#Ee$dK{p&8|bA>4Gj#FO?XT#rWtfCSn&FKwF`qWd=EHO z8TP&Gj9we9&Y;DQLeFoQS>6v?psBYu{B;ONnn%~)C+8-Plo5o46Z0o~7R(fUv<5%5 zEC@WO@GxQ0n7e`@;n~r%Ymk4Zmr?~R7E-i7am%Lh{8o=MT!F9Z-EWaqIO*|dO%j+2 zci{1y_}206Jn4cKh|&(a6LH@4`O(){W0NjR*|%I{-1Nftc46l`HX}jAcT6J`WW>oT zw+H&>0Mvv{)F^l1YR}pDu%X(n!b$gZ>UH>eZvV~u#EPK#J++jFzU%$MjtkU*Ezyb} zP8bQA%Ztg6=C+)E(iUdAqGxV9?Q1w5`H`le95y+s^t*rQCvZ*wIQ_O2w9Dq?dUr4km%nF8g^3Vpoz;dJ)*;kZZtdtjWh&$uZ&5 zK~lwLq^U(Wq$BPM219{z;4{i_7pm^SEKdH(t;Hc7!xU}efnhon0e#sd*Bf^DGu=s# z3NK3ed!o6y`4?WHI;NcszR~-8x4c8e(nEU&IhwGPX%r6Bv_sgj&V9mjGT2}XVm+xA z-O@H_9A=V*<4k-8-PD7Dp!8t*j{%rWS&WKimotJto_`|-4A z_slHu#_&>kFfwYVv ztg^7@mcWRpSuBTge`aMj*mp3Ms?DR4iWVBS=>lvXDI20z^W5M3a@)&Tt>1d5N%ohQ)fWnA0yn83NR ztMe$8A<}>MGoIj5zKq&JE+m_+Bh;T{*BJwZlV1{P{G`rOjoR9!21ll_X>fi zc9?o;A9DZ1O!9A>D5ZJ6=L_RUltoNC%+t17$30v9USn6^%q}_|2}&w+9F7nX0S_vH zR)=q;+7ZG#H62}!r>;u5iqwrUqfy2Oi>l}5^MfOBFuK8sI#>g_=pwj@zjO>jr>EzC z8M+x2=2~e2@D1g1z@jB%qb39$3DYi|kMYRItjrshK-2{>cDgD$(b>BWDq9fL$t;|Q z@f>xKGTYZ8nEvtboD#@ePn{XUtDJ`Bz~&99^~*mPmIXfx!9v$E{hpS!HD#>~ncP|Q zs3hUK^jfO}WvwaEQZ}Cz)iN+pnppZ@KEm!ck`tdazmVjbG=gxIcymT>Np-JIqXAL9 z38lM{Hw#4v?wE7fF`As~xPHJJ8jSfz*6g9zD0zo(b!)>@Uo%h~^iq-9%wB!#Y6k*~G z`YlJGZpg&O!G+c>!+_%Mem8d#_`vAu)AB`>Uj>09)Zpx5v<3)#1{q11grKmxuB898 zB|0-W6Q;xq6_a(cP#L#1646H?L?4Pp2@^w2=h13olGZ^OO%gJeA&}A+mxBHV&CqD<)#NbF06Wg9y_C;Fi0jKt~|#^{`0- z!!y;aG3Dm$^^%j4lHxAIcM)riE(7fH0jhZ81TaB-BqF%&A7G}TCe zOt$7s324^X1R^w5G+hgnulPJJisyRmx7)tSIaHQRbKwmi87I@&??oH^zP<76rsd2&;jI*V+D)L>&s%Rc}7BJMaMF;y3Z71Rx(4s?ngRp z7ZPwb{|ctERG&B03)nn%k`{*-s|&h3W~FrDL!-;*;7;YE5|+0T#zvEqWYw)7RW5+9 zMg22&UdTqLwG=|~Dw(LBXJvC|Zc(n7=Jxsh&)WV1=>o5WO+U)KZRQ^5aVruGLSIf9 ztq2{S_)@)P*+u(-5e~QM7zNtm8Q}iT#Ey)9Oc)8+MR6_=wUvyG5fRia*ILG;m_hLL ztr(T6V|i(mY3HI+i%W66e4bBbw-98w)Gngpy3^|gnZRg44mxIhjhb9*vKb~)CcGhe z)AM3#$*?$UGvQCGa-Ams0%fWM9JxPS^452(g%(ztZ70r2Rn9Ygu<^eOSt5+V9trWGS`k%~1&n3GG@mOPB9KFI_AGD^8YDqLmhI zAe}N*_GaC2s%FdQHOff?g9m5Lkr%YLA_yM4bW056U)%!zA-m|nDuD%974RTAi5H|E zJVQr^h#p$4smu~9{~w@hq2{FfOTF{cpe}y_I+|BjaNSK4pS|ID8ZvXK_3E2K{(;oD2G*#x2YFb6s{M{Jb9s?a}wdp0w- zH?8->+3#dkldY!iKshYbK!OD=j5rnpPK=Y80y;hNo0Iv3=9STZ8)5!TKo$_p%TkvjJmV8lC8V1e{3## zUSMUas0rARdEZ@TLfYRv`5-51G-HululPkg?Q%8eyh0QZFsB)%6O}MblqQd$4Qo6k)+l**VN~Ox|`ipt<^Rx7zBCX#~61HR)P64G* zJDWwzE77MSl|QXU*UvQI!v8{2x=|48g?fddE^R3%EcLjvtzkFGIf^swS1jF69jvr( z!pZk}j*wHiurJ-PYzSG*UOIB<2{uCH`)ugKBGO?N9?1J^NkIt#8>u8)ku(MU4+B$# zQ5-r=Hy17%otdEWu2qbJ^g4(#gBy^{g_y`p{F!~>^Lll$yvgNw*nTkfk4(!1qY$P6 zd2!(yV^DLo`^!m@Dnx~mXpNpha{>ggtlHGjQ5D=|$^1{A=?}Eyv~IRMoYte!$Y!n| zctFB6{37?t(T`yWxa+X`1P#Muf7Ry(Vmx+h(_>l==v@;EPPM)=4K28WGKTS9<-lK+ z%d()n$9ZyM^eyqD(#A)jnqF9tDS;B8Ms$7_K2`X$@x{i3SY7>v_*!&qK`tbAoIa1D z3+I=ATpDM)S;0_y4vdNp-JmbN9?mK@ zL+uWXci@T~s7A1dgBV!u!wQD@00D@j}UE5inXXP_DXfY!T>;`5S%De`?@x({(DN=WVw(GamX|{Bz&Vqv` zGc2sFOVK$NYNSCP5#x%SjKTbJ^k43wlfmzLx$as_^$#q9e)S*Bh^q)HvBvdZ2i14y zP?rgRGcq^4?F{<5F^vL%kwXWGcX2N;FsEl#G(5^l>_Kda%}I}J_hBdeP>}pQLcV|6 z`R1AiL`-;OQX|H3ra|PwN{feQrC=YG!VA$PkOKY{PP-NJBPJUSt0Z;Vbe zO08*13iLbl5VkNLE>zW#m~%l9-fm?mv@lAeT-lshBjnN%`!$WqnMHkSS9jLBoYb+< zqPJwa(3-{3dPAaQbg(AEo{spSCbpizVdt~GtU5&dPk8Mc{~#hQ59=eSDU@E`S7oh& zBDG!PW`fa-%EWa||9=HswnKLNtW;DJ?F_l%E<*C`HB7!ob6ez@rz2 zR&o$S1dco-N$l`ILE5xHV&Et+aL-_0iT<@)u_pOexj2CFRXP`oE)MS0!V*S#pn&`w z0hCc;iw9k;VhB-4kbtr%Pn55Nj~|&%hDyXvhu%uH4b{cAI=c@5%Da!u1-UB{fE^M+ zLZU|sTBuGsA5c|#HfTT+wx*UU{4k33T|8X2p6nR@s}rx#?0vEdEdbOXr_NdllP)Z% zWGC$EA=G`&KFsNj5y`aQYg|nLd%(ZD2=JI-O}iisg2^*?1`A?VwEBgCf)sLcdPojj2RJ}zt1Ux=FTRUt(qL4=_H-^1;rtQeR0r5O` zke~$K^VRJOg)NAf4BE9Z6NQ`*CG|UN?fGp8H?TO(in!z~MUF zg!1uGvNst%9}f90*y>`{7;157U_jacP-psH56Bo9WV-^gJRMm;8frA`sH47~Q}HYY zvlEsgRDb@(%xHa6mfHZ6oA#^TfH-aw52~PyA@E06FqQP+xqO~FRmmF~xqg%q`Thh( zBhT7RJm zIgq5|i!x2_cxJQv;Y7WH)w)9i+BX^pIy$O5l&d)Wd+HwfT5g%Y4M2`2H2PJznHVD4 z#_pcqVEFEv=GBChLyOPuHRlh_jP*24*al?pa`gs6iC?21MAWwbjgogOeu5S ze1^jUkX4M;%UwpRJgfD2f7uq*_#Y1eLRF?92fUlvA9o*Dw?=C1k}OM!qj$WD<0O~! z957967umziDlCLi-aVUc?~JH{j?)@ktwHidZ)qAr^ul(wAhCagVH5z5pMJQ&$u8Ca z060-Tky?<{43l^u6Zv&}bz? zdRDB)iEsZ!er`L~SX@fbeFEgNc!rw=i>ZXsnMJt+?gR`p!-U@$28`}fu7}QF`ev#l zUlP!En3|C2w1xXqn_+uRXb5pmS&1RK!6+-}MhwWZ{!vhP`Y#2&Fae40dT*%Ca^bsr zmA;(sr=mh24aPk=8Z1cipv5jkCzb!@WdHfv0iJ&q@phH|Jy=;pI#)L)$DC>l9K7fi zNy$(_r*iR;@a-*arP`OO9254vlnK>dBN+gEUOgIEe+EOIOK`0LQC+Pz2*>j~cHcn3 zB&nNi!(D_=36H5@l3Wv{WnQoJkETldu8Fu9yC}NEf-KuOpcgT{3ER0UL6G z2@=L(+bytltD+S)BZc~@LWjB1T#~-I`L#@=%460FZEX2o>vM3Yl2oslVXD*Y-57v zD0$Sr8jcBcRJ-|c1HhvpNz{=XUzM$I$Ng2@ky^QIy;{+wac1@MT1dMn+5|mdUC1fmZFd=6e-B9B&je;R?@hod#dq02$Ys|{|8BJ^ZVRMqeh&NMk*I(rdh>Gd-|JXTUTFKq_5T2GBSU=z*I_kYsZ z;?rXO&2}k$^(j^d3FahG1lda^SXxbv@;u8{6@=1XKA|Qg-wnF3ZAB1sA3ZAT#qM55 zzkJwyAU521h`l&aC;aXJ-7096S^9Oj-)tr`1rcV|Gn-(2(N5ESYx5I9scejho*HMw z($hD%aQ7@QQ$J0*47$RgY^77UnBGlcJN_P8iwNw@qB^Tp8#SAa1SO5byfwJc>Nq&s z_PEDwhc;hX`VVc~>;SNxWG$O&X-k12J74wt4yVfxgpjjVr-y`)%*tgptvUc3JT}8N zXu-i(nEr|I>3>p!8?bjnn%Mc)qx^sN2!fX)|3b4Jm5URrGD8&(@>#GS!$j<%@_eaL z%3+y336(;?xDxviBFc#1Piuw_sI+4Ao$p@(u1%aM3?MMsOWoVZ_% zT+Fs-%Q3+qW5Xu!wvj4g(xbGp=4u^y=rRoi$1CNlXi}xDHXY5>`*lfj*t%dw@>*RQ zUdt$&hIKlN2dANo^2_1kzt9h0-VHr~=a`V6t$mp&Sdwb~xe%KAz5_HrpzA$>Z=7S^ zCGK`|NGLCCy_kalEqm!MGd{WSMbwaHl@l2*RfHw>rY)h}*y*D75$N0bk)*a+sS|<7 zWT&@60E($OcHtW_t9u%ruTQOe+U#p(r-lDQZ64CWv?}$c)Jygu4|c}Ly@EE`#{cP5 z4Z2EL4rGFg%5_q0XErfq!(s_ng7MtE;2dv31?jSdFpA5olqSCSjy%dkP*dof4wV;$ zOciQFn_uaZI_gC@VARvVI3{v6HD@DjFn}g_*3VKCl_5lVQTaK@S$;H}3>^%(=BdY{ zW6ITFJE5I?Am|D()&1TTnzA4@epz1itku?z_gL`7$kr8QN1OGxlLY0`qv?I)IQUNU=bYXc_ z4GlCxBL7-r)S0c&69Lqc^9iHnJ+Lu+BaO{U{q}B7p~ilP_RDxyM~K6J{Cpmg!nD_| zb2ld306Q^!gxlTb)@}C0nr|&~{3hn-wrdPyp>NyaSlvsigxg&K!+lZZe?x1A z8yUy|xkh?xa}kf!3OVm*9FkgxGSuhRtYXuvmuq1{d{&n2%`@+_u%8Wy$%|ED zyfBFZHO6Y;Ge#ToinWU*(DC7%>%VzRuF>2QMOEs~$!yyfh@mbkZsT}F@_6Hv#?M#I zPZ{LfwA)E1A(!`bBA*+aLwV~0rWwgrZ}zK?ZPWG7Qt$-#Q>!BTL5&aD$A{zGMOv@V zH(?nvTKsD z@KFy~hh^5O%LFf^=*7bh6#1!}!{qFg4+Z9w;0+8~nwPLqF4*)zrr?2+LOq}csjHtk zx;RbTR;xnXUDTmTa@bf~A#D?)6 zhG@jQPS9f(3>79h7BwcUkX2;1>?us=ch8Jx8zt&1rQzv`q1)Wfvz|Mg(Vv!94u?P8 zRg|59Sa6WmR%`La!ur>=1Z#%xg%_T>IyJ9Kw6^NIW=9k0RsC`xr#>r%kpDlu0LyYr z;;rD}gwU=N#$Jyhj6W{&rhwzi8?tAUd{Z0>ywQyOYyF;6pAZ~xkE(d1xQleYalR%F zfm>k^7_RiLdVj$fygj~~d0*=2@=!r;(7Nwrd$Urw)=mANY^|{qVOGP7OHM7nZz7z% z{?yJVqW=!vpS6heRaR?oL3ue%sF<{Tr-hYvlLsszQe5_QpnY@L#7R!1O3QUdQpvt` z<}Goa8&@TW!u;!M-DeKZqR>oLHtu#8O-cpExCgXZ#c)d41SOHB2=Q>pYBx47tuqNr z&5G0Dk=qzt1IZ118TZeL8|G^+(F*xIQw7c_?R1NiDh8Qh8+b8VJbH3?TJ!If{lFfi z2Q3y{M^dheFs6ep=xmYiO`%acsXcFoO+Ac-jSLID} zh?D7MTA#5d+s^FdR@bJVv&@X^ZX`?m8-o&8-to{~&$yG^*KYjwtL|RArFPf1_zK3d zVhRa*Z?DkUj(6XTrPvEnoVumnraEMXHLxf=pZboF0f)<2cxZBwsqQ#L&mSyz)?*9} z`rFr3uPIKKQC^RZW;Y*?9WjsDK1@sNUXT*(8lRG53O>;%H863KfrZHHLgM~)+nyQi zQ{<~@XsmxIYqwxEuBXo$6eL#Z+AoRG;y`}~9^z$8g9M_E_J*>#qrnTKNLQG%yra7^ zJd?3HwAP*9cFRj5vpl(c)sW@>#en}51b867`>S88$4ZhPJELG{MJ1{i?P<9##RP?1PB@hDiFScKC97^*uwNW zDMOX-V+X4^?{%?Ds6tl)RU^IJp3Od@c^lg;I6<4)QbD$a}i9@t$vTyIhxEn5d~mn%nZmn}zbFFZs!TEEy2i6^@bCEi{XVs3|c_@~Yf zwhqX$JJaub9)-0jAEZ|1j`;=|cFSiVQ z2>F}N4;3ddv-mKN$^Q0KND}9aXVU&AP$ttSYV4HvKCF(}Ufy<2u1B!E94^gSq?%ld%^Qg+5J6$nNjJIE zA7(uZR8$uVOaE69e9a*>YYLhs(S7f3AV4jNM$2UySMsz=76f3`nbZwNfhor!H zxa>eJT`QT2MLg|~u};u}9wyYbkoJ?M>GIU3at@;)Jv7s)5)01v*|PbuPVXPW0QSlKUIC|Pg%+DbzuTfX{--hCD=ODR?9Xsog*1hs zWvqB(GspRXrdy-NL-HzLu&(A^QKXcF!jhJ#u-A-UWGrq@TH9SdIrQiL?6OP#vlH5s zje3F4$6CTj{O|r4qwOCdjE;x7p3_;DfV)T$&Z|P-Y21u#X^*U~CJgIp_Ota+8ZZnLT@E&Dv{b#<)4&FPfp38?ClL z6nCO}Z$vD&vQ7eC#{#&{;I$|zycSqy0>UGXEe5io)7%EA5|)|c6-rgcQxqGUyDE_n z>r2mU2jAbfrlR#4H*u2I2j@t=(R`GM2aBf?JkJ-5dg!s~P+L`#6`(q}+F>ctR?r;1KTcngibGDlANIKN8w1{3} z(b4|MNO5`-H^TBe4-|T4xa`a_HUDn9*7C`nXSm1tllubqZH04l`Uuk}%|DcmjTm*@ z2@@<=VLf7#(R`oBh=x7~I_rHoz=~KdYp~qdMl^Q!AZ=X@EnLhkK2AArNQAx$g~Xo)-eDkohI`Rc)X_lq z5pui{x8}rQsPLav9s)7r#|rc|@|Snp1=x)68R^&nH9j8je>6S#8?UJ;W9-e~f`d=8 zg&JQLw3;97qK$m_=}C^97jL@ueE)2*`Tj|otaF53MY#uTv5_8-UrThjpHsZt2^3*I zy;5cPK2qF30?UBcU`;@ck28_~?b*vdkH}8pWL9_UfTIseaQDaG@#}+Cn?MeK`N=IU zmwVFpE_>XD>#BF5&`AFp5?m*(jehBvcjQjD*sb?J-anr^-|_c(uxQW2f(nz;Mt)f& zW3zO!7vq#LS}fH!N45ofd}Q6lpbz*QAKeN3o>1u>w2Qy7nH(!Pk>3f7@6~I(ZvODm z?4?cZ@nszC@>Haw+h5Lce`nz>^~EWmc%H-eb{5U$dB98Ge?|Odj|J^=t8rgVZPx7y80Oh_ z*(JxzlD4l}tWPkXRf@l-30E{HL#eQ%)4bx67afn(-|%8o-&%2f!YP&we@jAC`d-P2 z0R^=hrc4ozOMg-d9)DU`Q{q$RN|cAEI-S8AUlYab@FHZ)N?7KGu~dr^UZ4OkR>48<=q=2yJOL~D&RflZuD)e;8_A9 zoJyoU4(_(jYx8cDR}i+Ew5i-LqrKCeMQQy;IOc5+0R;2_M&9#qDO; z!Q)Z(+wK1S!i1D3cn$z(pujos$GM;nr`CBkUTrFy8?z7B&BbJAyTnm36gSoVc#=BYe6mx*VI&-QlQ3A5{XzRx{yUN;vUbvm!%D*UExPkPP)X3BR(Z~f&@U^- zps_m;L#{KSn0x2gb(IZ~Z$B>Y^#$x__Qw2%0e@n0oKQQD#{ExHX0us7>?7^5FlBnp zr@>6e&9MK8AfcwX)am23PFs-JqGgK??8{a?cg~!mtm=7|1-2iZpFTGlw@Avq37RvB zev#ETQej9%Nl^{DsZ{6bzo%yQfvPC`N_q4{o?OFFrteQ4mg0x_uO@sI{pMDXt|;_# zlvL<-{4#sE&owpH5LZ$R>1K6S42-2KVH)G%9~m6;xH>^-t+(QiRNzto0=y|$L$^9FC!+J3}6XR>ld%~U1&O1lqL??=+oSg+z7~{|P{T5YUmBZUjno z+Y_y~rhZx#LU?^p>~J1k*9RsKETbq8dG#>vN4Y&L*)ISl7tyAt%4gcygXMyo^vy7y zPnWvFi~=&NItTZDK2U2O!8s3@;5rY#WV`H9(ah9dsu8tH6HGP4YmK-xKm2NET?3aY z8R8G8K?%TvA5|`kApWXE#zw9ny%7omF9S6?_BI0SuIi zj~CL}d@tpnOf6`!K51%8E`I#AjoJRJyDyuX^v~iE6tMFk$_mL4ZT|vv28%((@BSs!ZO;9^F#`EY6_Z|G4V99Bq4x3M( zhaDrgu>X{)htA7FRSmqwQf-F$?K0;Dd+ZKn-dQ{+16)07rl4Z9_7bVwnLcGE*Bt@j zr;66;Ywk3LdZqSFIV8fqhgL&>aNz^ zUUiGQ-;#ib-^m;rYdM-IU>hA)3Oe5cr!?nyYO2pK8tg~R<$k0_e56Sqj~};y`;m^V zqDD;mH-!$0^Kmx8#C^RIey%a{1;X;-h+HCP{;)7*@E?4gG#coDqk=f^TVLE!TlE-h zmM>Ny!3Ib*AUx^gzKGohYnE9bYHQzZTKh+u49NYSwHCP^eA%CFKMoR?_?;-zVjKtY zyG+=dG>$*QQTUD*Zq*g4Y2AZ~^w}aT52wg{jB=mYhth`=k*v=!`<)9->OH-z^Ik^r zMfJx(37pGvMZj)=lA|=#K-Mj>gVCc-himT=QA5eM@7!?&LgDv{-+i(*ProoX??b&D zlxbZiigaXeWGv{Slru?vX|mg%I5!+N>v9(*bmu7;9^HI7 zslTQD5VuH#^b!gVtQ+DK{Fo<@0iK!%byzbiLwR9S6@n-gtN~%z03}^)zo)F zm`nP4@u=JKcg2cfq(P4|^#U>}*V>!E?;y?Vk{a3z*vo+UWuq(=pl>IJ}@D^37TL)3{LXR`9KV& zSvX&y4@BrbXTcY0188&*kgq357zdR{nAoQH#5nLA+AR}j&^f9jcrPDP_-DM`ADk0L z9~R&D--%xR*BeLN4SvktD6GD5bPk@0T+<@ypg24RDqVVrIHf(Wl>yI! z0|x9#1=kQ|1s{n!Cl}UhMy?mKhVNgtv#*3=8{JS;@B9F0|({3geGUDfWSf`FV;o*B{@N1YeAyYV*I zC=+UuXH`*xJe>9f*HwIiOdqSS^gQIKQtf=mpu<(n(4^rRe5*wFDS!nlUS;}oohwJY zd0Kxao>_|hh77q?!864gPiiFONH=6I%=jvI%RE*V#}5wsVmbu{!l)_Qe73@%d3$*e ze37UK!%Qcbd_-+g+zNIxD*||@VRJ~i(MC2});HG0KC#r9w>*D!iz3UFnKZk+^i)^I z1kH>r?fS!eZ`E8nW?dOix~)QVF@0Z3kRYLV%=6n3>L1##`z1t#cY=|`%>s|0R^e#> zlLN1JmnOCKM>gU2F`ny6wDyRAm!Z0)*L~9Mc_n|I^MLXD+AE5xJ!?XPB($~Oo(9Kl zl*!Wg);;1EP~H2t`?gmu^^Nz}HoF7P?qTW8!iDR2+sRsR0oWwip0v=|wtlMB4k2BU&5t>56+3yXAQTvQXjhS~g17rXUvT1Xmo zhhyS)pGit;$>Er5ghF-$G$W3darB5MT>Xu$~6HjJolLo?VVQoYshfLFhK{i5wWL z)J%cIqhhw0aBYD0(=TGa?h0%5^SsRYffg2Ap+r{1?MdBT0+C#Z006H6Fg24U1bw?F z?(?;s=X>8|#|zbqoqLwXL91|UHtEZN&*SWL*~1xW05|rxRIQ6{G+#ocfP0_EzVyz% z*gq|qN&(&b;v$!kFC(pQAv?s8!%KvobCYfH3nFC^&h8fzA}_YGBDWvj1_OKuJJ%6u z9a{p=pAxmM;?D?yuUc<3;QkEtjefLAv*d{WB)&Wz=LR=B&Ja%4GgR%VupjFm3SrTD25QCRu?_Aw5QL?$jKMDs5Og@ zI~rMv=r;22c<36w-kJ!>w48H*)+J@>nWf}|MeeL~vsPF$^_I^1k@_qHd&?-ruVOaA zW_8Cux%%~QULXQVK}cn)nRLuD7!HW!eamVqHtFM^pI5Iz9UROr{Ve`*S{+x3mhD&h z(y^*l3jm^Fh5k@%=noZ|kqYVD!vNZOi90+&Vm|Irbgk05M;`W*y6jg*7XQo0$!kp^ zHWtrrqluqS1tWUh%d=qBRSKQq0+r4X6)))TFtl!4k@lGRft-^HhOL)=}D4eBHJt_qEC@M;~G`*l&yb&D@JH_%lZ?t~Ng2$eMIIzU8u;ToLsor`fxynz?t?uqR|e6WA8O`I*Ok0C<_f|JpjhTR0&GZ& zJi3j(-bWj8JS}(vGrUDi;H8<6WjvKF;-KcPKHNerD{RXh*7E1cEU9!_UqSv$MuK1{ z&ZlQz(dZz@jn^oC0LO)SKYE{oCgAU8|e$8i%m9W!Nm0-x)+;e$H2Bhu~AT%C0k zytKVF6-f*3SxzUX<3|-M>%8aM9YgmFSjkR4)%H|7x%Q^mpoQP_D?AwPcWX~hL<*P< zKe=FWG&d(nG71gS_6OXM?Cv`ibI6@?xEioW^(1_9xE67Cd(7dJRcwGv)F3OU4vo(z zR_}1+KuMl8sg*{y0d#2cfnBx%C3i{v_EL88gOa(FBG#WS;bvb^pC84#qv<8ERrVY@ zH)!B}OF&r3iI*eeHVyQN>0r0|3ZU*jHx;ubhj#Cpq-lO@uXRe%Uv$0XXqx- z-SeUT&nBBFGpAJrLPH{Ks?3U7W)zGy7}%@W;Y4p((Ivq83;B5eCa&lPHP{~AuySy? zW_lAKrag$lN|IZto}4P^jpDRZ7h#M3&T@Ns+%_p?iKpE*Q^8gHKdbMscY zf``DE@5M2O?ZMW1vq#H1rf!AvLhV(R+JQst$RP3DgNLbMNifZNk;>zcZ>F&2iL_~X z-~UWs+)p6+wr9=^21bh_RytqT&4xE6NUlavP+GEAgJGX|8&Ea)~g zh-?i%g1LjIWK!_qe5fO+8`!j^`WK9qZ!)aC?_9Bd;c&QpK`Obay?gIV8xVlM3tT57jWRjv1Ie|Z37!` z)A+V|jM?w;muCHrNg>-$wEYh$rzChKceAji?cvoGbo6WC(<9?q9K)*q4Air`LEQdr zZI)V2W9)c`dQ|mnKv7(!1kJ%Pa%kZ04)ISxEwH_kLRBMJSYP1>we<=Mt{+^r|<8*^8fjl zrdvT%PJ4XsNS(QYUCxGH_i<*QJ41*Ag<7f%zkR)^o=RxplyUiQ)sOWmw8WtqkH#N1 zDo}bqo3oSWb{F${LbW(#I)h}PB-A-sQs=)#lu4?nd`=BXmATS9UzPJX_L>#QH=&jY zwCqlY!VJxo;Z0*)VEhxcyvOgxuNWM(KvY6NOlrh|!qx6o=mQ73^qw}J`Fbx?Z?}=w zhZlU(G~;Fs6V`hf8Tz|2VKGDF)YjlwKX=>KXh-x8^ZGM*GDXSi zR#Iz(fdGWRcIIn;P^!xhtjZ5W&33sStaIwCo5iLYv{7WvFS&ham@wpbP^Vi49PhU8 ze@R&a9Amv`Qm;lcrzgFKves3enmNCjx~3mKt{NlkjHw=x$|f7Q^TepPqpH)P#6Y3s zau$Gch~3CXLT)on-42_6NdHXc)em=#&hoeWZ;2V zffDLOhjMcD26@H{N$zUPo9Bw?#3`j zjYnE}02D7b(OW@LokPEVU0X)C0g0lm04~MFMYJrJYpaRVyU?ob2+fd1(UqAK*_LyZ z0f~juoQ>9BjmZvT?5zaj+14f_iRV-@bSr%sGEW%fCsMa_v=0^a&mH<>Ej$MjFnc= zQo%<@*6x2+o9EL%{S(dlU_5H-bBNJsNGn>{@r&$$72#h`Yp;#TiI;oY#TV92qa zY2cyIG_v*|`Z#E7;$f2`o0)hAa*%J%WPCj$+%E~}Ek4@kk=b=PG07F2*%?04H_Db$ zqTr~cYpum*wc>H=fK&u@k;LnO38v4aHCV2hQmfwcPQ_39$7|14H+O{}3s`I7Ci`Ak zjRn<=T+=q(gb&B8lm}r)CvK;}TRT>LixS%!)=n~$LiE^e*DVY84lumna^UlMxG<@o z2ZwndlqNBK8xr3OQB+dlvD4F2KAjD)i6iIJ4usCV4!-(rU)`U-zn!11wI&<1x>wv5 z{+n6>-I?b&_3RFN1tms{U1WmaaK#jZtMX3}+CO(ReeMqU>13KzsUZsY!JREzQ`^RD zdcMxU2XQ6E^+nEp^AK$HP^2kmT22tah7(D^>gGH)NGK1anqTa8E~nRr)3*PGYzaV! z^-`>=Mh9kWkU@-Rc_-Y(9`~wO>DEpjU_&Fbwm$5+5>w<-7r~}~Ubf-Zp~ip5z}85o zUWpo(3JvQJl38LDdwIgnQ6~y{0wHyUFj#G{lKwjK42v=7z2>i7WIyQpK~eTTvVYL1R{IIA6XBCq)pP&||F$V>xp-y4mcN&JzqGPVpNWisDIW zm ztC}FSlHajf^G=Mwyf??c=~tU$hsi4c=k=IIEQY3G2Yn*qy5J4ekicd*`l3ohA*R;H zeSz?j8?-vR$sBbQwJh?O)9GsVl}3$p-q~4Dooz8t6$FVK@wn{<9-^CsACs$dvW^>V zj-V}3R^@i5dXxSuxR^nW-M^5Hb!|zmJ9@x_@ks&EKLNYx!{=c~slcKiL7+TbAz?zB znDA7Yt#1>I$@PunF^Q@2G@_Ock0#V-l0=Si3EuJ8ECfh(dKm>AtPfk%ka9aKu#@y# z_|cYPzG7`;zl)q7LbUro4d9Jn&V=wsWp8ZmJA)BcFFF{~0d&lr4M6~+diTqA+FALu z$-C%6ZKtq+Ltc0;-5sE)pucr}q1{vnsV<`{R>gisNUE5i;y z-DQcSc*$-AKuf}5t;G26b-$oI`Lo8u%Ht8=+??<&BvGq$Cm2!EGBl4+!|I73<2OtN zHcWa_Af1qOr9imAqI6Mp>fQ_Eo+^NHOn#te7WP+!Zt5~MB0Gu-42>?pbKAlqxl^p$ zRs}Il{{D}2nN@}?F{DQdjGM(^Fsr|ioV;{kK_FG(#VTFB{Y8chq+EUjxhqQUJoGYp z3jwJDx{+(~C6q!jIO8Bgilf`Wc6`XEOX9%@5hbu7=u$LnonLGwTDFSEYHDuQ z3slx2b?X+Kqs5q9SUu6=Jl08WWn!XS93m4?+0X3dA!qI1rc5(*Mv)ceo^8^hDoz?F z0b6@Ezk;#Mn*BSZCUwDFM!2vea;CG+>Tkb zF-YiIlSmG4%z8n$OT3y9JVjsxXY5L_UFyl?pVB+S7u1zcH9Zr zioDN!l<)e}L3W7Dc8&7!%PjUNfOca3kstP7J8CohQ>~Fv zj;NQYTV<7lGY>8F=F-o|l?iIxm55SKabmDfLduE9VP_P)mN#|Pbcs#X^QbT_GWYo|w0;Az;@hF6$rOU1i``E1-@-$`P!U(t;vlmF`*bNfI(4WqN67c#XMI;IBuoJc zBO0o*dpFYH@`O#b8&-^Y8E3C&xqLx>#*`}Sl+RDPTQZk+1@#e0Mcs5h?n`+)ITO16 zc-bCl@$PiX79SD6CdG{ks0dY19ua3M%h~U#6=`y-X+}e(_9nE*L$4Oa;7Yxssg>}8 zoD=CPlR(%}5@_&=YOA|B<^P;vnf<^j2@yQ}5ib0i zCJ(OjnXKKuqY`U+3nBK4U47d{6#vF^ap>N9a0HJjmZ5mnp;w_pm5&lT_cQy0@D`Eg%RQNb*#fqUg5#Tu4D&UFrYW-l_Y>#nxCaBvF-qw&spFq5g$ zf5~N$Iy~lIjF}vI>2zg^*Tl{TXz{rxv9N3!l9^?YFHkixSI)&P|LH=8D`9Wx$AHL@ z31sI`N2kP2yYx&Rrjid*Ae>Ln*CtM>CCOPx@A@@A(?HF(cYB1FC{(1lP*P`olLM$t%j)kv7@u?qQ;ZdUnN9g)JDk2>q4Ez#)GeX7+`8YlCjf z97|qmC9$L>%JIamE45@qo-|(zjVqzCUdr}GIlmWdQc##^4*mndWMg0>*-xp|BU{dtt;pz=`5^AJE;NW>!lnjdX2LVLP*VUPP@mc34=O!* zpILmp-(*-i&xWs@ZVlb}D7fyOOVhH-Iwzw$%-9{;!mM95fDP<+Mp7^3;x=qZOf!#< z2DnvE6&5Rs>0JPkwFN&n~{lLv_t9ZSyPtp=@{)BH7A) z8UM83X90CxlvbYR)%0;4z>9P8(=;u*t>@@HX8ODyw^^PfH zQevlysuvs$b{8JldS?x|l>TDC#zLIDp6TNB`Z%Jh!eBW2C1(VJXn`u(Wd?D^B(tyw zFPhC*2eE>yhb6&Hc%)8xEGYQbLruXA z1=Z{=;G|P&ukF?N8|Ez0wtwyOpg@eRWNsm5Y_xT_O0_jrg(V8Kh3CpheU8Gw@>p0J z!6A%s3AgJ?Jrr&6>nWGU@(havpR%)Du|Ae2b>a_tv|>*~3jvO3&HSn{GY{&yA7!Cw z6krVAM=8KeO+mh2J#Hu1Sg2FbZ?}(Ou{{1{U5#(x-7(-nzk5x^iVpq8N;$`G!&FvI z3zBc-s{lqdR?M6kM|IZ_Q2R z)LnyX8L*@;nr2e(=WdhdLpNW~@!aWBO_u4hihJ42F_%qRO1G(CnChjXZHeN8$^|69Ox%-G~fw2R*ZI9k}c9l^Y zsswEX`9y-Q@%@#t06F>bv0ArlEH5to0Y}oEIB^>7(+8{30?xYVOg-W*kdQB~5g&3+ zv7H}=4>z_7bUBg8Q1{MtxhFv@>+slq#n^nLHiUO7#WQK{(hg`|(I!1!J6*wO2UI*6aoqI`o#S166PLcnj(>>z` zCud(GIWROZOf%VAipQzfqAqF)eC)?Y>_%)RL*?qFoxO3xGG0zS9%Du2IHP6BKN%bi zo9b^gM*n7xvNBtU+rz@T-?EQb8}1rcp0hlQBi`6eS8zcubnm=}RQ z|8dD?EUr`@ia&;{fgIDWa^8X)(~y8z>BMt&?2sQB#RN~|YZ$)`*NNr8c)hIT3t7%j zkBe7`VDby7`uWAiXmSX%yyqx#c*tLnllL{h#0Y~S>*pC8l2fwMKEaDQKN8_3SgR0r z{;~UF>)KMpSU@?3wTDasn>Y+ z+w|U=*gFL7kJhf^s%$^Fq0e|N$#yBlMfr_=kd&_v3JtsXR9`yg=fpOofd|Ecp-`yY zclXGm(+#Ib^!qb`54Sp%OR7PJkrd%~H4B|tgpCSYeWR!C=e!o)No71*PSN3(*fl?- zido7#V~9^8zx7ODE913D-`Ct%PwwuQEveL8;=}?DyV{Y*qV*Ss0`aSBXS)pphy{v> zqnd`|!qKil&WR{`m8Rm|J$k%76yc~&Kxa%XEvBwQ?UlZ{2BOO8n*;DHR|+_%5gx!? z2xL|7c7(;#F)?REM0L~94~p1o4=UryES-_$5zy}c( zdo&2`=4+_cr(-2k2R}5@fXNwaXc}HkVAhYiN74~S=Y@?oE)Smdej4FBjQJV~$5*$- zb)!pAJ@WsM>ouid zSTR+~kicEmiy5rtaH3%?>a%x*%U#XR`^`*rJqA*KC4xmJ$C?ZTy3ISHyl^ewx=M;D zWMU{?m0un$#D#gRRv@?Ia4MzEb5i@@hi!l>CkY{2psHJuGGviLpOx^4;A;_h8Z1@D zk$}!Y_)?>r!?&(GFF{^Ng20m>As_Xg|3lohSg6ylX5m^}A4IxL@ViHaU zkB1EVdG^!%c=I8veA5(wIEous*(*^HKM@-=WR>nKQhIgj)@o)3z^LMZCWD6rcZ43H_a!TD*N1uwdC219HY?-4pB)mO-sEw%8FTH0KB9$Uk z5Z41W8WCT&tbLNRU{0W3DLb196!xraWP2pGO|&&VA}!(a__KAB&*6_laes$Asrc~3 z(R6E`b4&B)*b=O~g+hJVgaq~LE{Vipj8(sLKn$~;vCrA`gR##D;E?#hNaUK%|7^iO za&qnDVpq!-m-MAu@E9h>EfXCf{w&@`uSm`DB$}^?Od^i(+CT0_{J}GBU9zw zCXr;vfbP1PS2@Bii_!6>tGomC?Nsu6zndN334e-!yCaWld#Z0mR~u;WLQ1bg*%y1U z?IDglQ#;&3X9aDCkJy`!s~PX$(bG>X7opF>T{DjL`DO+R)sIgGrz26hI*&2g+X4QA zK73b+A_q+(x7PmK?{6^vzW1YEqXO&Q_iaz9cEGU!W3Q8nCetW<1TM$cvqo1i*#%~+ zfVKL^JDP^4e_=&GlR9sUy2+Qr!4ST&H(re&BH?{mWFsR2;%#>J7+DCATU_q!H6bEQsx#uN=7Ag>vs zEsUE)4UF#%9bDn%*Vu#oc4*HzRkNl@Gve;q?-#gIOn8))+_7J7%=G7p2~rZ^UStJJ0skp`e(#nT(P`gu{7V#>rsti{o^k3GTL5%pWrl&Q`Qm~E-h&Tp)d!4u1;;}0X_ zoTWjg(kn7K@Kw529tE9BoSFA3@8}X#h`~}*9`MU6EjSU;7*@p$?#+4ze|YVNK)&%_ zeD3eQIW{3b}^(6}NuZ7m89=;pKa8+r4M&P`?56}>-n=Ta+} zk+7lAa2c+;ptoChA=-oD4aznpd+G#OpvtlzCU?g}`hb;Gcd zo3y=rA;Yix#f2Q9RRQ|C>&W)xVQRO5FNQLa<#83>`AFSpvGP!gabnr!s_2&iAhP2$ zVSN$9cI>7PESy~Bd5_q5zHz+J3-G3q(`pa#@O#N@GcWQRenH7D)^lu6Bo=V5HXDp# z*ylp}o5u7NDa+e#{tAi2JG2A0-4*Lml@}NwLI~6yw4qBf)EHP2u^VzUU_n!7b~&jb zzn197eL;$4TnQ(A4`;5_Q?V^jA`m$%C)@m}F*}f9V!0{I{4Z?xtYh+8@%~Md?iK_n zEYbVFjO)@k)UVOQ(#SY7PVv1vENY6Z-8r+W*!W8qT|usJO&>w`oUb-wdQK}gk zP+01)Q&gz-_TI(K$@?C;NF4tB_X)BubXy;B&LvjbzjUB)D|n1Jy$GO?xtiYwI)p0j zVZsLJKC^c;gHzG=C{MJ{kPfEs5ab8xOH!>`&lI4s* zR1Z|WoHgIZHr7Fh?LM@*HkZ9)(@bhj8U9VrZrr>U*n=#YezOp5ISD#Lq^JlnKW1KgEstsWxq9VY`bjS0RNc3|L}Ofth!h}u-kL(gaT%qfhW*b z@QD8NMp1715Aq(p{I|3O^O-e&!FNoS(0FZNb*C5?nyjeF;=>OyUjhrt>e=4;x+P;Z zHX=DS4jh{>dsug_ry6cEjkGenc7|m#5qS6@2yspv8d$_oNLVK#xE1&KEaTJ&d8wc= zlflCJFC1yv)@)@BCN+-Q$JS!@7f=e5vCAU_X0!rndDv?t59B6zp{XXtM`(ieYR1j& zQS;<6JDV+)QPy-Jyyx-vCpHbT4g7o<4kOP%^le|=Yea?37j^MH0=rZLPFybAULg)>_W>q#no})imNM7_f6(yO}e4 zGh7Tw#%DrpvR9x4mZ;&cip8~Er7$O73Xv9BP3c)JKjWhDG=&C1}mhPc7E7lTUy)=dAMMDRLR~v3)P~e*sToI+D8TYcKmd&A!z(f zRna5Q!0vWqAb`l>#-9a-6xt)3;c0mlbjz?J@c7VaWXMyxm^KP{VIiX554?cL+NUVA znplr7hrBu9Cn|DE5wg+$BgGEHzPs3uGjL97jp}^s8FP2vQ*u6)h)f9AeOS&pB?@&; z#IInp335v>JR}hqo9d@)-5+|T}B zEkN&J8juVHHLgZ)n*hZST#L{m|Aw z`Fg{0Z7cGEujq=MknXs-C*no8@fd|TpW0(%R^1rD(|IxDaT(lsW7u(xhBC8aG@$fi z)O1z2;2)x7eizKkv842mn#uCq)x_L={YbA{mT;QkM}1Cv8$Q&C=Yza*diAgtb!xf7 zX0yiFh>UZLm=D8)zkodz(l_GJlwqoX4x)IPl992n)ZxXnSo>ZMts^bR z;PYp^$wA}8TW>tf(2SABspX%ed(yg{7As@Rc-6?vu%q0+GHJ)gbO=PE24;}OqfvLr z)NFgmN+&o!D=;~&@)|Dz|2%yD4fd)n^Z(-nC-~eH?CbFHvRg@4JqAKMANpCYM!I-somVzyn^0py3d{9!AIWV2*Yel#f-xC@m6bch!q^PgW%+2C!*t)I6 zi@6A#es$N{IafxT;dk}8F^UnxB<02g$&0`vh>4?11@3pD&45=m!yoyO51ed6+vAi> z=@}Y3EamGhDHf>mbq4T)WjB!VP8=(NqU$T6Kz83FIV1jq6aDMD!-G08wTuAj>ytjD zTTA^iLR$B84A5uICKr}n2m&FL6$Rmerd(+4 zSCr{?DOC>nDr(Tp1dSCbXTH(Fx5?~NPxOs2XVY##Ik;*p@HPl~6C}(VZDvkdIE7{T zLX&+mys)$#sh+X8x4NHoKURifVp5NmFHfrDk4K9$%WVU>(Fx$)zR!O!L5E3c_e56s zrBIxY&M_PInGiXGmD4&>r-I9CO%l7XS{)Aa37Vc%XmUAw8dK~$$4FSp(ul%eWk_te z@6*SB?gIENGb$u;Ta}E-ibW(aQ98XiXv+9P-HVqoGBBfaOxBtK(?lSFCapf&ZX;DO zc)+(niZ7)|t7OJ$|I!&{)i;=F)PsGeg)ynf>Lx}F@z24~tV=Po9}*0aM^(Y@q{>x# z{Ya~mfOOaALHkWF!+z|90?FEi?ZCM)3fw%-So1Dw}Nf zmjPSUH87$Tg+tJFdv;v>)UiMp}ouC73X$`%9Z=6?36+7O?-p z;He2EbO_o-8VDuBPBX0-uKBBC`5w9SuOf&QnK6MaHmr%N91Ayg_xAz#RSgVQN59Rh zbXlJG>tDhv0wEo491{-YA%3DQy9Newz3`N6vQ)>HN%A1OW)eid1&+Nk!9ETQU7L3{ zE0j`=_$MSDrPDqNH8T5Uitge6y5?$R^h*_=JD76gsWCr7)h#e^U4yNY^KhDNNvd4Q zIUG0n7Q-Q4(V85z5T!dla3HQL8megmr*;1>gu?rZo`lq3C9e>Wr7SM#hQLWgClu=a zeDXm4z^{8}l`GY-W`Q1LnUmcXBcEdv>Zg8~+k27mW)Jp9F@pcSOmpbYe6CY1$!UDE}NV8R{P7S0zwJUXeO~V#qNIpRB!$vr-bp< zVsIWSx$raE%6ORd+_sf5u7;^*H{R72hG_LrSL&zg@OIPdZb9vcERGk$d6yFaXekGS zQ%C+_ki00cQUF=7ZbpYHAV&PpHF)1rrpZV>!gcy^iD1JYL)1i`1hgx8xKWi*Y84-y zreA|}(DIKAmnmtRP)UhJ>1Avsth=Gc$iBH< z)VX;qiAir_iQnUVeO%~%wU?*k5@rA3U~;r{LfnJ!ha6?M;w45)N?!j zod(R_6bw6G;b9sd38&D*I`v{hsu3ex=G8Fdp3BE0;T#>w2(YWp|+(KRjJ^Z(E-$Rc1Pc z2GVWmP@Q_dhro*MZPQxwF@ylysLrp!y+t-|s(%TW<=EV2x0CsXb!hTM@ciY!>Ev%r z0*1(F)AfZ?fFqKD3EnmXE*jj*Aus3@{bzkf^mdzXHCA-)k^EN5DZrs>^k@p0|R#BY%+mM0h2HT;%8^4)Ql= zcSmtN`eM%eBj((L-J+%m{M)hfug9sG47ZH#)+C|oBeSOqY|{9Y#Jzv?WVDHdq@e2E-8=za%kIeHf6BjiNAP6L99_Gy+NskQMAGC33T zyZ>5^Nuv;%9()FLDRhw+U%&hP?%84_efYJXuIXYp^spCq(H^9ZX8b^YZqF{Pg*^_|#P zG0QV9dt0oVu7o)@vN3|hqSIL%3+R0)8e{s2Ea#Iqi$VY~Ik3P6jF<}a8fmtTdE}Ty z)^=dvtzO&F^yfB=Z>3-Xvz&zwOe+w{0s4CVMo%%{EJ(|dc(ri@sV|cfk71MUbF>ls zO>rDuUyEQ&c7%V&?&?dK?JuIkBC(35v$P8 zbGe!<7jQA08FYk}SFSoZw+LEq8*?P1vN0^C+tH_g#2y~!^XKJ?9!AoZ&osl4Yl@hZ z&jK9J#DH(POt};Y$aZfX*4eV_$NX4OV87Z_vHI}4SYB^&VOxCgKzUw2`ac2_f!N|N{_^+3`6nOcZs z5CJRgH@0~KM0y4YZ!+`by6>P@EW4_yapq5$e^);^4lf;_vEPod!E9fKE<#_K4bSzS zqFLswF7@lX?UX=u&ePBBzngNe%HswnW^8`*PVg5@RyNJDUMU2TRuK9QmVV?>s+}-J z6-LYrUhVoG4M9cA+cf#R^$xz!ixjZ02?wz9d|uByhe&vGCi$H@*gkNKZ%TT4-qL~o z(&ddO9l5+7{<~oEy}3#RU}6Bz`t%X-eodLa?m$mFFQB}UbOUDL| zPXcSz3)6tx?D*^bI3go+i>2+?*eV3G$BuXva!|F(z`kn z6@(Q6&&?yEQ=_r(yb|c5crz}cNgXdu3u7%Bhf0R)Gtj2ZS+~pc$fMO zED@y-EeLNP-yP(?fs~c9^3F>N`U-ot!?S$2>%d69uG*$)=)v#jy#?d%deDJQm| zx9qU901OkzB6=1y|BUc|n25$|4UG014KP|X>o#Acor%6qq^1;2_6kE{WefMq*9XhE z?l*VJwhYKLGOzK%=L+`dU!S=)eo@RMPyr;OIk@jLhNTS&{%e7**Qwfsx?>Y` zM?X20<8DlG(zNRBn*6x^rd}7phj(_B3=7h`;I%zGfn9b^>}D<=`*loG2S+GV_jgz2 zn+o%>k!rCQ#DthWz90DASCTUa`-?7yv5Z;iD*RmzvtsXp?$A|9t|#&HXO3JUNk7~s z<2B>#vxf(}Z7Ji)U(#li1*3PnA4W$~K_+dWUPt@Yvm4L%M8V1W;`)<8<5NO?6|daw z&fcP6)+oQO#IW9OC1DT|6LZ!uYvA^}MR}oIp_y;1@GFO{fuWVJL1W5PA%~U%cxqx4 zRB0(#SC!Dk>9x@HPQGXOfkW9GsUVnoKu*I)Ht5+j!i+V{BA5tI9HfK(hKvf**Jz#R=l--c zuAtzw{S5{78N>*bz7r*@NTz&?Z*Yvlg`@OAJ0LW4KWn|!ze_qb^vgR<_$RV?tQ{O| z)9_H>i8QFBE<11c*I~g{4iEhykG$AA4DwCx1d;PW;o!`g?VE2>^Q9!lQ%7OpD)G7Q zbP6QC9LO%{Re^qAGbYL&u^w2dkJ7O7TZ;pl~$V zU3tbi@22osW!U?KhQiQ^Y`@r({CKn?c#mVA^Qk&`bmI%pPvr3Dp?*l@ZpX1?TMB+t zs{+9ohQv7%g_Ms+8fiQ@gM-3HOd+E-+`J5;tkYi^ErJ`J8a#xDipevMn5feNG>!@1 zn~1Ye2bsK2zH+WY!^B*`!nPi2GpAHaeuO3WuiP2DNqe4t3IoH2sv5_obpkB7_Uq_& z9a1r|OHRNUTnzb=%ar`~E^z2UqK(>-ycpKXTkc<#rxR;)NkU&ISbGh^{QeFY7WtV% zGWF9M;e_k9*Hcsd#EQap7utQ@f*;cgen0ZEx&|9pjdhkgj5_ZY(84FLGp|88P}O*W z(*bmM$YF}20%508m~khcHwq%fH_Ns5P38Xe%k%=-?A*pg3W~drbvY|%d<=XB18$^{ z6F32b4%T(OD!;67g22d5-TN0+^G7p3>3I+eZQ0W-XcQ)dSf_;y%o(*3k*gR=RTH^Y z&3p+)rie>UCv7B`xPBsy1~xxj<}P;K&ZIc~=mkG2P#p9W>*gF-ew>R*=A_*J82p;H z9zJ0(p*O$g8wi;2zS7POV?I)q23iAi9~7pLJRt=!;4#GhScZ+cP@f3NQL5XXA0otq zBA2{FCE)$SuqGZ&$S~Zci@4KtlCjvue?QG!WLYL%@j>IeEC-)lrWp#QX1mB#&3gjt z9@0+r``MbMkZiy?FZe{+mcICy8sen!VpApt{hg2^{TU=pTVG%~7pV|5nC{p36F$Q; zjp4f`bDhT?24)<8!Fg-Xx-Gb{P=mzHF!=!4fvG?KUr${5m9PrfFJ%3-f#h)fZzlpT zZYaP#9k%@#^t1+!CEyG4un_clZk5;A`;TfyRq@6m07+4hLcm2aYOC9W{)E zSkMWj#wyo#Q%K_FJOv%NoU+?H(HQVL`;;VbC&MW4OZ^39K^78hb2LsoP%IKViigHL zfg}H%0t$1KpIlqEcJteWvttz3zUwm(f02ApLFj-0jjJqXOn+Fv#n7t7$K@ZLmoV;R zgJKNRXx#?i=QR&J1Kt&dg&`qi-g1nl{jntBx-g*Btqkp+500G^9~0^;ho2+2ye%4V zPH_3_dwR?Qo8G|X_=N;wOUQ-iv5aWyad1>(q~sP$!4Le=c~U04)KNLYureF#8Vs5L znG3Iji+Z8TYxFH%9nOqX=Chr)>Ba_9Hyf(%&J;e@hjuWwl9*T@yI|<4hA$e9^MOosm%GZA5-;EsPAG07x zmtk8xc9s!kQ$mz;GEL*|5P`PtINQ7#wOq)@K+y?MlJmLj($ygHXNf9eZNJ7 z{pTi^;j!$w(iY&YP5SG2(@F))Jany0FXE|$^=nu}K_u1;7GIJO*>Nk|S8u&F<8L>27F3JG_J5gADpv&h?JFbxsMI zTIot$Y>5Fd;(T-T3ETScj`}uZxcb+Bmecc!!Z#P@y^Z{2SdP)>y2T$8iPi6Fg^$vx ziN+v!E7`~%7##Le!I~u6PjMhSpI{UM=iOk65Cf;m8;WVLpv+Ku@BW^^oHE{u-g*rf z$+2pFOV{E(rM<^Z&d8)tUsyut2h9f3p{qE?<^63eL)5^Um94UmHzco1fjH=c&$P_2 z!sL;?t$FD74DbF<{Ytg;2Adt?LNM`q2U ztNHKH#DWWPYxD9-O2Rd5h_Yy{D%HajSn09gHZ(c2G&pVh)TYr2);I<{t}J) zC5k#9l;cok3W+jwecw~YP@5cOq)}fw9%;Pvwc)XB^zu)aUbZajG?(ML+C284Q#`%p zx8^$01Nqr_XLa^6D|GGTk1}m=qHkOYBDL7}d4TzPDzJ6f@7+8(@R!tWo^3~BxyJwzj(2n({mZ((QSxLQJ7@`JO7(n8FA_{*O0Wq#iEer#s;Zc0=D zuw4EJ&Kr^0cI?*_O}R@s|F@t&UF*-|aKb07zMOK*#kd7FZ*oF<_?mKRgD|vpo!rd|nzImuiX< zW93C>N2r{@X2Ktib+o9?WQv~w6}?r#;v`Ms%i5Vp_vL7Qce?H>;j^YlXn^7dT^jrl z7CIoNa7$`v+d!a*E2Eg^(C9<^I;~_J`(NWO;Ft^F{e|(P8%U`wE&N^{?1DGQp|10* zLXKLvI9C}f;N`=bLrbv#YZuIA%{o0$n}x8T=rA2R7mvdJZWw~IV?vE|U>txn`lywW z%e|ikFKqD6f5Y`_CR`qhBwLxK0oWt!}Ly0EFOR*b|%b8dPHif$re z>!ddEh3szW(Z&pX#oT}RB!G#hbj5EDN{te(q^f5ag4W0Izw+8id1JAR$K&R3Tu>-! z;~n6HU=BLPIuRag405ci;wJ8*{f&oKR~suWh+hF~Lw@5+Z~`JbCTd+ALv02s8)X*x zE=Q7*=@&38F?Dc!yk9z)5OJ*jb4}(Je5U)bd#to52XZm7p+m6cPrQ1c54b^duS5rj zH`kbF5R{PEKZ_ae_@lk+)EtK!t-VO2 zwiW3=_z4W;4dn;p1ZqM<5ix3&4rq}%RVp;=v|eznnvUh86#O4JVgp2SKq z7#x8U+KXE%WaG-S(ek;(9+!NsVCWpf-trU&uG|=5Tb}S|qw~)NXF>%F*W44--ka25 zzoeRCxv5wSiNLfM5dcq9g(R9n>RjDE<}0Oj!}3?r{1pv!v%pzzCaiDE2su<3y5Obk z*SU@MzX9sngA8ieBXL}nsLfBCYq$u2u8Wy95WEiZ;9Z{1kR+h}mreo}%@boZb*ae9 z3!u~eRulmt4M`h9@)ZeC>JZ|Z6XjYHb$U{8>`jihw;1HTtfHx*Xhh@rdG>XOsEE0I zoZ)LQ;qc;k(y6GFPq1+f<+)T~TBs2O*msfpJltE;fpV23^It@DPE;R9gR&E7;{DDg22p6JttIt3(5rE^LhujsSGded zlNCQ(ecA^BpJUxL?x=nEDZG zy;WL2m;_@1v9fUjjgx&xzE5N$!0H43$NxGl#fJjiTG3KO?m))l3?yRu(Pdipmhrfq zo6(hq=+RCv5hAQff|p%t&nLVbM_eJz{0p#J=m>|RQ|hKt*H3-^3jS>Wophr1i>4erfvrHdZlownH-o5)BPZ zn&hjTd1DtO=8KuC%lwOJ6>-(3W1;v|;)h~l@<}dd?78Yc5=9~aSMD3D2uug;?GeA% zp76~7{__l&0_*OevC+HCGzBT44?_Oq$YgbTP^=pbYIC^JsM7SQ?|4zo>sLlvyjm)N z??tRyy}B4VOpQs0LpO@syhXBQ&ZD<_dU%*D_Wf+M3Fiq z`7imA8)Z2L8gS=rg_U0X!w2_FfDbs&3B6w!J>LbHnwz(N!yWr?FTi%JXwuYTQY&hGARL2)rq z(y{)1lBO??^d^;cRY$buo42IVi8!ncx|3um8fBt&6vbEZ$M~%Ysvq$N zw^4=Rmi~@>VZAgg6Y0=MwaA-G`!W`WF=kR!1#akDH&U3-9VqpL2hgdRbEk%_B}2gL zi<*a5P&%u!CXUHJA_L%(`vm;_{AJn?jvHHR`pT7As-XPSK~my+4AaGR9<(ICs<{xO z8U2?yH>3=B49FCX)|w15s;bfkm!CzPX~+>~>$<^2+PW9R$MuJ1Tehl;KWLI7#?`o^ zj7IlZP6$S<%jUf?yMLwfnllrH8ZRA3(khxlYK#pyA1Wk`jFDq(oZPG(1c5${k<96_ z#qzgF9KTfOUsqaiQQ*>?hPI+s%tX@Es^`5LEf5=g9VVopWRKb`eB5fmQ&^vKwLpr1; zXEV8h!*YOQa;i10U&pz+%&GR3tmR9nJ%E)-PP^1iJiITt9PKOSjh(AhA<2*95qx*{ zCU#3dQ=N3z7gU<6M%}kZk9RDY(u@b16wFeMxHgL#@zKi+iH`obV;FSc4`8znTwKt^ z4|}L)b-ps6+}n*H4*X1RP}k8#2k8Dm$sNv}4tYJS{O7?Xh>dFM8uhGZf6J zhLG*6;r?17WOyIH*JYo>YyaNb)?>pJP`$*!2dY7+VEV->n$FWfZh)bb*jokNWav(X z7`2Lc7$M`vB60$pMGxoDas$uimF~74(wx)=Iuy7&RP8#tlV1C(y`^gm@+SSaIBVlw zuD)=QkuckwM7Ea3?N>2-{;e`|hGCz_4NvAIoNlHOTvRs+;Ao~2<%rlsxGsY;JW2|R z&DF2|!z<-Y%Ubyx#&QHDwhGS4>SO5%*KLQmJE=q86!$GH5M)*DP+bFB_<})4;aJNd4kh?TTK9^ zxn2G;bZB^=nr$@^v~)l=Ry%=}3N84XD7EX>0Nmg+OTx!o3Z#J;gID??)NxLwp{h$& zKq&gCHzlFN5p8FnT6`Y6hm9HcFdPlcM##E7r9V3w8mDA^L}Vb{|MIKLny^WK4npUu zWsIeFYQN(UG3WZmZow4vr>#_a*+JDq-6Iu$hzqU+Y(AwQQ7O1~;bNDtF)|G6JJ9(XXo!iR;gE;4 zwF#7zlzh3Jj0hdz;+O>#7UZDx8s7h7ysT*ZM~tQkVcsRg=}jewgOS6-{1QpZ7X})- z$D++SNMKIZ1iC2@hVd6;JZ3yr(fC|=LCTRN6+JYynlPQrR7CcvAFBLYOx05Ki3?U` zl_X8y9Ew=LpH5KT*2j6*=URGDF3A92UF9@5=DG?Om~2i@nWjK>RiDG#s$jvIM7YQ- zj zsYw+_#GU=*TZzk~#@pz4qfkWcXC&MghIfROQU5LEFY=ox)HV+Tp z{L2?YzcxNpzpUno4s-VO3W% zmhM|@oU131%y)p9T~78Ogg>>>8^<{w4?0eVBtO>vVh;-MCtr+4Rmof+B`O*N*l^Lv zjRTub1%F$Etda9lj>k=>k=!VELy1EJ9aO9c)7KP2^7AZwDIwQgWEZXd9k{jzdwzZu znmt!%AcccK!jeFhK=;*ze{#XI8pmeWVw(#WhumbK9}hFGI(OY33gmQE&_X_J&)MIGe@ykvvK;%6a29T57QiwE zY+ENHCL$iD(BBF6CD_T{(Qjs{(CTtJ4EB`~_PM_zQK6PE5_QZz{lPW$mX z)f-z4o-C8` z%$?4`pg9k_xDQ~ zO}njc_N9hN5kCbS+voI_u$wv?3zMJ2akT~^71&5RGE=sTPU45g$#mxqzb-v2- z%8&RA(q2zZ=B;K3H7en<3}36LYdv-I&*PwW_d|SPRjrNE>Z^AVOj#YN^QO_%nE9c? zhr##F6#7fu$6EEsPb1mN!)D^dzMqcMEVEinSn&Cvu~$+H`%+JDQ}x-Y1FY%WNPh&U zVT(%7K_qCJg>Zm!&1WPgnhL-kwhd}cT9;Ov#|-5_ag6rJ$eQ2m6IpqCrph5t%%qOj zl%NMu0`q)FKj|GDS;l7Mt>_MfuOpLlb;QN{;yu)Izf)1#=<#%u`)Ek^%+Dab+QmCm z7=^$6@uNu#=Ljc9v~tedjbODAopqKvcLr){56j0@4enDf7ce`_kEEc+n(hppKZ~>V z4(ajX2i$_1mC+;Y3Fc+u6&K*$l+;}q*smM)jCHK52NnR!>hE)6WLSElgHLDqI6!RL zh=jdVj@vy_Q~xKZ6`5DH!+*DqAx$kLN(uLsdO$lu2%V(#PL;bT0wZYep%h*!_jCCt zZ?L*Sq)+4V3h0qE%jAAIXlW>A-A?NGm#?2~g1)U9;a{|}N-jb0v1d3=Mf7K; z%z$i4H`@4vx(_=PmbUb{?+N}CY5nY~z zV#A*ch3ezmlU$h1a^Ka|oxZyH65Q)mdj%Qq3W!>Pool8L8M4&c18CEE<>FP)nk6OP z7%#xOsD@d4xmrD7qsFARB)0p?F(g zem|q(H)-~|1hlR3GwYoN-LI0~L_RmasWKY&U!<9*rvL~d;02}M3g-E7Md5OPhjTw9 zkZ$vMQNQdw)0^S7SPa~|4AZ=W9i0v;m{%RU-uN(Y9CIram6jA(FABFj!aa5W72Cis z_OV|IqOqG_E}m9WQQ*ALt+@88v0Z|?iHin)r4(_T?7gQuu#w^Oz$4?fz(FpYRp6`( z!dg39<6BkW8jjV1GFS2@z`%q>(VQ$+6BC*b&Dt+m+gl^e*ehYf%x;egUbh@$+RiUV z*_9MINAzz9$Y6&T3+sa?ztLCw_f%?Gz>P_NO9<0;Dt zd{40BD(jGJSEuPYrtKT}#mRJ`V(h8P^tMH z(rxIe@T`lVFkT@(xow0r`aC))>csB`mW?@9`6#gF&e9!DB+!{U6?}JJCB0d5*6?>0Ib{Ol=icDaX*K#2 zZ?d$`7+IxeXo7b4gP;%MmH$%X-hOJ}T~je%(JKx(oJttDy*(}=PnX+i1Tml<*01?! zlYBCOId|MhMk2nMayL?l z-ufBi6Y!Lx98q_m*LE3a(YW<+=6Y&^$J%>o>T@g0*KqJ&;G$fEn=;L?`$1=W5!w=T zHRc-GyvpNdNQPE#ps2K%a!6@r0CGd93~Kj*Gg-N$uTaB$RqEfjKtAw-OG?{`doBx< zoz`Ricr`V5dRVUrUMI`muyrbYE_E%~MD6?&+Qoq2dntsh)cormm~DhX*)(Z}KB+#J zv03|66_e-MBMCzm_i1u~zBcx`w={2LNWC`2Bu+4+ys8!!vZ7FXg&BS#zeW*7?W<}4!_s>r(ZjbL9LmQHR9#_!a z=DK4hhega^+NM;WvdQo@HB}|#Vs(Urye9N@&kE>yRoemH;8k2bY)JkC!rDf?H5&b zc4V&GUz=)&ulh#{FJeMY7jy)Lo;09z=?Xj@u~686(WIk|cXY_%Ed%Ex4z9Kvg#$6- z!Ft_scB|w;87?j}lLRfY-}e3{bTe{J39i3};{0Pf_m%dc*CiHlo8r?xAQOk~L;HZ! zHl^rv->7zU zq*iAEQ@X;~-TNJ%qfAYRCyq<|gFaK^Ib#M(q4~fW*&w^%_HE2c7xPnfG~)vrdmQ${ zxuSe``EY7yOCx4Yy87;Aj(HI_=LIrTnqu$g7pj4Kc&AP1-pNl*f8|8-uGJMsviha= z1RzfPMNdGS2Q0}9fYwwYG$-H4t{P`;72BRfv!CfL2tj1zH_GVyX%v5cJwn{z|O)-O@Fh$iAG5p+>VAXLcD&scM<7RaUok39XlFaimBjV<$ zX}IwuI$e!pP;c{Ut$#shX`{ZKUb&7u}tzAQN?Sjrc5g`X4wr%uF2P6$L`CDoh_(@x(q_#IT^74sxaBLfx_?dxfEtPPfxgjC zL}M@@hy=(vv^aL9e0^?ql44Wlk5Dg&eac=t0Zy{vs;5n9o*a8hWiK01)b`Tt^?qBOWxbg|iljRESRr0z&OJy?FbU&BzadJ-1>Q#%d*%oX+&0hAo z#BlrVuQ_KCgWY2Z)WX4zT zZ&v{cWQ+u}GYfiz*XK6_9pBMz676lQRE%eOUhWi8sKJ&7(O*dl*23_euQWC72 zQXKN;jw`UX+i$wT2L^8{<9JY~gl@Tm4+x?ueta*2F%p3?aun zB#t6z4Lc#@fje#mA9wnWs{Z&Ju?0<)S&bf?FDNpo^-&0P&k3AJqq*+Aq}18hzfE85 zA;)_}C|vQToy+nf@FH^u4HpLf@wzeN*=Y%=uP7?mWM114*D$gFlfhG#HN)3J9;C4* zG@5fprhGtwD?57D%4@xjps?1{r5-Ydw|G)S>Moquk=edE*JZ6f)HxOJc@~kq?qjhk zq%g-LAFtQWl(#?7v6nl8NTE-2JYBi@D(R!3MAS zN+TsA5<%Ft*wz91&vd-xK3f|__fZ%u6mX)i*Ezw0t!+0M-0P&Ew;v|o*Bq(~xb5^e94E=xPZ)2Lo>-$bZL#4K zo4%T%uh``DQ5#~+ZjT~+nnSo=gm_Dp#i=pUb2~}A^O*3s=~Ll6d3rokAG=XT0Q#TO z3TTNPx2H*F-}9U?Z$``s4tliPQki(I$#EVj3OG&gGGu!QxGeA4J~(kY?A`hFBzdQ& z?MiZ;DVQ85;Uv~pJn*kIJ(3EvG@@f!7BrlquGWZy06EK+~($rq_Hu4=0{sz0XPEXnUOkhIcL%T_$k1n)2IWQshS23xRW zb0n#SOt)jgX${{+7npbDrtR)$Y{bK%Kqu{JC?7~W_0w+!FKw1T6P)4}A4 zg@uJ$jp1OaR#xah${@_Xw!R+Z^miKb;0f6Dm@(IS=w#4Rvf^-m(tmUSv|EHms!9(3 z=2sRTpMXR)*`{r$eN>vjP>REtsFJMhqS{{+>q5h+5dCO~WpCM45wLgdz$#i1U?eYs5bceOFRP`CIO>I4WLds*;+NLs+lIg4j1lFwItVI1--jJhSDx;VPIJ5A}BCd6ua~R z@<3LBVtWV6kN<1@NY%YmL{06a*MI5p}Z3Js;lf{Nlf8w>SsYFbJY%jTU}>AbE#n*+BRK;^a8q?@odP# zc%0XTETHP}j5cB~v?sRB0iSt0Gkq)675)Eu+m61PH|2@4n>^Jb5sL8J8mw}uYODUU z?Zk=>>OxvdZ;CD3Tt~#{cF2|>kkSFR$mAS{^+psojSbmSjNqiPbISt>l#wY0Pop!^ zg%&+?ugEB?hhE$H>P}iW5;4QIPp90;NKBtE#XFz+1Dq#-cptumV~r*au%7=s1Ft!8 z8BOnQZ$q1aV;xm6lg4})f|G+zM`~uls@_2ba8c|NrvZd4K@cz~y%ec8F5iI*fmWc; zdeMx$;7!tkwR3@$hMXyPb8a0v6hG4MV#(dDu@1U6h0ozcITX!N+DPaNN3E2odD*`9 z)bmAqadEFcvyPE?SYOg4er}|E^NvI-@7VCOA)DD)e&~Ma6R4xIjg)R#QF8I+e*go( z3^BuLjd9~C3A&YdmMKVhV#<%Y?2GOZ3S?sXB0UBbbto`7f```S_UEtnRO=fHk6Y96 zAm{OCd>od0OV&^Ci(b3-zdiHw8@CrvB9?>RT6qB`vSWZaQ}xN3l?=Sj2Oy)S84%=h z+&zHowl{nBG;J!^HnZge) z|3SR$?JVo+tx*3|w}8&z&jOwXv`1rh6?8SL8*{cfF-i4nw3Y(Uo@1JEO_zy$p0TMtRi|H9j_97lWcU3)(MY-vn=d@=L&dPf10wyAW?ph38l z7~uI7o1L~B#=DncLjUuCP)n0~jq&X%E*$6Fm9jPES*F+Ny+b1KN*1^!oD$uTlK=Js zSPd7j{zg-aG#e3$+pZYvC*D1@;J5{U^trgYz`tj~8ZyzSP-iQ)b+Z&1CgFB<{=jT? zKwta!qEts`ezCcKj=ZM*pSM<#+Ok#nzmG@Rn}GoOJAsG{_An%zQGQr`43{1xDaq;8 zCsd{%I;9*$2}-{DQb!GD>(B&OH#0P3%xI3rA=4ZruG^~y4M`2Z>St2y_=zMPmpSol z)oiKSfBye~!1^Ff_A-of63{O4C6l8k!k>Lf%#dPX2Ddg<(LdH=bDru9R2|-EB$ug{ zW&}@8<7=VO4W9(hQt->!K_$e5P*uSc@9z8-ZPs#sdp@AxMV0*q$CPz+@{e5CMdwFJ%}a> zOs%ZFz*_cDp<}<3SF35{J5fdS{eUO^dq8AwzI7w6uKeC$ltZ(>pqRQV{AQFA zjjZ9$WqXSDYG|FEF|d!h{=#lNgTLN6?nK0a>9g2SVw*TnNVz=#UyUL;X4(LRNPK#F zdjEjx$EQJ)>w_3?>Cxh>_RhWa>Zd!}tICFfqgRO@kx)_>!Zq!4CDNvcsA+VMq|Wi% zgb1~ZM?BX#8*gZ$QXPT`SwABvlRelN^8dm!~r zN}}pLxKnR1J9;kB4QR)!LyxSz(QUZ&nG!5Yiz>3e3|p-BARM{|T7%D;kA5hP*1Dag za+YQt%ZxDv5*dH;>EtU=j@ib}cn~HNMkh&uKXQD_(L4k z&ig!>ahZtzBhQ~%QY2qtwaEh!-27z94Rl4rlV_|s&C-?5o=1}NOy7^ZE0)TdtN_Ep zw#VC53>{XTmu6cgB`D%vVjaHXlYRK@6t zn{FrXZ;D5k0U(y)awS}9inlaB0*nFd%Yv(Et&8jk-E6 z*d9g)>{$4I?}nuqDR_Sz0Q8Ztm8uA~N`kyUi&6jd%TAYlIX+qQ>-x+|Cx;auGcK&? z>HFi}$kg@x2lkfBE}xyg(w*<(c+C8dc%-`{?+Bta0omfs+C3cJzN2_V+NMBfWi(Mx ze}*jziOjYXUYT$?%H8ti*bo8(_cQczPF8@-O=lX7LwOc~6Kwf34<7G&8FT0?7bkA3^ zK94wN5}odIm#1hHA!q*fmP4r|sCf?0YDev7afgc|_XnJZOV)y!mL%qOY)^fj{(?I} zXiz_cuDuoN=$Ur+l7Is`5o=JoG*NrwB`UGgxFz+RiUN*};p{2s<`1(rC8$UK)lH$z z=DrcaxTeBqr*4L%?33?@+Sx(N37*@><^`ILoSi1w(tGu;1}l`SC*U?+WR12Hn`w^B zVRRl;L$d8#EL>3DCmE%dRBrLMAn1c0(Ie>C6?SOhIyE7gvY;rc)Dp&R;+?s3CiMTW z;(xhq!X;RIawREjzu8N1S?yiP7H~$e^7^May-&t=(&9N1%XJNz08CI$*fdemI7!7s z-)c>JHYKx`Z z$M~R@zF{+>9|LN&vB4`iX2RSQS`gv`^%-#q=Bwa`ePz16=DdzEt5aPf-L@cWOp;s` zjG^(lRE`#OrptaB73E9x`amxdd%cwHM!6!svZj*2e}*VR>9&>IzcC(T=NR>7N*LyV zYO)culWm}|15e!F2Vo>`ZW*<#76G$dVIoVIAjC(Z#&R8ror_oLTywj!?~v{lhnlA%jgpu z#0p@D>G=-y{l*g%A6O>2lXETs^?OtL*qiB3$4ekQ#o*GS~ z;Q3!XL^H46BPy&qkgYuqAW^*NFVFd#n6K%%Ny6uL+0W&gOgfWiG0Y(!r&h*uVwxmd zqqm^f&0tF;cyoVHrs2-+)BfN!;^K=Hy{S8EH^9z$+OCq(;Zbnc9Bfj#bwNWciO=Qi zmexM%L)L!Nt-LDe!Et z{l2X0bt7>h$)Qc1Ii5pTJ4yfPy3W$7&i{$~ohX*;|6g%m;6oUjL+Mil7-~b~J;B_# zlUG`#l=9CcZVJii!BRAmh`CDXyzp3T94XS30hsL75s27==RfthA{`S z$#T_#Zts2O^{ip<=y>s7mG2o0!4z;jR3#CH1k zf>b73P@_#-K=LVV{O*QM`f?M^wTf(bySM9GCw6v#R>^^GEZdgS49@x(Eq%g`Or57T zGF(;R_S#+EkUe%12HOZx{iAa2I9)j!ED8InsVAz*$EYEGiU6@kT&+G{{W#Upx?eN- zwlo>Nb>@}o>Ak~yAS{ViRLGf1l{z+|h?Ng!mS#G7`eJ+h61CZ}Y&S}YO0CMrtaIy+ zDU%9yRgTg=&=fqh*b5JKcv%+#^qW8vA2ZNV9_w{w<(@aYy*A(++2~M@$>u-7Ze_@E zF3Lm^4T-8WelLC8Ftye*_vB2O{J5X)c`*w;E?BbOnyy?PzFHNyp6WLK(+_cOVCkUR z^521VWpJ=>@plbcA7vNQ=TfCGl-5FQjHRbIlP~|G9K!>R!U0Q&k|I+SQ4Nw(DqLp)D#pm9p6WT|gryqQ_BxkHWd7Oh#)s!xJYZ+{OoN2Q*lKf=HI!Dsxg|a=y9hg6(#dlsv^EKO* z3+F{aLi_#6rxrM7SPmgF&x04gO-{vl&HY|?mPb)a*T#-k_r{K9SJ)0&myyoROK2O6 za69o3%Uthf5@YS|EZdE?J);rLx4%H}DUAK{-D;B|Wwj0!D2YU3cPC&3vUc((;{PG+ zEyJQ}+qPj51(6bwE(HORZiX1}qNEj&?jE{31f)wu>5%S_?(S}+o1trH2IgJEb>Gjk zec$%Q`r#kkX05Z1*pL0#Paa8aCkh*(W>Pj;?Y~^31+4{}SrxjHSBt`17e{C1TgRAu z>24S>dGodEw8deU8t~9QXUiZ5QE$tjp+e>7+nY<+YDE+s)&`A3{vQ`|vw8;{1G_I& z2+y1_k~qqtL$1p51%_mJoi1!3umW^fz5PZC;_44f*v;KTtbp_2rP>-p_h%4_+Z4PHs8?(zW9tdfsw_51f)!vN;XH9XD1@ zMuBEg4K##$L~}R%)m@1Z+D!4}n{~{6f7EODVL;hUOiV%);tEo(1>>7cud`0O=1U8%9tm3W*aSG|P_W8o=0Q^uN z%?c&7n^KLtLD=$7K-7tIVN3J1J5YA{MD3@n*1v5`_d?U!Z&@`5rFgXm>&7fxXD}S{ z`>M;Gwc`z!oiyGwF8-n?fF3g*tx0<2+T9SC!Vo44Bh2)?hhbEoh>-crU9gopUmKQO z16L=LvT>}kk%pS_Oj{ayU1rU+36T#~@1cQ6ZSJ6XiOF3xnBRTe(p;fzzZFeqZhD}8 zNJ!QqkNm_ghew$sGd~o{>jXHxl$QZLJ?5L`rAQ0`Z&nE{D9P{xvUvBzh&y#~=n~;e zV}0{@`D*995xV94Lxms)M=*FM*^B8;Cq7#h{!>@1bZc&YX&@m@8b8h96?DlPH;vDy zUU!PvgM#YnY6reoLA80~vD7=lV!1U~Sh7ZRbk70Ye}E4oHPHOq_TO}ump-YEZC$;s}BiVE+d#2ZMR@WfS0d(_ws98zxdLv z($sAU@Q%ApF?(*Gv5uwSDyh$RCX32vf!goPt z#WY_^&Vp=zI`9HcqjeYU9#*^2D*o6*Af9E&wglAs-eJzPFWlgZfHi{-xnENpQX(( z-oD8?I2^p~iGBNBan9c~Ue)zEbX-@jFk!b_>qJurGWB~pditQO zthdR-DY}~FwyXb~!eD!qh;QGX)Vmxt!?1xkf-P-CQb&gp=m4pj^WGXv2&WXP_f@U( zkO(Ip`zr(Vwm@EnV#0!Rz8NXJ9gCJXpwSi+=L-(EU2_Hn+a|7#$mfO(=&wEZ=D|ko z;4&LyO6=t0wT?sWU@Kz^`(ZCEPOHPS5m)AEg`(3k6u(45tX%vUduR_!)sGY{d$XBt z*1j5C+&AA}sLPU>wIJ!Ph^4Bq+w~K{VzF;5g_?JlEK?NugrE=SfdjIDWb&c^vx zd9&Wr>UHn#ICZG-jCthNk~9*-5i5A0j6E1s>ht#FIK%pIy(%N|?~uR(x()=MrGTc> z*Bl61a~a0BObN4&{(b4oeWS0QO4huZX&PuyGZt;7uXQn(Vu^&vkN_{A;(t5j6oui& zd=!UzP9}MG7VlcQ-kX?`PRTZ7Et$@jWpcFInhe$cUKYKArGc<6GbR@ihK-3PQup? zHnTX&8n4&&X=J0^5u1C@*=7B^3VA20tirl=#=Q$K99RgnfKGzR*ng!19^eEkJ-?IyIc5-2nqcQ0ywRZX*ka~b_ z1(BIQ-mJra^gEECl{M^F+=_EN-=Ijm`j~UIV!ZmbMKuOzete3$p&107vc@7Ba&z(H zI>Exe=5*MF@sV1rl89YG6fA06qV%HAX-+}H9;;Z%P`y0owQH0dwDKKcDPumA;K*l^ z1?cL?Isv(WZT{QeB4*S>9_ZWgxBC6WGqePPsdy1rovmtU)}Tgm)*Q<~(JE-E>Gdca zGArS&RUYlnpYvkVnd7&N!%KrjO9mEboq8B(?YBx+r{t7=lrOKLx;%<`cY}o><%n|| z?UT8#_sHpo2+Ei3jpm6yz@G(;1cx<{?H24_T0FNzA*fl=Rnid z+|}_Yy2+l?r=;IhWM=EkJWUpokfvA~^(@yP{*fMo-P#*r%1Prf4IU-`TLl`hf6j*O zFRWsv60n5>G=c(CxFpVO`mn)~Z>u3uA(01e3N)X778Gxi#~-5?Px4&A9}$qI@cQZ- zg+gDEC*(fzpTTiDurQ~`rpb+y^)Sl8kK-FAYAqfZA@pQV*DZ@)LP_Irz16vIW5bG8 zh#|*Wfcx1nD|RDq$~ZOI8TUT+gSR(Y{{25qsAA4Z9bC5YbU7aYVUIk1PA)!9F5wEr zc6Fz~oThahgZZIh%hvKik$IKv@07Ynx5GMhB+n}UIwhiMdU=DdA+*4_7?>|lV5bp9 z%UXW=#Eu`MN}YZuB#_JOO{&Fiy*gfMB+#;(^gVCB$ocmmURVt`mA(lAtd?n;-&3b% z#mV)PQ9#uWO(XiDBJeSweB2;8$G3xv?R`K(4qnkf(SrSS-Vak1es1GQbEY5k{00rT z)j|zbg;NQ_OtY=y+)v?UMyQWS`q+VQXpg>|XUp;k)1EVRBD}8%X{J!zUVIY(EQ~n9 z(f}6Av6e{_Y#Yx@F6BBSW1CeXpPzn? zy5U##6KM5Rg0YS;PS9saV9ro#;|m1TxDH+}7`1c|!{@H289D&BWQLVz|CPC)<{&H` z&UKLZr-^)EucFLv#|l!HCak)Nzv`$*N*K7hwlbU#?6F3fTgsomx`81xsVjYrkPDjp z7;E*eIL_$^f~<5r+^@iRh3W2}6;_Y@*1N9ptl$XI zr>TFTxpN<3lh)Q6hFIHIXkO`0-F*R2Y^kZ`kUN4{f^D?umC4J*2*@|6VPGM*sID?0 zztk7eNQj~8SqiuL(x~awfu-t2{}8;Y*YARMt2Akc~ zYrI4;(@gd9wn{R^oE&is~5#T`?+3ry@{&LCvbttE_#86L~ zR5W+Q80Nfu-+#4tx0B45g}D=A*GZc-);vNi?_)+8fPGP1o@{f(ABb;*@Ee*p`i&=5 zyfaxxvtzfL`Ot+cm$u1_S((I*C0miPScKQesvHNnnEr(l+5!}3wxY@d^DSdE+HUdk zihjAYmTDpr6{k0;}*l71+sM;;v7?#6%lGXE$%88v< z81!{|jJ8OdD{BAS6N8JZB!d^ia1$_Tb8Zx%3qVb+=%U^S_U>oyShZDslF;^IV!_0l z?nUxZ?VY#ufR|W3`RXSJ&2;E~R&d3zz6r1;j7k?|k1qQ7Wc{g2qegsmhC-bJfQ>%( zo;KD7q1H2aq%zvz#nCiJ9}`Wklxt{N=wwdLdT)c#L+9Aa^``I(CvX;e$}JVwXk09o zMO*`S(1qb0*OHDjeJMlb#sk}<=^fTb4b>0UOl&uXBvlhwzgwcC$En_Dj2bDI98pw6 zLkOW$tT4}Cl&sg0Br$>ZA`OIGVPUl3DhhNeF9KFXb9P2`3*r0~QOn>{$+KJ;369ji zu8A(-6kdC)R>@(+q5kRUa29Q6%$yw?{qh9^;{ZxE$I(qQ*MMMPoC>;vgynT&A&zWT zk8uSEtq+)^B`?FKud4ZMfF)Z#=OGViqxgI&%zsOd)<%~jOcUSxHUcwDu_dv0PK zf{HOCM^i%yPx~inb{5}9Uv*;P@WEf+-#UDVXu^RHQ0bX@TCiMYG_F~txp1qWc-1+5 z=t|PQA0luJk9^C)@#BgnjeaEXbYvvm82*v+TELORMDmD84|>{nzD_a;T=xiUefJ-b zMd-VnrW(Idy6M2u+S@GON`>@xgZ?Nb_$=Ph8f6?S%m*@APugN=+1Hhtkl&03~*jTejm z*5J9Ka~ZOfkUCMh*J$>vxoj%_G-bRQI`C;KyJtOL(T=2`UbzeKl);m~l#s6SLP5L& z(E+oNu!X6kk|cT1X`>1EOe|~l`*--m^!gE7x9Tk~9JVGJ!7K!)gePqsF80HJ#b0@y zgQ1R&ljdXyVTX87ZZJEij-GuKpR_}Y5iRK}FAtjaDDRi+y1XZya*MW1Zeb19;MfkH z#vxu@ihmG$sVi$EJ+=p%r5y076S&hrfS^^%aKl6pzV05Q9lYr7OqwMX{`@mhONYP& znzd>IiQaglA-do_t1GPTn%ukZ!&^}GZ0Ai7k-X4POdrr!85^H~=pkO%5I(P6A0EI* z-xX!quH2Ku)$%G?TSSmcCZd>V$-Klmwy78zUMe-YAdDSM=@^eya=U3uclF$hzZ{fX zu)EpG8oc$B5%_gFd!B21YeSQ8aw>Wzb6W0vLO7Qb-_U6N5c0KHEtI8BpAZT&Es6Xi z`$T@?S#?73D-$$P*@ABg5?3v3nHfX-k;BR)B8pl@SI%C&v&w@gRxv;foDLXiTaX-9 zM>p4Rg`ihBW^u|2l7*xE+>x9(JF-i$iuX&O#4e{pDy=9bhjR=x4o#k#<2fWB^?)mx*L%2RY*7vroY_OPzQ*k^2)y3$TBE$B>} zTsS)ftAwpq2b!a#=r>kDcc|bLp6}mC)6BoKty1bVZea~KIq_+yQnruLvy`>M{>ueO z>rTj}8fs&HTsnTcj=uo&VTtrPAg~5eQ=$^Hvqda`@oSHwLCM#>y(_n$T_ltp_>G-t zDio{LXh$mnR0mkh@YWnWt6PWdJbkf89XCHuymG^?j4s?+4vE_W&jaF_Z)|&y{mr0$ zs2)dae#p?!sHKL%1@&o?!%pH{GW;1mOC0J1*5C+NL?};%uTimLJc&84=E)~op1{vA zxNM8@%-@pOm1dOw#(5peT0@4DXu#WoTLO${ui!k@a%{i3$=(hYF)^@kkdIHtsWAKz z;vs;~rDQ=D`$q4KGI`HgpsPuLyB%Iz3sYZdKjZ5PaPdJ7IEVI~TzCMvfye z;&+7)3e4y7@r$WTY0A1XkS_QZC%b34x{P!^9x9s;nU^^GLRak;%)Nw**C}BLj8yOFCtE*CO%n2ASc!eFN zHIUxoT>;8iTo_d;v|3>h<>zdo2_`GfD=MG!Yvx{8P`6?U#uL{R#=TTI0s?F%xG6rZ zF_>xIWFl6vI`+EE*SZ*g?_3QVa*$&%ZT)^LZnMr4zApV^joJ<*dw)4kssf--dbdW% z=+j=HZ@Cih*|#{rwSSrGGBI|dr=h|BE#2@JaP8(qNS_WZ)WcKkF%3r=f3oIA z8o(!Iws}uEBj*A@pg_o4YuJqHI$_m<67zED0iPYHtsMUK!&2y&CuX^Ecs9D!;bbzwGS9HYj8Bv8 zq9p2iB~Tu2d+sXl=8nPL)L5V(n}x^&ce<&TIcAH)beha|5FxBkmHOvyd$^w0UKk-l zI9Xq^pZqOBvcGlS+2CXC_l*j~ZZ?H7UJtWv8URFyUKM1mBn;4%Hq7Ss@i>8}o$5u=Yjyypf zsQzPa#!L2cr{I2kU~vkt=2b~yeXQ>@f7d1(23A5_ou%Lv`ZE6HrPcp@iK9Z){)qC? z>Z?dY^wGmqPy67%faE34DJiT5lM|)U8?$t|eUrWh<`59>umnR=S$-Oa6b{ zHkXt9PcxtiK{eE*rE}h*!Tt~uG+wAV6@!wZAmo>{mA!bCzn-e!6-(_=D}>DRM~-f7JVa_ zoafyYwqQdYRgDf8)3JBw+i3F5&4x?e)t(RZ4xeDFMl4u=A1WJ%hcdDHAa?<$is zjxk7ywB65NuO$%G-u*%4tI6$4bUFzKJHNni@@#Ql`ltC@nvPc@Z6Ep=s7w4r*Fjyw z=zIL6tS}lfq5N2UJs@M(Kwg_{@|%Bx;A-D7o=*6r$xlwR|8He>I5LeSSUL&4f3FO=9yk@*R@45d zu8A}8OxZ+($9tlChpp_O)eGZ~wI>)e?MvZ+AaY+NV&#qlYMyt&O(6=p_uq%qiP+Nz zE>w9WarhGLhSD~RiG-mS&5zL7rxkUYFWd+>NwY492b^pcJ0^900iM7l>7Wa{DFt`= z<4U6DAvFAC?@vrQvD+bIqwP@o+quH4Bd5y>$5Dz#LGTN~ZtS|qdHSXyshQ=0pP~R% zu1ulZ5Z(GaNoVVRzb)BkIsglPNFe_#?z_g-dJG#;V>>e|i=mY-uFF4Y>i#2Z`OuQh zZ6j2(fgM)w@bzwsIz+$JLIZcG3c8$8cTkOk%8Xus(U}=b-Ofkv7DLh+AAVmK`dqtq z!_u;w@;%qn&~>-pf8Wxvr#Sm}k5?sN5NA>N@Zt^&1ntzM^x|#)sU-2F)uyw;H#^+F zd+vC($Zmn}<8zC~+v0>;i0N62KW{hs8TWL9c5(KE;SDnSihiDha`+5ge&TJr@tQMQ zyIfF5>kb<`HqEg1WL!J{~1!$;Y) zyA#Nq&4Vb@OEE{=Jg&m_I!$2Fb-GAhHkr>3mq`X-$i~IQh$OGh)-+UK@J}eOLf&fJ^_(RQbvgF`AAmed6B-)&k?lDUHEQ9+ zJd5)W)~hlx5;M!mAxSQUtl0;G(KXWQ$?4>SdE1OpguQdlks#g2>uO+*VuKyl{`?9+0Vj!!mqgkvFk8WL<` zIMw$~#A?;P2W(~R@6HR#KLBsK03&hbEcX-oW0iZ1hL#7YDtKS1zwzGILn)Vrjj7H3sj z59*L8yY=(yj$dWzE$IHC(&|AFy?K*b0%UVr?-Jb$Gwb=x|AAAp_?=(tVJVv?ektJe zuZs7TwNHY{Nn~qEWS5E(BzZHcB|x5B=sHm7-zE3XCHW2PR?5I$ta1VC>3hj|b!pJ* zyqW_cU8yw*Q1jo9LTMXd=xQx^q%^&2Yo2(D)PnKr?)__c|O#>0QRo@ zpinn2d=&PWjn7M*U`#hH8hBvSua1OJG%dQJ&%E*p}vgwlz^2jzZ{mo zEis8X+E$Gf)<8E$tIgkdmDhi<&T@fSJdyN$&~eT)4C3)BEUXkN>k=}X%w_C4XHWaT^wPT<|@i;8hcwn3~W zW1Jx03CJ33{^xB0E~nvI%*D3FLq@AT+^zFpLE+$iAx{&ruGVg8>SUY+7Ig2HfSGc3 z#;UmUd9*+)Lk*rleM3VO-%k_hYSrS7S&XCh&J9AMkUC-Fcv7`MU&s>^vA_bJwr*-R zLrxk7x);`@5I))Xq$ZrNU&zALW;;=bP5xK|=ewiaYAu7>?@dmCFu9q=svhTLA}^Qy zB!sj!1?N$}m$#=k0XuTfH+QJty>mi-Ci00+{6Z8rD`NC5hA%f{#`NkqS~WS&)#ZwHSZANEVb#npuPU%6)1EblwY_w~NyL>Lnj17A0%=6vjItCOWqtcDhk z_BSxXxUDTE33Zh9>bgPrF$y%Xu{eU#maa?u@#vdHQO|I)wB>f9qV_XY7+%3}Wd}?G z^@B;}{8Jv+J74J;3j6wyh|dp7ef!!-&7g83wK6@FfPere_sE|0bkmVk?Ek8$=#m_| zT0S@T6jbIW-gJIRh5tdSj%uL%c>am|c%t5ujv*0C$Z7=^f0gX-tOax)Vjck6i>`n$ zZh|p`@V43a9%6kov>bwG+^D`ez%*Nvt=i(ZwzBPq&MQAjei7fc!-l1@Mfk_v5RbUb z-SD@zm6uFN)Z@o*EP@&MPUcm_gGsD8$>QU79Em$6YHj1ubXo#N zY*xe z7F$Gd6;*>l9aw0;Onz4tP3Y^X^WgZbG>;mZyc^}h2uhN8xA69b32B=i{v%hEykbktCyVc6+fe_`rSt1f z$%F%PEQyazBZTcjgFnW+q*_)k$*5vbyJuc5k~R1n%+kqoVgIOM0~hQAGhBq2W(5ikQV_b~}Gy6W40vkXzFatx=!cFgJpQ0Y>bMjW0yc5N$$&Z2rO!&D1E+ zPbN+H#Rh0l#FfOHeUA+bE9U*GiFmM7Ep0JgvyW!FfPX11xx8Nl1rAM8SAB=C@#{e> z6sU&PEls#gV5>n^cS2wa{&a62Q`+Su~jlK z_OShT{e#>5!V191vKT%gnQwRUc%>9cZ5x*|HFe9cxBja3x#?32H0#v-3+AiJ8m^YH zbS7fgjJq2Xoj?cC>!2YuSIec0z047>|H!$ZQFsGzf^cd{{YQ1=gk{O|+^pH&hK58w z^K60zGrzEW3}D!uwJEE+1Ac3#tkqw9-lL`CnOT@DTDCFNy4F4O9aTY?Y@R z+!E382-CKKyi<%HD5oP~7v@6h?S+6BoNt%+ObRLL21bw+5{M6~^EeK0y2Fjv7x zMn*=D^(@tKn%h&rBPI5Q=85sdd)@k^z)oYw_Lmbk=EL$=EKTDm6@Hd8TBX{f* z|MGAt1h-bd;beU{S5w`I)s|3C`EDp+Q$`jK9;#Q=?#MwXmiNvTVj2zkHb@nOq!7J@VEE9(7p;3 z1X{EAu`CX#44VOiq4L0msm8mWRQDKk-^J?ChIhB~BRZU(Cv_`D0UDtt%&5~4EP87^ z_MY3f_OGj=0MdOEDv+k0>}Ct&@qD}Ek*pf%w}AqMOijz|AjusfW?yLs`?jCgY?GsviH{|r=msy4Pn5t(( zI(m)ZC2>=RwAyuxWbJ)5ejOW(Xo-&jyCXPr;7X>>Mhk5yQ(FDvuvp7(3i9^y=2ye* zQBh9icf{?{UXip=tR%~U()!>?hK-F8d`!UrzDJi@n@<)=YhyPv)MNiQu;zLzUDV7( zZG-tH)P|R`^?>y*XzB2=w}&<9jC_;dMUVCfw}Ot0^iHXu*ARBH%V$+IPe+~vOZ~nD z5Ytd&D3p>yLTiU(DcPHLg5@T#qqHtHh;^VEPo%QumMEGjIeR)Nwq8kECw{M637(-nm`rgill5AF3?K{=le>f)&}5tYl#N!)#t;Z(^Uk_ z7G&dFidPmf{(c-^{*7K}T$hxESAep&O@B+>kCIeI#=~IZ-teatwRsUCg#*`u-s?SB z3EnJ3EKJY?r**fBl+2ICX9w-U=JL5k)<%Jk$C!^!jtyte>hw`Ksn z>Dz%iTl3sjXOs6$xI#bmt^QFs`$NgLCctSl`0R$yH$CO=m9fp?l`D&;!VS?MJ~M%R z;yJW5yHBi1b)^(tm&>KyyUW%+%Uy8BYr{U3#Z_WCzpb+YS?6h_-9kiTIMPlw(+D}-AVu1!J* zF0E+{iJ*?yo3N+E6NX(9b?tAAHe)Nkn3yZ(AM8^;V=jzQwXEQqF>wg-&If2%(OwE{qsl1&$M3#- zYL7$5$4IdYOO7*AWhednm{gAg9D-uL-_-QtlxbzzkleifV5b93!*M`xL+ zulHk4kYJ)A0ZvU;fbnT@ha`(F4cm>dlGB%1QE9zQ75Zn^k8l_5fvbJ4HxG)VR1g&5 zYg|ZKu{Vv~oMjq-jd4%4p)d~}P})eFrW<5xMKpcWCW1dTHH_}cIQO=%;mLME>bqun zgRspsgr!1*dnxrhjYczF(n6mPSgBb+PtISWpV)N`dfh6iP`ErVmd#@aS6W%pq}t)a zscNwD7tp3#r+r3Q!3h`ORmF z8P8yQiT0LcJg;_mF_32T5_FIfTMZ-;-aQDS5gl9Z<@35A)>7Rbb;HuFc2o3|!=KK4 zU-PyKU^gN?90B-I?Wyk0&j9h4)p9VkxbY> z=$3}VGh3j^g%a#HP#Ea$c*&?+-Lmi6-%Yq4v~`b}z;!N5nP^K-pF-7#P4CpBbxP;5 zBp5e|zr4~pP3Q4`v=S4%{^LXL$!uE`Y=~2`B1XOix$xytkZn+01jH&mtt!CVTjPsF z3r{q0VB7DDa|ecv=0yLJx&6(C7U?oy?(61m9c1J4$4om?G_L+Ed%&6~FFlQN=>OYO zxI_YqH1!eqr9J^2y&t#XtE$4N-Er#^N;F;#oU;z z&~&BZr2P4|197i@&z{TrY)Q}wHt%weUp-Ho*Lo*ykp zeW;7MBBTJ>UNMk=vBc*+a|V0T_*z)krEzwb_Q>}eY}Az=w{r*msO=dEpB#Ox`rywP zB0$jcKVAqIhDR`!pO4sTMY(eH*&3BB4Ot4u?qXCUu|1m{85w!q$UH#EiIfW*djpza zsp`M>`hNp074=j$ z6y;)T;hYeSM7Y^Lpj+_~U*#4QCs+8z(hw5!>cjlTGpvr^+M!7}`PQ->tpMbQ8h&7{ zbi(~rU+)Q6|AnidD{nf#d}bsObD%lO{tC9hRZv8P6?${^!RWyXbVVY)htcM!!qQvU z#_%Te+TITD#WPN16nF6yV6&B6dL6=<0C74l{--g>u~Ew{F9|}+8LkCNlUw{(mk{?0 zk3||@@g68G5TiMMjOb+htpv;K)-ow(p8phz&7`SI+N@0&EiECzhz$F78|5OJ!~jbX zO*fJs>-_MGz&}+rtRr=n1^07m-MVB0kFGpFPqEb+!anlzL}5jxZ2u%AC>h;PRuSgp zw4@2wx{J94@U!ZGFv<>HQD`Qhu zY9L}QDzYxpDel)ses$s#jbWZK+0nF1blst$%!5^bh-N;h#OK6A@a#&lL1x4&%5oFt08SeEB4jZJs?~{l_(o)9G#_9MKaKOKxWRe5 z*ReZQ=$CGCDT1l-#R4gzZJCP_Q)~d{uMrx`SOga31xbvE#_dwL&^qCgnzCA0#;2tA zs1oCLDO{MSx>r$;!2~O-?IMQg_FOp$`{zGzYd~Mea+d~!5NhXkUnx#Y-G5<T?2>dKgka3PtVLr#QW-nue>k#tNAWQId&sH zz=JifjKmB}|AWJ3LB^A1X36rOA~D*J=dzi;0@b7^2Ou*MA2ckIK0JDzVSLV`txpKS zRJUtYPiHWd?ot{K&Kk_J5#}Xn(JSp)8j56P+P#jCN=f7BN*$8#Z9@}SuacF z$KbA*x#)CLa5rvJp@QiFwvBz)Q`Tr<_=o01m>_SOgP>%jz>oD@tqxRH;a&_9-+!jc zTb9~>miZdG1DmnroX;k3Rshk z1;h>+1cypx9JKW~KoQgU8gkdBreoC3e7FcJ6hcOwrmyT?0x{9ABpsERqz-QE;3)`0 z?aj4`-N>W6q|022KHmfkv7Wc1856Hd?mY*r?^2Lrc)+tSm0mE=PY?&N^?*bTP>1|B z@2a9|m*4s^F>$nIXtd>_mm6bfbiqG7Pt73ZlZIlbRp*#f`=%fp*njJDHj@{WkR{EX zL#oN!wEL0j3dZ5*h4W1>a?+=PG`@quE0Ke)?kV;Hre|_(Eo(xDjqdMz_~%Y;V}kHh zH-ia5N8o{=Bmw)`GtTXe5^wE`Zd^W}K@88IJOP@iEn(H!EywV)Bw}8cG<65-t>5p; z?6)}gZtWvo&j{qrp#}rvm$kR`q3aT?UVarLKG-t>Q70$t6d`{blfQf}wiWr^Rgy56 z<>S+Sey6QhD`mdtS1x?LwrFD~?;Tnb36T@~jPgflVa04HFYGAC$DGuz`O9Fv%ln#S zcjm>4G~-6o`6Cyq16c1)(kDoL~h(3sTyKGTe5KN z-JXlEW+(7qLTA(DFdJ_np6~v)>i=YdUD64S^31~?9+|2!&I4s zuuNJ**bJ=a`0os^odzZV`1D_ye;v`tos5p1 zg>O0$!%6!O=f7JL^vZolMr_&~4{?9A@47-)0mDHK)<;%_xyh z1^OBWj?)i)L%p3orbI?z$gN2NkV#(tCagqS7L+!lqSc$>SRb#aDF!9aD7!;)4?YdM z?s{8O5nxVBY=QYBG}F?CGQK38Nc8AimiIC-WLjqmrBohF5;{ys*x${AO$=vwh`Vug zj2T*nb%Pi0R06q6|Gv-Idls0pJk$e2bh<2jHIJHac4#=Z)Aq`-ozg$D@5@3}cRVv1 z=HHWzq)3)BO3{(AoqF1GydcMWlBVEeC~l2$=t>lD|4v%k;(C~mW9RRimo~!^r_6r7 zTh3%-t5=MCCjp$G1{dKaVdLjY!{Csy14FW7-EaR6@#9xNndlVFCigFFllzn<3qSz_ zZ+4l>H6W*OX0!47%C6E;GUj{Y2MO5Ul@C_aO`a^0+6}>cw`p%In%1F|SPY$1IGoStGx69;r*h*?)-`(SNyZI9o z@7{~Lz4uDJ_Rx?k+iiZDLh z&61z=NslBE0}zQt{ni2aJZf$wOpS+;K1Cg>pAwx8z)O;fT55tATi*spWeX#^r=5E^ z&ofSZdGeQ-D?@qweCcX4Gi~RRdRfcfn5bHy$!W;(MYi#3_j9^5_3f~dD|9x(cg+!# z!0%Fd{=8A7GxBm(w~MvKI%_sP&UT1hG~^LHnnH?gkI+oTD@1^=Ss=wsXS zq%bNe6fs8w(Y(P#H0^*1U7Eg2M#>g_amqd)$rThT3}+G2#5c8C>Cv5DjXD%J_~}bB z5re7`d%Tazduk7Vq6mvqN_2X@uBmc4ZS?&Y!AQ;s<$AMACdoVt&izdgb3?R1QvMVL z!U%1nxjku5bF4v>+NABDtvjVqP6mu9d-P~JdN8>8PuwTddjj3lZuC8nZ8xRhmac-` z44`;nx*t}C@B5RW&>U7~$rtUg5hPbNv3C;V| zjY{kA;qdN9gYwYR-jo)*rEo^RUE`$A$n{#M<4%6`#1*o^Q5iw@b8-9U zUbXm~+vArnhM6x7xP(J{ggtyUOX~fTl9~CAk0FgNGn;2oC5xXu$p}yHMRD!V z%Oifq+hw_s^-YZ|cXdAbilTLX1Yv60up1^rJQ?0v1fBSHkf>_^4hcGOX&ID@(;iSV zjWBkPju2o8nUqJLXr5)EizwNR6pqD7GCA3|N@L^BeI`&3tX$fV(xn2O62{kFa8Z2J zi11e>U9Aq4w1}NhI&iTNI*vf1Luq|528AK1hi^kyuza?5t~{SoCO57c(uq(4&2$j= zNk1g4aKS4Pv?1Ftd3t(#D(qZ;QlwrH&u6zrsTKn1G>9SfI=Tok>l5B{%&6QIw z5M4yNY_7*E=9CO;Rfe2=dq~lb0lKqY7I@4Dr7FlvbtUzY+j+f zgQh81uV?8m_I;FoT|DuEPM9rDi?P=K$5A{xTq~yt_;)Y2WI(xc*jAmeVhbSc#~4`~=--w2AB>w*%PAqH>AM@OnO499Z+14!55`Im zAIwFY{+g4q(BsPB319}Iy&^bmr`>o1IRQin+!n6zwyjeAi=tnIT-$yjiOZU-hPFLD z>+*ae^RnRJFJ7t+w|wD~8!2YCf&XuUxTVhvNec;WNBTRZiC|c;*VQ!8mJ?8tykLaUBl6094?aVQP81qGBV|qLTk?^ns1f5+RM8sY_*uVu zeEKIXeDO=E0gXcYR$I&$?1#Vg_Ov+@C~0u?UCHnRvEMjdGfA}EV+jAE>OWqg`+ue` z|J3o?^QAbLxP6MdgrMnD0Nq_js;Gy&m>xfB6(B0kQ zz7_CDe_sIn#V8F=kh=&eyLcye&8s@feiOQ)N@thS4E@ABVG!KAz zmGdpl@VY6V>VaI7Lf;O zkMYha@8N79w7C1cjX(jS&5mCW>6u4^xjc3lfX%i}-9BIPhVzXVwYY7zb6&YX7||{V zTAzN6uoCFy+aXzMYBc?2LnvYN+^S%Qj?D8_2ixP3<-phIA?jd9V2&hcFO_^i9V*tWDYsjfAjd(+k&G~H0g8wW-Yrxu%L>xc&)M1KYPLp# z<#R1YQjHRY`rKDp?ungAluC%(7jo&p@9wwzE+#jY=`mCa1P+&I99I5^3foaXG5!P# zwXDpN3|@G(S)=t5rzyUW4*SjHV@c=NOAB_tXGh7c8bhKU(76Oli;h$qNdK2uNaE0R?c5J(=*g5n0_Z;2%`I_o>S4rY*>|pH3)}OtljCW+EPd0Zr&-aHAXt&P^&wLkLb(E-n z#_fE5+EFp#Hc2k7Qt^+ss|mkC@m|{(Q(W4Y$8>bjW0}>my^XU5$*ZJ?(6GS7`_tGk&Ftn=;r-m<}cBRFiSLo`b6YwD>+} z$TZXBgsbH7G($wS1y*~VB1v=!Q*yB0W*dvXQ}va>*X%V?yxWJzadbqz7R zO5waNy?uXpcj-nBTFmM4RTq;Wj^7%lw@2b3%921AbQZ&Zf~R>BiXWT@iiOF`afrH! z2-Bg&uFepAizZldGWknza@vu?Nd$m1a z0}7piU}IolI36uE$&cj6_^BpL-Ux=%tDJ0M*kQXQcYnIV$#vw9VZLPATvUo7+b*Yy zzKeT+=xl-zQAL2@!2i7oJz7E$0O<0jGK0A=(myFNn*jzQD}69ksA1`H_BXRBN>u_u z^qry?qNVhMf8SkdS~eB9i+A&2Cq5y#;mW4j{nP}^C3!rWCl-(ZtleW|o9({9f84zU z_8juMxU3yU{h<*~x3jM0Yg17a9hc_^x0J&aM@#(-%u~q!VB~`o(?wn(`w(Hz1G&W3 zu#u^$(x*=%*4EaBtl}Gp*#=lyUjJp(dK!tulULuxKcKka*)7`<%_uJvPs%dR4_N;B z!dc99IW9Y2)qFo$*`*~tu^??f;59HQo3}uuEE1mpyY(0U^x~84qvg`QM8;$g)a}qA zaCp!OxI`PJd}D8qk(HG-;M+Go{#QM){oK-#O9#hSb-!yhqO?kuCHeVr9zC*c_%lrV zRX|%>k&~&2f!Pe@kx28_=(jC^{bE!zKlOhI`^vB=o4)T=FYyuuE(sA3Q0bPIM(NI_ zL%O>gML|HiyJL6hZd7vVS~{g!LY7|YJ(uggpXWH9FYg?D;Mh55&Y6GA{Ns1dRGG<1 z1>XblRoTt)>@Bq$BA{RwS9%Z#)Em58?n2Tv=nL|AsWWmP6!`f^&f~!+#H`T+e6uI#yu^Q+N2mY3@`SQWS)RbXAu~7M}J%p^t-sDB_ z1ElP|<0~0PM~m zYS2N7j8?Jnw4o`nBV4R|h?da~Zm*ScerLGCdWW~wmac=fvVK&4(|Cil!plFY8wWRP zJ&FeEpW}-9c=t<*=^{o0M~vZlG+PJj1l+z?nmX-tI-$bcZd0o!+=nUVDDI^;7hqt3 zZ~7gGxpMloT8$T?4i6h6o-HI76&2-)HB@WW+t?!i_EWJd^uKIKYaxiT*+kpP7W*fW zCbpqhJ_?{hz3V9Q-sDYLtpu&dGf|RF4ed2ZVuJz&2m$_sIK-al0G$3CSIN@6`{jt* zL6mLoBPxpVWq#Q4VBSwg=RL{O_LKE&MQ?K{I~+gcE#ZEE2AO%}WVJpnqx+t*5Q%LJ z{_W%j_uM-FIp-kqH&e!3-i3+eHso?1>^81zy=6;pKFGA=zI4WTIiPjz@IhOD_D`Du zsb6^}bECx30aJzmZ_&Lq0S0&Dy z%?O=4eH@X74z%VM;BVWl+{=P0?k$c4S8JN`@XX;0DB8ZA(M4YKJQ$g6zpH=bB{jFI0Qv*04QG+IxW-a_A zP~i6w49Dl~F=Ux`=GlJOi13lA?sm{T569cd;AcAazFvb4toh;d3{T{PC0q~Y`A89b zkT=zC$?b}~J2?#FQ^=o?6=!y8xQyA3hx(jG7fr3I)@TMAa*JL1?yV}0b^6#ZhTHLN zMb4Uy6JZpG4AcY*gC@AWMuIqMPu;X8#P{V5B-&4MUOOYqXN}L}Q+UP`8r=qu{OZk#exvx7aw$D%Z!knSye>V1I_AGJ4Br7GY z7kVX%JZ#6=ZBk9GQu)F(igT+9Ir+W1X{n6b-FG9#zjj3x$41q+w6?DAsmVm$zF*V?T`i5h_f)3$S(;^w~O3TANVD)=xDfnJ0^VI@Bxki(2}=KnFY2PLMJ_JLjX} zhHB-{xz+3Pi^6>3)eAByx0NMHB-EZ=O#Q94+O2@75>vvV5K^knqA%ZiSY7kGB_^Wr zss1!Nhr*sbcPw+CaYa+*1npn)fJf%Eor`;3?UPc8`iDN>Qv!5Jc!-{!-kK`2I-7%h zJnMj37Cw#ZS<5b^VdCb&Y~$)1?i@`yM5&&nFh$q;K2dh62M!{=HEBHkX_{h5fD&Fk zq>RI0-5>EKg6*-i{8jPoLgnTm`Q)mb?99*k;H+tgXrpN}-gLt00}~eCI`bw1W9LIK zpUZ6JJwgqDnb^3v8jg;RpOm!ByBO%T67-CZ`{&DR>Ssj3XAOnlN8P0<&kmr^ikh|t zt(IV)=8RZ8O_C|v>WljveC2S$XWh&X_$4w+AiNd<*-d3m{B>x*2+!vBOx)SBhi4bt z6Pm*|e;(a#FIp|_tde#SlDTQsedLvib@GN}wjqUCjwnk;tGl5euH5}DCN15N?(dl< zzpIOBHb**WL%}Lw?%&nh)*E&RD_y>ad}he4HH`rY(VC*g6yvOvCYR*l>?IgW1ZDEv5;b4Cc;sKZBdn6c;Ui9w> zjiysOED)hDW&`eJGd4D^aWbL4GXpZ6JU?&>$WxSZS$D6Z3y_w6Yo-Z|va#C!Zac-_ z_jF6cTZ(-In&|9aHcKFRdin)gG*0dPb)iuAVhI;_%2>u@jX$Mjjab<~0G7DB{$2l- z&oNr~{5X)p)BM}74~uGP&5w3|hF~$~)vL*7OU{PyESWTbJ2=Q}I6O0GV^tqr|vake*uSkc0L&hbRWE z&Dfz`PCZ!~Mhp(h12i98E5yzgn>nVZ^7VwlHOuHO#@Gg zi#cuP)duVldtY}5IX_M-)>w4EzOaAp;wA!FEyB4twa@$0?Y2e>vETVYGH9+6>YM{L z5E}fDw~e35_pVKT*wBG!_>Xjkdf|<}_0p`qstvc>3?vI(HWKzAZ=zyD+mCnKg#0N< zVebqZMDm$TQ*O1P4%g9c#4sf&`(w5~GbsDjzxE2##hV;2yRYV`I87K6O(T!Dgj$v! zWJv9_kMDw8u0!g?7wJXHT8&RT4Rb%PRc2tn$-#E zn3CG1;$+~XJyM=*$Y;G(x2vdtH3D|&l>+j`1Y%KP6Aq$d)%rJ#Qzo&KuHGthiEIA zywj3$Ro?3!7G5vk?V&6V8Oy>=Rd%g)c@gBAc5{4#l5SlO?8ZNI z?IpB%pH;y=?|2{t!WSpRz$`MWq}v4f466nix+q%#gYes7!#rBxFpURvjNSCKx66(= zHva88m&C`N4+G~lEOo}7pQyF&E5QuT=#F=J@nGxOMJKZ1lc|<3RAUOg{SdBhXVTTJ zaONlu2^zDSt~=E19FyOUIQ75zfs*tW~-cvJ`ziN3yz zuerkF&f%hmH>464_WVpErRpAvG1VQotCO)XxWj*9-yX(w5J!@FV=AYRg8tlgV^pKl zV8{ukT{Y!#R*YiXyXa4RBN01WG)E0*gbl`XVS%zmdlj3f^Rx1_rT~g9-`OK;JRwUL zEy6k5g~Be8Fr_ir_Efnm&UhM`(#~K$d7S-I;_{q(1jJT9#0!#8O_xHcK3AC&jnr^G zqi-6SFJd^8(6pRCwqmC~Zj}@H@ zr8=dvUvzZLdiso;yTzhn_6!E=DIA{qbi)QSUr@F}>0CSSSS1|EkqRCD(6KuomZ;sq z+T0C-tBVVRcc2FPn@prJaH5wD*RB#?#Yy}uPG0b!`bc&`AXLZHK1WjF+R0gt@PAx@msTUR{imS4LaN-C^Ggx=K`D|*W1|O zXjxwHx23X~yd?xBB6DoQevt#7Dkrs*57izt6UdgJw2yJgP)=}Cm=Li!+xjlJzUp_~ z13-9L5V+sQOU@*%GA%R{1Ov1V5m@wZUWK@9%pMltl%?cL%ra zKACr=8@mknWa2aBI?oP{-`f=xKX*ohp+zEfk7ZMwT`Dg#?JBD&h)BTXE_?sUSU{3# zLxL*8JZdXXCWz27Q5_m0eC|}Y=x>%Txo{RUq%8|`FVZ|&5EzD3>h4p(7{VLBAKhBc zd8{69e`|LW{+Ww!W&>N*HQYK$9GW@{VZkpXwrGKhWTipqngi(NAf!9I1=US|t=SH> z7^y-Q$7{EK+j!fdJ#kW9G=|p6AXRbN90H-A6+pfE)U1I0L2~E#>6$@z$xJ)v2x>3? z+%WSR)LJKaA8*C%{k*+CGrTxIH*G4Milvd8X!G-#*_$z7e+qgzWXl5_tevnkTWeD| zr&X$v!RI1r))xK*8v1H`XrxBoN3{Z8o1a6!2BCi@0qj z*U-thr6b3ur-=*pNpRCQkjL4gZ_@^==Jrshr){N2vB}~Z`0jqCQ6A4#YGuvV%tA*g z`-`mSu^ik|rqm_K$zOujTrivakG(qw%#jmtz>iU=v775lB7FQeaSZ(w!JuVF_h$Xe zvx6j{)_vIZzeo z8v*$EAo`ZO&_I=O40Xq-3Z{)H$86VqkfTNJoiu;{x+9PIHPX=zv0fY6AMaj?5rPJn zyH(D#^FxO=rrL|)a?e;B@JK-U4)4iGb@VS!4|@O(%x^P$EjUiTwX}S)JQTNJLpdc4 z@_19`_mvpq(FV?o-x1(fY*KxkEe{!Q31~j%X-D*+dGiX79@V{hNR>;3_)QOnsmt!t zR`1DPyvYCY;}sCX0L|wx)25(Gwo4G!U|ZV~pw*0RB3#@@7xXpF=mo&tu!sn-vGH%5 z`gPLB%ZhPvHqN&otW;nlns$`$7>p5hgSVZ@J&e}Cp<=1|E2 zUI8Nf>+V(26c}5wfm6#=nwA5YHxOO=iJ=E*=1zF9MZ&2^T>u9d}7FiQ5H zsu2gwuf0D1_Hu?9is*`Ij0|%wruQ}cyQdwQF%wZf*LsPO31GYb8my7jdDs8NW6X=` z1)2HuMO;R6CAI{+MM*e4*And>IB-n=rY?+!51ef{V5duw?H@NNRr$=97{>8;7Coce$?Q8Jl z-5|XNcDFFyVoLU_u*k@I{udO;B}+suv+9`mTIDKBWKsYf$9c=jCW2S z78e(H<{OU`_HH}yM7DrK(5blrsz0%|~zJNaC!_;$zecR%29bY}H)<3bh@;p34qajwo6&hWRYlx=&nki|bMRSiuB zypoO1>-HAYKx5IwyVm<00{npk)3>^X&#uun6DR`07=V0)#tcMBp_P@hoF>6x`!jh> z@83hW_exV27oi0IyqeN)uCYM7(sXwh+T#ITty$bLffOU1j^Xw`{WHCf&x+-4&uX}D z&{N-!n7E-9UnnkrNAF6ss`I~9zM%o4s(+nA{8YUBQR6<_)9 zzfFwewIH!A`ub)%)sk~PQA+ig*FbRzq`V98l4Lw z$pzf?ecsA&qzy9a)|CJn$L8U2_Oz#zcu_nP=|8?>7X^K z&F{xy{f(lbxw&cKDBB<%orl{)8oT#xL-1LSFu|Jy)pM(K!<=Ue-pC9?wNqYK_ZisE z#>AS$|f^b%C7*@+ZVYa65X3^li2#-Ig5AKFZC6G znHp;y%H@tWnf8C&(+`bEz$buU)(g>3gn*U+a(t&v1Oq8xOIcMtz4Sy@eJH>OMjjpw z6%~~k(&OMczA5F)Ni)K;OH#Wg#}nFkrz_3Uh0XlgJzcm+Q1bU`B=e*$FSx;FI--H-m0n(OdPchXgssM5lbe}` zyT&hgT$N#qG+?g-=7NUo6CD|hruE))ap#eROxDcU0)y|K(^fAK^8w4u=OAJ%;+}}_ zh3T|wJklqFn7AA~buMw2DAg*KF2W^6kF{EBP>FNvJ{{rogYNjQ@$V^C6w9{GGWuI5 zxnEl(nIcy7FHH$O#*v%cyAzmH_0r`R7oE#gC5Q_Z#^Fw@YdVcpYa6u4nQD%bN!LqY5}=1F zuCA)@rM0xwygWVMy(0w{XpwL#Yy81`c5a$`Ua6JN5jU-{sQbGiwdhSM#?jyJxfwH6 z#2=%Pf0n8;(Q@Ty+D<1{9kejWw;??)^%_Ykgp)RGPhfaEGw_Ao5=oJ_hsXG#*Iez9 z9_2;WQW^Q?{fUzIfdjPFsmD8oW+uXjXu5jHpne9TQOqQPEwl8B6~h7**#fgr_dQqp zBlE_+nY`5?ECykI9Uhq?|0|V3m(8^`BvXBpn_4|@%2pX5gn^%#Xdon{I#jjfzz)%? zRcDPK6cmIgR3P(1!Rcn8W3JB5O@~;tl(pY2%h(nT3cym{#Y^i6I!A`tv+a6v5}flt zqm7c%8IMPbHTj|Hvp$-%#9m%0)N171kiC*K9*6OZCiYqPs???$hr)m<=E;q{P4Mc# z^K0akRzyEt*M`cOQR}4_nH|~+@|?w6N{QufgqX>q`ho1w3?E?KfO;QV>Gn+eUYPA= z3F}X;_k(vkAH)0Qc$)PK`;*L=v{jR4Yt@1@4HXtL6RUye>9x>IH>Uc{R|SBnXxxdDqifjZCR>^4xeP!hds&3KvWu=hmPbj8ZtaxZ|zW3b<}}Jq zOiW&@|@jnG4h>7kD< zp%1EU;hyT7^$lH1cU+54DIuQVbCt9;iL7u;{p?JSoWvo~w#Xf5kV{nMRzq1)R zlD0`xfk5w+(B}SrnL(@f`q%{RMeQEs`H_@Otq3kTs+Q!42SF>pRfA)&gI$E zwrz_{m#l{J(W6v|e(hR5OxMrNc0I2a#>gO8@jT({s~G#Tx9(rBFj0^48BpDe6%Y_W zF5<8E-m_IQy?O*v5J0lvniU}$D^=k=9UB~wpE04si=*!kt9Tr8t{@$#}XP#<^| zg0b(Y+2vwawlI0!yFwx>V;cOGJ0T<_1Y@@@pK=QD|F`Z(M1i_ZaPZ`R4X5QgS$(l; zr`7Jr@4!BLR~#Q;Hq3d*|JbB~_rCyd2^7HjH$e+LME?yqmS$| z51XWqMsmcOJP&h+()m`mltDjU`}+E>ZEr)uh?w~K2|@LN2V8s|wHT|T!Jw}x^?x;* zo{KpadzT|M1eZJ(ZmJ5?D1Awe4SF|GXE*11{>KOmRfmt}fEphA7B%weVV3H^?*-J{ zfQ3*<%LjNGy#tZB%Nlwuo+@t0NjD(zz3h0hgHV%~|DM+Ya*R7yba81o#f&>8956ls zhN{AVor=B}`N3$&r&?{!0Hs$;fL1sXZ^wAg3E*g#e$nQ^TROLx(}Lz4XgMgA3oZ z65PnnCZEL2-P@+b4EQQpOOpS1&ChGD{iM#8jLpIqK<+ehG4mqLxM3IvGagW2TKw2& z4Sb|EFe<`o^$JevV57C49fc-L!XwQ`^;igQJkhPlH6>MoJ*&j zbWTgU>$9eIVxlZN>jm6~{9V{}@B;F`{ixcxm{FN=zC5?vB;xbQ71p_~FPT~B@i$1b z5X?^mU#+cZ$S)G1ZH59)btAjHO(WWLs|Za(0S`fvhxD|~2DRb@)dM4VX*_%WCW!2b~a^s%w- z!%_9VKWUxt>($c10&dSyTm|bDU$PJVneqBv`G=gd-7*xPz=V1$0lE3T+J(ulS+I>oY*?28gB$eCUN|9a^ zfUn!UwT-$s$V8r$6{xen(@In=i#1xa*Wu^RUQ7ErGfi_dHA?zt%p2joO2uNg%{Jck z2C9Xc)ZNs_pO;yUlc0OAZcC>CCue3FO0o@-7t%^lr-VUWBi`UX=Het-ylOsfraj#)92M|GSpDHlz0&a_ z3KuF=Sn`81_@&v*$SqmJkqYZ6R`vQ+jgAOuMMtxm2wimwQ_G%!T%G+ zTG{9ITOC`QQaC8vO?+v;Psdg5!OURGUvr~Lj<-|57VHYnn=p*gmHSN- z75!2brKR1*#>=OK`WWnd1RxvQsp`3GAG^KekY3mI&sC}yE~|_{P2Glo0(Fy$9uK}z zIPUwM#Xarq=DYe!Qo-LipC|sH(A{iMT>vwEk8B_|^}mZmZ~cHOy0H3&I~D46v|+v2 zlCqO<6g>miM5OX4d6#J`ko)DO$|3^vb;p>wJy(#IL|aR0)sbXX``3DS9`?}Gg!^ln zV@mhHWU$SnEbIQ1ns61`t!bsz<}6@*8%Q44MS@{rV7K8Cswy)l z&{RD|_|#-*)Xv`01McJI6SVcN+Rj{Uc+=wLm&MF_$t65kaVr7k)bx_E*^A(kM;lv^ zZ*Rd*7#Wn#@KH~^q@O6xm`PV)9G&VQ&ArN9LwAW64{gYn?b!v_qJ~VrPiK>BIJcIS2%?={BurGn}wa2h3 zbpA-p?z-Iv+*kdT_S#5cVangWZJ%YmK#@|S-`%JT?di3>@5RpXJ>N@6Pk%l2bY)bJ z08fkaozztKalOB1b@?yG1l3S{e{B1UH@>rBh>%0@hx(fSz5{Vdw!Ba0g;ztkmb(Ll3I zIf6OIJFlKD%B1(3Vc}1I8rP|iQFy7V+R_$`*L$^nN^~jTYyJw$M%(v!kA+CVdbx*M zb?W;1<~plK`53mvRrR|<)`iK$@iHl3m-i7M?zNrJq$`a*h!rGxN-K6!Ep8|MhSdC)QjPT{@m*-b; z6IfFa9S=8{ue{vzg3wT@9G`SyKb>%5wxMGu^zpX**+b=u_3aZ08udn_yf%<2%X4OTZL|4V% zfCBZ0rAWi1o)Ms4dxO+1Z%H#Cw*+q+En6P=Qy2g)J8lGIE`>jP*p5Q=L^fZUG z)CcGC<6^%vJx1AIrx&&pQjf13aBmONO`O!g-dLQ)8V2X^LB^KZxXwcV;e%6awxgQr zywdiz%Jd!p1QUX)V*yThfX(x&C9O!tN1@#HnzPLj1BqQF;UZ>x7TwGn|)4p^Mn;H8ztl?~|MNsPhv#A%gUpgvMVF z(9SpG*lw}%5`N&H#)&GM2CahE{tvy0N53w(aG};;dox$GO+J0nxIb^IJIX^^ZT*?Q zNtV{-@k4XOy_tq$5uTE%oWRZF!}#Zn_Ch&(Pl_ym2S)N=U9hDhijD`b3gPdrgM!_6 z$`1rDtvUx(pnGQHG35pbPD2Nkcm=4B-ICF2F0SNFuGl@ctvCjjHG6>^g{+7%0X!TH zcy9tJ>C5sgN!+n%{gu?W7Esbl>aBJWR){Zr&0|g#96{D(gGyvUPc15&4NiUi^5I8M zk5p`pDLfWwfCTa!J4le@k@3mp`6q89JFnKNDo6V)n$nZH#H>`|34rx-#kg+^IE`ZesU+q zoZTAQ5T95P45Iu&XRZ|1K=mZWiLkS3OY`F^hN$ z0RtF>fQwahiBCye#%Tb*%hV;>%99Q7J`s46T@l<|$7x`Gp99bZ&x-N`oFJV-#Y{hCu|G%9t=AI~p%o-+Ujcp@x}F^}Fsh_6#O2m7coNb!LVZ zzb6TTQ=($DLu27W24KrRkYwuFNpPjM+j#&1*R3mOgrTs0)L$tyBv90*dP=(@WdM5V z8NAWg#LaSCbtqP6GGeOGTI^&sb=AA6s#@`7R7|6*P_3P|AyK$=?M}XsFx|s=oao7` z0uv3f_pTW%{|D^$Ncr7DW|)}RroX=eoz2+J+UZD2vWp+B9(ao zDSOewEmLFD!wWBWA^>xo+MIp*^0Zz?d+AWpZge+^jY)&m*#&{K!C{)*N1y9KKth&i zi6CkIemLudoQq*7My6lR0bq?9q z>wU$pjac4p3q&NE#zYRoSLp@v{wLQm0`{~Ani_jsWf?tdMSpN>&Co|4dkvm$24cL^ zbo#bVvyEm^-*@&9b>A5P;M2KKle@f^x@v*ySV~-xSzLY7Q!l-o=3toDs)4uvEdHL5 zg#xd;3a=)^X_fIFpMXEb!vi2@kOM?(WMr@TV16J%+xpd6W06WNHeo*9#~w=KHU958 zirStR2?^lRuVl^a5G7w}AMC-r%NGhEsd9m;UC5k z6HPJUiU-q$CX+2fw3wym4#ymLMcYG^8XGb@KPO@Epl$ptgmUV-y?o3)%28-x3oF3K zfukR8E8Dhh+WR)0aKxkVIZ;wF5Lf`oh`(2F^DZnL<9^MwljqF0E9Sj0$b8TT{cSTf zHRrLDtD=%QweM(^RCjF@Cso9V;?!T5Ql52sO0sP~O(SGeKV!x#Z?&ON9U(uZd2H6% z1-1{HT7O41c+ud`3a7sP<}=~k8ICr;U~qeKuGp_QH8n28sMmUtY%VNQMS&JqiaH0c zXI`pMr8XIsa9|EJ6_YTJfwNnJoMI&9LjZyQ<#SLF(aVuY z(VLI?SHHCR9X|BaJ-)}{TNLLz=9l=QYKBJcgpT|6=+pLGYTEporYcE5Bs|)veb(vC zo+rbt%q_+u3jF*T6TG-uE7VwFnskmvCoaG)T&8T+qsZ1L%hFOC&DvT|{z%#hZx z5Ura+pgK8<{B4&N>TO|Ab(tf2WNAsF*g&jHGS)H9H)`9|Yxy+XS*rZil&DsaP z1%d3ZdM;cQ;DAztQnAETsHRX}ZFVe_l`S)BG$Kqs<9=Hp&{guEj1C=I(FXzffzH@X+YhBxM*e#A2$LE7`Z*^q zJOqg-=sFpxQL*0hvJ1c^{yGVLLq1d59>B@WwJMVJ$;SC4WhWuIA#^-RF#n zr*6o5enJ<1UT#pD_Al3H>t8;zhIQ?d9;igPZSm&#^|Al02ltkd*znSn`1`U$ey2Z_ zULGXHlQ-rxw&XD2F3Z>LQvnk#kC+%?mmjxbBP02ON=>sQd@EC#jcA4IU>*74-5L;@&1Y( z(MAkKv)uFCPNUQc*7j=E#zjQy6Uu&b@p8oNjkZFFGoezucFKfW2{XR!RHw1^?skyfm`m@>Yr*0=5>iT^RU)fQ^q&t&I#bzJ*)%A zN_SF@{IY%0%V@NY!Kld|w70`uy;*U#AQp~z-X*XFk@|oz+o9SyKvhxTmQ&*)t;qR* zmm~%Wz+%<9dzRJt)PEM5rSe_`V3R28whd_>Z!#beeYk2QwMbQ^Jjtk0HG+)ek5mCf zp$pJr@dAyUR9Vmdxi%dU|JwKDly-$2IkiN&9><5e)!;G*zAp0C>>rsz>h+!*A<53a zj3qOCqr8boHYfsfnw& zq!U!&vnGy`aK>p|n~VBOQ*byEW?OdQF^fG18}726`>c%~FKYYjm*I9%E=}HK8Jpx0 zntyEKIytpj+AMEsgAGn_2)8Z}jc zSQ}xJ{Q1rwfKHY?^;1m>KGo0<2xpZzm}(4rFi{@UEKlMWDO1Jet3F(xS!5*k2s*I! z+FBn~!sE*&TRqG@R!pZ`Y{|nfInP|?2gvM?y=VVPRDb0LySfr0fXL_EKCR9W0JL4d z6e+b=dt_+@mqL_xaj&P)@}}JCLp>-0q0?CUQGHWZ z1x4loIj7 z`jJaSL}@xipXcGxBT ztNgFt>aKB5#S(*~ti{_ENVzX?SaxeZ41>M2;-n77M$QaITaR$O!29`AY2w9-1!VcH zaSr+XjosFBbZpk$IZqf#m6e{Ew2#gO#p#|m)r>|zdp0Xzl)tDwD)9s>ae#xz?hDrBKg4cpvE@r^xTGsZ_dvsYXo~|nzZ|or zh9UR)q>imR7toHO+N+XEh=I+v(NL{8!3WmGb7cB%4jsSYOONwivrK@sq3Ba}TawrD z-wHkj<%_Mm&}_8+$xtrui85F&(;(~L&~-f?Can=^ip{D$^sQchbg1LfrLJhNqHqBr zyR=ZN%4Ara*!0}|*RU9&_tNXu3agY%dJi>nyJ|b&yRn_=%2fnU&ciQm2Y=Pt31~(< zw;kzB?5bM4SKsZgPU&i2;t{lIy;M_Izc~3$jb|@wM5lDKb}V+#0j3h8_j2~zXkU&3 zm*uUAPV*JX!|^t}DF6qjQ)&Daa4V10B}x? z!;%4jLId@N0J)Ypq=|XWi#421WR~9}S9p4Ht4pxW>~{0X%5ro%ZY&H{mHN8;9oR>l zYCk3KJ=LPx(t0_>VCeagxT3x;LyB&P!F0*PC%1_fj(iEuKvMqfEcI&ieL|~4thJ7F zJ8<)@?am31G?Qa|uLB^!ZD^8N*u&b?TEN z>+M$Reo}?`4&dCn?@!f=&Z2q~f5OZUF1Z2@Dg;wz{CBb^2Ld#0@UCIim9&jXkHJTe zgb|NANZ~Y^796H7knG5Eh4E^Z1s69{%$NYsjCp=dHVy#L`S|f486fJN$?-6W*`jl% z%u-WPcWC1QkP=Y^z%N$A?hm@VEXmI=(@RKbxD;4{e0SItw&z#N`NJT7-yPZDP_MWy z-F311>gg>4*nHJ9B>+-lBfKsg?_{?bTHictto-Hwy%-+DI&QnIbdE5Jk`?D0wibx(>gs; z)BjDDtWjd}PHVNF=WWz8Gc#w~1KK})_+ZLGG*QD0nkQ{uD7(Hq>mMH<$K-Kje2d_G zcT2G74F(4#YJi!oP3IhbO*N=4Jt1G@_DYSNogK*Qrvs2kP{GH~& zlLo(TUfACPau-1U>pmvVWB=s~Er`%m@;mNzlemjWY8lNVJFj=c45gP#nzL@Nv&x%& z0ecK>)FzyA8sTo}#Ol5U9P>vS0A8W%6DTgYNGY zL|<<1IAA{ot*QT(RM;99I}>78HKZJ%{zO!-eemT`Z?&sjD~T81g$#`K+CzEn^tz~% znXZxveM>PegmtHMV_2#rdbCCR$J zw!OVBak@?`&W%b-5BFxQSTsSSJ?%S*iwp||p|3l7kl_(@3C=0?Jz0oRApuwFdVL?HzEH{K;%EnDEi_kBO0l)1*)e4>knlH#te~$_KrR;e9Ek8=Bw-*m z+_q_fvzvT0%p*&+zB>Wj*IFD#c%m+9`Q*gQz3|P^@`ig^rPOxNsW1vQuZ%Yw-dEGt zU3X@O@Zf=)x`Wg3A{nlQ4AX92ENKmy#|;?xaUIC#o|b4qA8ZaGm&ev;Q`|J}i5d}j zXo)C#>*?=kMk`h3Iy;@ICD67W&wqDA(p)5@2c4WXv ztu}2~#Iyr|rPZ3k@iAHe=r|8XjR7uYoN0BS%Uwxlanz^^^wh`m?7JAnRrJ@ z?6U_GT}8!uQocD(H2#ULCviG|{s*~N6JP%EeyqR7V z`jBb5^)2qILg0)0Lb!3)r;e2$0~4IoipF&QeVtm5>Wmk*=J@G&<2H``*8JM8t6w2~ zQ^%Ob)@dVMk?+n1&31OGlUdhaRJcEMNUT`eAR&ME(&O-fpjDOHW}?4gqjF16Ki74Q zkcj=up^C277l^lWrGKmMt5us18Us7HXkTioLMj*h_c!1~3vux^#n8|hMP=o;D=S@W zn)YGedV}bIt3LB`bCcOkBvW2V^E*88E>Sn?uqSCF0Vp0x4hxq+Y#jPUin8Ams#4qgWa zgJd$PtDP0W@hbkVpsPO>&C4-DBH%^s_xK;X9X6ViNY&nW@aQf0!Zr7wp>J!_;v>J% z(h=h<$B-5<;1hl;41cxB|~&Ef1f9}t@?Q?gAQJoxtQt{F+qo=9!#^$ztVyVs}9C;IKsHSI>-n;*x&9Z zTzJ+$kTyROC~&Ow9JN+`3EH%9ebwCcV`u3Ti3R7`;kRCL^URISHl;tKsqf_L-_%^V z-V=yS-%#JBEo|4%5`0c4Mck^mi8mSd_q!+-ZI$Php1C?GtrZBzH!n(hnaaDN)k1QwYQ`yF}M!nw>T z?0sV5AId3n)yTwGf)#vtF!&phxt*f@;ONS|yL~$FPkQF`!UOu#HB;Q1`Y(%K=|qOY z5*Zbuf_g%K*wnwC1`+?`!ys-kFQX{%DgUGeJVw;5)G=F(XX(0 zk}rJ{`Bd8-;#P9x5G=G7W;7jow4G*O>+@Bu_l{y$W#t8T4Hzj<5K5WptDWpHM4$mv}W6ua?XouOBmqw zX4TM2^5O#@oZ=txO6$_CnX>ySutDjMM>Wr`8f>2g^rV;`9Z_GH-rv8spRUIp!_JvI z?eyoR2BpY6e&qW)(Gs}8d9E2t)6V#YULpwUD{V(5aur3W` zQf*dTU@fl;{ydPA{39QplyX}h;{SBsh42*@SNurs>vKbJf${6}(V>Ymp0{z699Dg0 za(h;B)9P2Z91(ZEJa&iF;P31tT$DEq%c8*Ejc9RFx9z8aDG$Z{pBHgIjx?MWifN$4{l>ca;WXC zJ@aVj@^}4T9;+Kw=qM_^n#*MqfqtWKwnjBdIrg}B$2|tD-k!#M)mhvGm!rp{Z0Fac z{q#Cj>xGV%^f@^ak53iyEN_32=1$Um)Cuw|HdiUhr<79`S4$C?&O9GN;CNH} z?IAD^CW-Ox*P22SCZ2^ul(LqiEuOkKKYk?AooBhoohb-*A7|ohUv=Ggr*MfL?D-WV za%4Vak z_1*V3{F{np_aDO7@q03h507oGnd71sK4Z&uD4zNzZ7{_Hr#K|E@f&Lk1u&4gQ~quV z7x~nNMd!}C65vO6s2rvJvNa_dpXbmhFJucn^O7g)IM=dKb*+}d;-K4XKsP}#%}t7# zX_MJ_aHyRxWmiVjU0~B>iqmd6uZA)awcz2uVpFrvomku&x<+r1YTUT&*zzgrNgy3! zNBp<-JHAH7FrUK9vUwFwiy>3sBp1!rvV7nsa^Ps3ASukbEfluT-~Q-5XSEg;1(mm+ zlHT0hBpDu^oo&`m8Bo1?^-7I^@6^_;M}nZF(X-fq!M?2-n((3ZxzqX98~mSZ5bFz5i?`Obo|<)hONcW4pJGlQtZC?SO(u%pu|+R1oM0kx zq~bjo(jCM|YKXJttK!T_#P9p5U{}QeiW$r`B67uf!DQ}I&@!%p;&Z=650?28b@>U} zb+@xFuf1(9PURx#+rMWJCVhH$5U+&pkWg&|!HBi8l27Bb z&S@b&@U3Dj72IX}^|Bu_MpC#QV9eubx9fge?_pW=xb7>*S=2*?@~Qdm@z_#xJ0Fu8 ziFs2QG{QF1TlSr7wMS)J+~;9)hLc`}6Sn`tpX=8$Ch|Id7f{RxnWpIKeMxAcg+3A= zd1F}FQcfTFc!~*w^gI$qcUztWQe_7lUvW!qJ}nQ~QmDZ5cFY59zEzjj5yihnzN5Fd z7um?!skc{nBj2Nxu}1O)C9zwN0ssPw*Nr7xB+_X5ms}VgXLJ;a$YizYR52i<_cAI! zNe(-`>g)>LIyy@2nEW;di^lk_TeTTvMZZh#t{j$zjXI1F?c4`Yy{_>j;)P}5DO!9{5!{4DD5^2 z4yOm<=~WZ2r61onaKxqp*z0CfgO-Kk%lw3s#(rWI&?oK9OsL!1H!*D)cg&1Hm9J|T zAzs?6e{dySa$43{GX*bWk0yVMLM*e`)0C6fx6QOz5`nEsBrQ!F84E;WvhEc}DOY%^ z5P_At?}SN6(YoR~($elVRhMgwNbVlWT`t2`KaUvcCQZV!nlV`+#=_OHx9@QhN)S_5 zsyI%89boj3AHnTnI4uB^HQ}%GTvB+a4X(ovxF!feCZi_t;y$^aD`dF1_Z~TabB$m& zJ_%rDSowI@UBxn_+ab9+nuN%Hr%D0tyOZkX?#R`^C7|AQ(hp?x_@$D|!Pf42!O)ii z?NgJiFf#Y~oKSm$C8-7X4-;n@T#LOr81-#2 zv&Y#PnMNrfj)p3R3YF`gS--ZgpwX-k@7`C|Lfr!7D9~ZaRUks-r`NuHnX|PqCWZ|r z$I_|D@$_VWYRE9W2@BrX^+|r6fXU&#`lc+bjoyNcFk7<#*+u^gnnGV#M_A)4D^}CB zymv}OC2n%yeg`d=P5W93H#x^s13qg!7f)pUgkaJfWrjV6oN9?0^i0(<5lUM_%HooD zhP+ehd&^^wdn`6|B0NLmw_BJ6=?4r7_t(aYYXxFP@RZuB3tgFj2BfF2_Qw8Nnuv!>2_}W)+VmNWc&J{Ek_3Z=r^qli415X45}C z5*-r8+j2!Z1(qX5RJ^P!z2>6~51649g92#Il1X34w_SvrI!hG3$>Y{0*S{?NQWUVj zh!O`rLiX1AX(oDrBvr6WFxFhbZ`WUqy3ojZ8#{*9V2=HI41Xfz0;_s9hM+{!FGN(iRjq7o&0SCrp*GwhQRQ?mW1OqGbiPUfwfx&85ubwd48OX{>BHG`OiTg z;}FaBsvuH8C;SQ#mLrMrDM}2e>=hnBCq{}ATr5pQ{%f%Vawel4M!FQ<_9uNY_7&NV zFm6c?S!ONVs-OHTQ3;M{2tFPM+9unaH{w2nDpzo-ddc#Wo<@C5g|WU z5vB8m=jpxfvpS9|l!;N^(`3ZenC~C&{a|oCCX^A$y?VQUHqbj<>bENu+Zg47B^@E< zmhw47o)b7i`cE%_!0o`6b^<6qr|x&Wz$hF2R6_ukY{S;X$R z#75jMO3t|;EVw0%HahboZf4n*l)_J)b+L9h@H~;&nUREhBrq(?*cO?Cu}O^$n`0`?(Jfm*!5}kU9(!haur)9 z9drb*_BaDaf=2_yF9w~d88Oze^RHC8kbQhv+0VO+Q2rRci&r_W-vsPFLkz?bEnZm& zM6k3ONlK6Gj}iA-n?>t^sff9BxUN_iuLJF z(+e{9Qc)VU^V+SN}t09Zp>R7;#X@q$a&+Emv7I1v~?&SinE=TJ|SohF19Ou4`NV8 zVD`~=kI!BB)yD8Sd9w05weUHiha7*VpsFB$IW%lg9?s+qCAe{pTrri10QP&P5hyf~2x86ZfR zmT52^BSl_d#;l8;+^ z-O+8_Ytp!aOZ^WnoYHo`RUnQVLS##wPz$#uoL|?5HN@sVKNyA`&-`TSSWP(V3%0*| zCr|FXJUiS9i2-+o)m0ylD@zj?ZZ8sH!>3MkEC!lv+c;422Wc1k(Zb^&E8{ zE9$9uSd#S3%+7@mPw)Cr9#5SbY#e>Z)P5Mz4(U+@*Tm$Vx|4^nAhzXQ>|+2diVf{O z3;pz&q;bx`0B;lL-Wy4nGZ-&pK-!FQ>7=4tv&dlLG;g<0;$~;6beb{_lH~YZ;Mn3R zGNssMFYCucIIPy`TL((`06w9Xpyhgs)ZK1X$Y)~ox0ho#1{y`JU6EbxvfRVTHwim5 zYRwT{S31OnY@|%{5Lh?-=VZ(QPMN|Rhcb@Ym)e+W1~KzI zocyXJ5?R1RNTGz6gauz3dn(V{8*DZpsFjL!0PTxnwVM6&TDMCxfP?D-4sNS7Inf@> zmNYe&Xt(hdbz%GgQ8XC1O@Q~i()!U+36Rb8d-lH_iCSx37uDJRZf!cAA)wSpLTtAN z9O5PX@Zp0hey<}l^7E&nAZbZ{{9y(AI)*Ht#X5uFZYZAE6J25a%gm|bFCKH-N>?Vl zW0@`aXGjBn`B1|*=odv=F(1^4>_iddJmKv9BGGcY2DH~$a=Mw}Ikj)d;;f#2?k66x z(pBgGug$?mqv?#zUyZSh@aTse6SE`parZgw3x`8&Hx>jynLqsI`|jRj3N|Ni zr9e_*Vs=Wh%!1@k#Kh5bm?15v{H?+1lC{!fYd!`>A>UE&Z#?JdCN=ivOE`>~7BVlj zqGCbjY5plY;xX1!T1dTV?H7X*iE6a2$1BczDJN{z4A;e7>wDR!+GyROC3dSi66*gV zsf%24rmjCIs`%xB@=DVh3ke?oaIe`Z2GX62wYaLplJT#A$?D4xi+sV*%c^zRJBPWI z@H4_M`TRel|B-?<3Y&mHKvvd9DG(6fVQ!5Y_%+j&KF%b~ z@UsLmk3>s%=hm}WKiBoSDXX#v*C_FZbf96HVPCcL-dE}r9zLu3dbb$rVw{qDNh0!a z9gW8?ZxN}*3CeIE5Tcb%-NdvtN-%&$V5RkXn_e}v@6Kanv#oSkWAPM`PF!Vz$fFpW zaPsULnP;V#`+3Q$DW;ehnFGu*kKslO6OGTKBA!iU*i=)QmF!<)Jw(+%$wKoYNJNBXzU%iF8G6ww@8t# z9D&)`DS@tT_$298p9(x$U0ilan{}nCIs zc*0W*H^(#wHk48`z4HtRqU#XyJ;PD9k^2LRLa7(*{1wVT%Edfsaz>r)%-Yz6N=4Em zcO!e_=5MEFh0nyc*E}w_!IFG>5Cg@YvEZtO zj0aWZ^{cm4e;h%8`P7gP>?kl9^0`!I0@TEIm`&4mY@ylpuyM^L*k}6u5P1%nQ>U7w zxvc$XFkMS58Dk+l4$p1`62{1x0uOaOXZ=mZt&=H1Ll^-!=eoLJ3^W{%5 zZ*ly7GdyLH#Gg#d$;Ie)MT{|qHPA#Ga(?n)z|$6%*1iVl-d_#v3^@6Lws`6z{EJWu zE>{h9pO}(Q)+`b1wX#;b{8YS4YUsxm2f&`Co$Cl8QcauM=!3k$aI%%%; zSgQeI%bW40<3?!t=51ZuJ)t+p4Ma*U6Vb<8$XO|Q%ho9s)G8HY{^FNrKkl>*%{LaO z>1=!y<#5Xy;|$H3)e-qtF1SP6_62hB*i|lR|J{+!zXku3sJ^J2H-zT02(X@Pph&e> zefB2zs7)KB=NoOeYh%jjZ1Ya9-=q$pLC(6~m*Y_$g#MbjT%P(M4-|%Gk%=lp;DxU$ zSuA54MWD`~S^cmfmC89As9h~yL0bu#PZnDt9-2aJ+=M1*g35)yW)$B`rTrlLF?mUt zXZ0F>x>Tks8hVESXYj`YJySurOdj%=fx)?mKIf_d0yRu6S<~yUG{PQV`O=au zm?^lNKJ@XXxM1GW_eFidV|@IWICYecN$azX#KPhD`z`EgnYolvA1=n@ALHtm4YEP2 zKx`xvbYb02xHbIEZ^}1PNkJO+$-rX^m^CHY*ZxftX?2Z2-M>&MtSk8`e!tZyy%i|A789hPqqxb%~=>{bSRtteq4r@xq=SMFvZf5 zi*xdHhe%sSW5Yt@0&?`vVMQ=mh>}}K->I^0%dnuMu2{RyxmBKHY-3FzhFTF_EsrXJ zl^r@-Y%)mF?J#4MAKl_2wbB~teS;Z-M*XuF=18okHFSPP@AX#Ot?yWq@@BS-mf>#J z^)G2P%^)eH$P?+R)};b&myc8X7@Y?XcF;A)J;!0bHntrBsDY5}!BZ?#Vj0N^d9mx# zTM1zC0`DI4`kjn#jmCqXy-CdMKE0Gk_znF#YLXyEBtq6R>Ns%WC4HH5^Is6LoG_hZ zLR7`{O~jx^pLM*SK@#6lBRK22Nw z(X|pgfr7oltIU62rcENBYbO|}N%ZUas4%4*wZ+^|W_apX5+=0YXFEmzIOgyh3T}}U zf8q^657ea8s$@tUJxE;}jJFQcjnqa>vxwqOEtG@j%S3d&s%o%Mn~Z-*ZZo*af?q%O z;RbQwI~xudF}0PgbvJyrbh+#?`bw#og0dqKvj*ncm$U5osJYymJ`{JJzqP*>8(eN{ zGj$^SJ7nx5P1yLm=jx@0kmW8Kf!Kl+{@|I=IQ!j6^N1D+mGgXV^rG0_9rC_^P2+<4 z!vT)#1t7huE(L&@W_w)pUgMnl=u=nKpnBME*7PJoix!OvgR(1{mLgA?bYt~(?|Mur zi8nea%U!eGc06mfXmnm1g4>R#D?vVz{Zj#YUP{u(oIa>!2MG&A>ru$J#g|l#7o`H> zmt#{UVw~r$oSgPV0qEu$rrd`@gGR{hYYjg*`Dyacs#yC|XEUSgm^WG?2Ud#_8c^%b z9={LRq`dpdfi@PdN%?h+V>BWU}X`4O@$M_I1y4%hR`l=SIQ z1+*ds_2Mmd-n70ksMCd|5!%au4}021?7e59IK8l<3vkZS2-d6jVvdz*Egb~UoNq?q zb#=hcMIqE}?^GyrflaHNO(s4>&2cM|_j|=X=0mvFD+(>PE(b>@dw)7`A}`}tO?@E1 zWy@wmNj@)j6$d`h7pr1ej4p|X~G2veZC(ZEefI$*o(Hu0LsfB1=&ccO=XP0l~|`kV)a_%~0-s6J7NQ zmWrPz0cy-S*XgMmS1!p|PnG?MSNLkFuRsvD^>%3d_amW8eja`p)dlV?c^m&TnGEY4 zocbt=1J>;KJQ)PgJ?|KJ7A6v)iJ=gj2_fu6Wa-n5$)Dk{Z&Cz;n1&l`X_Jx-vad1}&2mqwRD0BpPdH zT#rYvaBYY`?Wbb1AA1rELUYigk0tNsvYM4KYQ4}RFT)D{C2$V737bSb>_UkUlTd`^ z0;H@e{um_v8K`vI@{Ie@`9Zu4qEH;IA^mWvF7n5ZAH_&t1h-2u@bRU9w3bndc?wY7 z6BH3a1^i15ZRda$HMILrHp@*>^H?06L`=M=-srGL%Wov!d)Qhy{^Ah#?`XJ`M@@{S zQtVjbE{SskvNdiM2E_UZ!*_rz%-=!q6=>n>>LQ_2wg9?QjtskG*}ztM)*J!)m{nYG ze?JLGo@9WM!^e*aK=}HD21YDY@xKA&qsdC6V|P!F<{Td|e$s0{u?YeB-#=rHnzcZ+ zKptIBul7A5VfxI6;GpGrIx-f;TuNd)iXPxy&USXd`|Bui{{Eo`xq_L*jg9y5)a*cJ zI}3!z5pkaGJi`OAAQM3Y0tqD%Fd;}~xd<g;sN{NaPx><& zp#B7(EAfGLY+;N*TarNO(>)Vmg^ndy|;!QrU?YWC}Z*L<qH z1U5Yt*s;h+t-rT+-uJ){MdfJINAXOG&Ch6~Ab9Oxa6A2xtyf$8a}tH>xt4jMKd5JJ zC4yI-jtW^rg6SKWf#jem@2ke*OEQq--DhNwZ<1U1wMdE9u;^!ei+(_5e>S9k{#CwS z?S=F{oNQAic=@d*j2m38E`C(fY9;+=k;oNCQelJD0MiP@QScl3Lv{Nw01$k}^F)(kuMhnxZzIt~HTjIv`W@{eABLQ9o2SYN*RGB_m)NcRo{=E>Y!lO@m zQu{Fkz5V}2=h$!D(rQk4d%>abr@N8bU{UA;t0a+p@Ff~diME?>NMx&MAl}fe(XbL7 zdtD7CsOSBOtkKXbH>4#2`A%}ad^`o0d>c-22Pj*9PH-;YtWN zF9(5Dk6t2H)yP_|oitW~+a91TYD-LfXxe*$91{btmAM7ut#F;JctL#-)u95@)aN9NwWL3>*La`D zD<~U@&}^<)rlztL>*t4auGN2enwLe{z*kXm9~>D;!o#z9J_alx&*SvCDpLlJP+3w6 zJbpX8e6k=6%OYL6^bTKCa?)bG-R7;IR!d1)v!NC%SYPxGO>m5zyGBM3+1ZgpE;YqA z8zAvG9L>d5Mo5@{&seg+9tksKO|epu#IP|r9U6u3-;(Yey;2HsJQfI7;Ae6>E}#xE zvqbaU#a*Rp39eW>$$eUnG$lxI3RukbaK2+>!k%ODEyQ2lT-v#BaFH05{JTR63QnRD z5|+J?-0T~z(k8x$n7ACNNbR4~FpdgFOIhDZc_jK>QQ4iJ{UM>%o zN~W_^uYF(TN)wr@cBKqJSgK8L9sv^Q z0E52P=}1H!#s-tXl&OeMx!ORr9anB{Ii~%h1m7$rjtU&~Rf-U!Oi#*p5*XQKHfkH@? zMqLtI_NS^6tVOJNL;b|^r6{J^iav-3)d@{!yM<|9-!X+vrhv*2lQU1Meu^KtZey9FiSKT+JALSLx|(#9@g$=J+;&m zI2)s;rDi@Uen#hM!u9}r$9gijpu8l{R|jwWwAwq+DKlA6>D;K85Z5mM6RENNx6Gs`lcZ z*DAQ4lg9d?V9}^!3F$#{vlbj-UD*gv$JZ0Yh5vmZ&JEKT)dgZwB}J6P(G@>679f9) z*%i&kSmnlu_0I+89eVYNV0Pk-9j!Ux3Irw%%(E%9(owH)xv8qdFt|bABHR!iB-% zOkPv@5*~E(TEo+bw_3RT-{V@@FO>efTlGuH0`5^!!;GKpx7q1AP_b{jRB%{KegL63 zXu0m{Xh}p=^zAFa^=y5*Kw++?Ej^vka447o~X zF=TV|g5K&dGE{Ow13Q-HFv$Z?If-WLqM%))-)M#D!%{Pon99MV%c?g+~RztWzw?O_P3W;cH~Mkh6#wT=?(Rt9AWO|zZDoUIG* zLKT0xKTk_gof>D;m-SDF$jgF?YDxxfN59{wlMEpBZ#TX<<^Mh14nq1$#v_hy4YX5| zXf3+}#h@{uK1f!SJ^Y3mDh;b=-b$z~I z7V}ynSSEGV_PaK}t8dbV>#5Ts5+)1sm|tWv{FOxml{MvZju5G@! zx7s(cL-dxoqS$lGJT>dYKZbpTPQWfZ5H}4?8!>IfmTRef+VP9$G@g zRJQg__81{Fz5F3{7s_c}lV$OP-O8$UyKFl4tm%^eZ?dqh_2OfFPE@J097>jy_`i^Y zKbxK-JloEvDyzP_x)bx$-}(oqU*&|fbu9dvD_HwYtF&|6i8+N?uCr^hXuLL@^_#z5 z2ZFTxsfC=y^%+_G@-_lW!fShk$^9nBk~`q&j>3Lj{)rXCVqp-l`&wXlc>gdtgv*Uj zNvQ4^E``;Vh5KsOX;=5 z;Mnf_)<+wTrjSeh734CxPADRu7=23**YioA(c!Hr-KB!lQE9$|Xk>WVg6e_0TiOI3 zeRldQ#MkYo{X!;ZZ_ibkLKqW|{HQniZqk(x75R1htXp8ksrkMg8k#OrT?WE!Kbu1{ zLl^7V+bN%G+_7kWAuiA^f_5^K<_4Y%)A2(ha$n^6DcX`72j*Dyj-j_g{KGj&<1QHev=h3QhAFamXS!h zdUG^Mv<>7vUlLElO&3X-Iw?HNAY-l75sZOkI7>Hh1zN~!DAU~>oeyuWyU)P4deem8 zTbq+k?qfTkGK}8z8G8>EIUisA-Eg-tUV?$EtU-cRQ$A2!QKA*DoSdWL1)C^{OB`96R5FejXKMqAWg)KvO*CY7_7{p1Y(82i62pn+{ubm2 zWtuBHcUG*{6ju{p8>`Rt?q^57o$egkT}`26#X8>2-A-ojJ+4@zi%;lY%1`rE9SF|R zsI7u1*J$Q^_T4?y6uZ@ZFc6@aAoeG6n2ObIaL-69t>jo%kM1TZ$8a#7oS;#Zs!r$N zznW;Iw{Ke(uOx{2sMSz9+`>Y_chu~^+nw)Tq!q)Kx}+a)XRA8$a3U^32WhF8zSu1P z8BB4DzOze9>N80|#kdg=qW|9rMuck#@&#R;D#O#-1oFI%FIDtjhNoYysJ(R8H{+2p zOO2)EA?ucVJ}S2!DiR9nsddH7GGrKv2pf9GjkP}dRyGuOd#%OnY8x{6uSDR(3`9(a z<@m-B$@Y&p(A1xv@EH_f4T*!Uiy+?u9)(co-R%VBz4%d73$Z-~O;g>ic|B zCgkAzJE;`v=`>|+i?~TQ#yx~v-taKgq*~(dn5`JmqsMNqI>xPRhTMJB!Xk;rtB@qM7144*mf}9Zlg&%{wzP6N2vIfG!l*aAf1F5 zSFDW>!hQiFWQsHe`MP{PO>VO^c!|D$*%X z@Hlr=Oe>4SjNHaT(yA8@m_Yg51X4Sn*s`9u!^S%zX9YfSu$ zH3v1azHMOYGGsl*A#BZgso>IqIYdSCqf#yDRg}J~vx;dNt>K5Oa^y?GBI(g&i&OI( zf7GbC4rdZ*rVlc>A-AfW4WKvD%~lT?b3EzEp{jzD#;|4rO@Vi3hxa6>miHtzRoE`G z7GFq2sQIugbMMAQ`mJC0OD(m^ob7c%x?WJSuO6vD43!Qbc<6I-a!cob3G#P9O~T-Z zctDJxO&<)i%r!wjWAN0XlL-f;**kYE)2y*VWX`qN@NGY@2;$kHry&zK?H=IY{6e9vW_Ej=VzE;Hq^P{2rg03-5ab&}Nb1DdwqW*c=vvyu zGT*G)R9>ioNQmpg zqmq?0akh?5^Ih+`3?NU7h3+vpZCu!LhO!0Xsmoi;y!z>7kt5v{-}ACpeK3SC2Cwd(1=JKQWwC=((SK<(61oKl z{JXlhSK|RRNW|0Zf*}I5gn+}w@oq=pM+JqS7~-H2aSR-JoC(21Ytz(%&&>)Pr-Tt$Rqg26X0ergmSlS*YTvMLV zUgs}No7EdR+Wa@GAf6L=31eP5%*SJCRqXu(!7XxeAwJfurw z%}G-P`7&8ti2xnnl%7yb{Td@H#kJ{L0EWK|J4+yW1GbF3e~Yx&Xf-J>a5Sn(xYlCf zqin{jCy6^nQsM4VbExT(ADlM&REDd*ItZ01XghFAPxW z(Q8qYr2o-HF^A=z);2c`0!dNGZZZA$eGTb=@NcgIz%&AtwJI*W_fZb22w}w$8_=o7 zqYAfYmFg8LH1+lFs4BnkF4JB7DyWg|4fiK!_`i>rXKkM~_BN1PZVVyO?lB+H{S7G?zAtgVNH&1E zE;57ctmP;r@r*TK;a2p9F2Kju)&9fg2qG!uZeq#9cpYLaJk0vWeuVZDT(z4$n|Chu zU-Bnt`RHXKqE;aS0#!gK!gi@)gE2UfBbeXm=l4@k!uf$UP2x4lTJu6&d3KCztFYKU zLeOS4tj@ZE`#bnJZ;5I!$~zWH7@AxS1&PyeCi zaUn<)n#b&P;|`zZrWx}niDfg&m~RW5A7W$ch=w~=?Brl)BTuN6;1RV;;VGm}11Jzuo5xZNbeubpjK+`4AG2P#Le=FMd9R5TL&9m^a`(w05i-M?D8zHc4z0ib`8YQUUCIe3$3qOUgkIJd$iDt^2-!pG zny~L|u4O?uPSp4Nyx<|aSx?^8$HH7A3Dct)pF-Y31v#Mt`5BxVwe0C)oSJim-U9vi#STd(7WYxiwizH2ZiJyh>wwg+QrNt#F?tv8B*?(&ur z_fCQ6quK`!h!2KVRje7fLvv}l&H~yI!#E?&)HR8fhE({KE2)CDHm3)+4+hsbLm-4K zVd!s_l8EXe8_1aDXH$194g!LctkfVSgp9RoY!mi23bI4O%EUc6&CZgPh59tvd!FhO zmN4a^n=KD7-YqwT^&aJvg_>sS*Sk3puT{E&ozD7h8khx`Nf=zwvvnhFB?&Z=@nCIx z$~|ou3nT6cqlC%DuhoYWptVfv{2Ko5OUop!P$lW<{60QHD2o7DJs+}Qz}D^(VfBSu zF~2WEJOy7nJOf7)i}hJ%lx%VJ%xWSFZ(VO?{M?Ou??>y&3^aNLl&p*sakY7J+ zU;BiD==65jn(9t^J_0U4eAnp*ONUFIKYo>$xlBx98yg#^q%zC8tSe<%J#_IaPdh1* zdWL?m5JNU=X-@eZ6L)6nS1gl&Rc%LpNHaT}Yx<-HNfk>ao!Ds2yz{sySaKhis|yg` zA(~o^j}AdG#$~pTpFa*kqw9?I=ds||os`B^UZI_|e78*Y*6p~#z?#B+&!hvnAN{xA zCA#&6H_E{5r?XxHkTc9SnUA*$fzMx~29!ZO~+uq`YCw700~ajM;01VkMHRul(hyJ~MDvLhcm? za|Bn1!RlAXpml9#)5lK`Uj-*4v827$68FnWiBGp!hfmb7K+iTTA|f#LUa{t|i6W?p zkB_z>6c9b17iX)Vv4GiS+GxM<)x+hM{8R1nZDN(Pm-w5x?cCO>M1InsFWO$6=cmu_ z{?nO|BmS}{7x*kNOsX1Wu?3yM=vaUW`{Piaf+q*l@5(o_1#s8e%I~&SRK3o z8C>TCFW#8Ox6l4?E2?mJTWPn}lp)xR&OePWc(r0yX&9`myoVwy~dut}UN z9F+~0xu`)%0d@l)zIi-7@9614NFq1vCNd-r>jaZ8Mvp%6|0m9`OT(*uYi}p6Z){xT zAb#(*%^L<7dK$F-|3b*uQijD?ek(unoJW2=eG6yL_KI=PqaDh zMF%?g)C5=RMwcq;W{_goaT>{?QjD1`zP7!d#%s&Zumv1r{&s-fYKbZt`ADl;?TIqO zOy(&%*JwRX=WLmGeJFGm*@S{sk))f=zO`~Eg5yqhB92m{4^8-VEV`h_Y0FCYzVQy8 zPs1W}xSt%Y9C9cVD(rTT%U@!*XK0Vj`0AlMXM)?Nj6YQExKZsd+n8gltDC=-R)j66 zGH=@1DZK==1Gq}hQ_Q|$wPe^zVx^AY_m)n^N!NE84idR~_*pz1Fy?(SK;6MNPf;2F z{(WzHs4$b|IBcol0`MuMmm=Onvfv2NL_@}9ev zvZ1FOddad5nKqA8Q#VKKNt?0;Q?e1OW)rd5Y!Fbn$2JZvDLmlnbo5FB(7@(HhmKAN zggj|7jU%98MCRkC9uvMo>@uoM%4j~J6XT`PgJmA@>lz79n>pkZQ=V&{(4}v>uL1-r z4+xkQ^$*XjZ4X&fd@kE|lN6+>s?$c#ZRt0ois6F=Zf2$)YINunR(S^$&7qsY@%bIy zieWCy@3@K1?_96S5;~2uBWyNmNzyp|ek~iaF|V3_VFT-@{QPW*1m2$O`?*Rewtsb}ecq?80-@ngWSo|}$)z84-zL}ZW^71Lbl57b8uwrDhW!mYR zeJn)}{~YH02VD^;dk`}-FudjG_f`kGl=l*F-u#!859&vSh36irwN$rpbX0u#mN?8x zu_piX1u@(%r>|ZC*8#ku#+HRRdNqL&w>IK$*7Nd5COKXrYrYVzae9!yKKwn{Zma>X zim&5X4kqDQ%bIV^j=F~g^*?L!eFgfT*9*XoPyn?GTK?0mJpE;tXoxxWrdzAd_y;#0 z@*Wa*Gd(B9EDa?)XO&?x^vnN#rGAn>J1!}+DI2F_s;iVHL!#t0?bY_-E$#$c_z02S z8r~ht$KbX=W~2!0Gq{gpM|8~dxQ0{h1RsJEq!I_@GKioccx)&2m4)O3Y@(J`EofT#Zk8lz)uEFJi*7B~<%nZdGVH@lq_SV{jiOFs8z2WoKF zw(AYn+%;pqvZt>H^JdCKa%auzc2VlwF{OAoPwmfZ^*FNpCg#z^mH|R0U)rco7Z`lM zq6yQ7r8zhnHnbbbNaoT=I3tI{ILp(*K`W+w$G8DbX1$@8I1_i_*S8i1@OGprXCG@3 z%0xfKA#fNEi+A8yGr2#9bHli9-zY_R&!ys!fq}LIb*aRfPw0!`kFn)VL*+D}d4>w! z8HlQ_{Y~+RXesQ*O^2*lHL+d`-rgMXQ5R=bDKreE2%7M%$C-+V9V{?E{44e^ps6xB z>~n(wQRp&{uLoSID;uV@cq*hjLMQ7(Vh1)}xB5niW@4$(nl~<2L?QUeqava7yed5D z#YZI?4IbeL4Iv77g0SL0KMDk+^icjjI3Lzd`}=JBS6(_mo&=_ zE9m>&d=o9r#!Yn*5c_BhZnMOix2sdw%~`jmzEn{1Zgt8x&X^1sQ;1Jq7v<7uv&3FE z2K<@B36UV>oG{VlyKCZeCtSm2yfTfZyC_0Zb{WejOe+?r{x zX@}<7=_9>Hid1|Y$vnYY^i|ZBdPBW4a0!_C&&!6zYK-Y;mX+m>E|6wdT3w2eMA4}v zxZ^q(x*^xeBlHIt=UM~Vk`^-oGDxav47=PFAkRm4r<0{V!1-wY{@$2n#46Hwrj}(G z5LX?(b|{>Q>OrDSD0jgx%B$`<1F@Sf8=|4u%fPwc7|5UaSNweY38>8PG&*%agXA?K1y zQkUZRaVFBx`%tGZqTUcpx^G--=LC|LUWm;#Qmo^?U6e#GQ0?%CZ*sDmv`b!Dy4FK0 z5nxS+VQ%cL(HwR;5E|Sa6Tonmv13c@wH>mwT9K9>h=F`PLSD7EEmrF_6Z_eCcf3Y} zZ_q)rz2bhTbdzI;@{{zG(5QAL-^R_=c9B9d#Dx2N1${sQNz;P+5gYD38j)WW^XTX( zhdYbW>@C*l_f)YQ$K$O&1V+5%iNs_6h6!-;m`TzI!m`oYv7ey7XHRIF?G~4D#%X(r z;L1&)cj+$!$?bxQA&I(lB-`^t)>7@ah6@=Ghu8~0^R}+Y7R~;ivJmW6dRdSJB1vTn zjE3j=Dtk=Z%@nNWI{{H&H;3EJSlfT9_?W#Zx?{^)8YBwvV2SMYQ4LynraW3K^J-pC z@t|C}uoP!M5?J#;DmXnqxF}Nd6GB8;nC*D&UaUeR!`C`s(QCoPfY8psAZf*N=Kz^q zxT*;~rdw1%l5?13sq z5XuvI=ux?0+1QzAUnlmfr&??M&=oSWlaJ%E?+Bj{zQPw`kMGqzZS#7>N69Bg@EfxPzw_|2)_C|iA%DQcbHCtOVx=`eSAw9zNag zP}y@ePk+Ot5xu1|T33G_bv$ZDs+mFcd1HyK<6?d-T4;$vj#mNY|FHH}adBDcX#)~3kh0C2=2ihf&_PWcPQN5-QDThcb|Rkxwn7)(B1#}QtP3r)|zw8 zImY;gd}9e(iO>9e<18}nO9H0G7DiTa`+9dT&QQ3GFqBvF*MIhwa-|b+@i2FYOo5peAHNf+%FT6FwS9d4%VuH!Uot(KYVetRS+OA1s z&fEZ^m|FFxAh+7B$MGNu!VbsIb;Hano!4evYrfzZ+$Il&)ly$}>Fp>fxn#Jl=nm9$ z8rUB%qaY_Tz+jb+b_9^)=?akR%FwxK5C}LvYf=HI8B6gc5|#QJlX=jXTKN5DatylW zVAh2nHi|WOc#`h&TSb|&eq(RUH<42`ciUR3SUFEaAm zMb89lo@^;a$?xc07bU0xAmpy{jNNirr{v31sWNT0_4Iar6sj`go>X6&%zr0_6sG@` zGSQuc=njv^^(9*N$%P8i__orSKZb$j7>`JFhY~zrXJ~rjYU*C|Vi(WpI{oCG_TH9P2 zg@c2mRJ92mxS}o7DjZ^;!T}9MzLL~SEb*fLw^bgUPc2w@u(ubLnb{7RAAkPww*y)9 zXu!#s&>O_pXdP7>m6}Qj=@IWPjs6cl49?ZRkOHr-hZ6BOWI}oX>OcxP#}sV>^0E@D zqx!ol1N_`vO&kQD@3AkFNSy8pphrk9uoDj>Ho>7*g(&f;5USR{c>!?7JRWfcUyX4e zK*+zpMfM96l35PdTF(*l@jw_heFXnNeEZ5#M|0&)*ZUKoS^U80?X|xR4~mzt@=;`D zWa)xstmr*Ifggjpso$ddhuU?#*Zbv?@t-O3PiO7gQqL*#gh|fLd$G_?4}uqZgF~I~ zn+CqWJ~ts~WeB74?+##4dDp~qE#BrsZof3=cr8QFat?(`VrS&A_bv`KzNP~VK8Y4u zYj$OV5I4EGx$);f`1)*^RPtn1OEl5DnK3XDE_@VhLf#&B!Kt?Rb$=^!esYK^zw~4k z(YECQ{l7#jSC&Jvk?&z?Kdy(w${~7?^EBNHXeW&MX^2Y@>G7X45p>bV)_t`5LF2LX9^I0$&pGX&7 zLqmfQ60K55Z7qeqFDH_20lQ|_bWoFqnRU^Ee#sdmIotqNi z0UO>E@dQ=xJ`xYXljVJGSIkegSQ^_2cWxJd6oQLpQqkEAK7a21^19L<0DVeq%I?df zqawI8J}o{GlG_r1go{ByAidV+<>2U8<;?w$f0nE;@~>?tH0cSDkF_QqYEul6j^9kg zrAh5a!8Fue>rd0aLAV45JBb4k=R9J|UG$st$v7_Ol~;jU32CRJ+<;pupMei`qVFek z?te>rd$RIvtZxVU93+J6f+Dx4-}Mk_j`dxgvXk7Qi12Z2^?F2Dy!1VJa#q2*5~JYv zCTw6M48%a~g=^7jZpoRSwL9#6jv`tcx;Sf6$uT8v(KDg{T8&d6++5j?f#am{Il;*$ zX+FcscfrBgfl^{H4_VJ4rI}0W_MwK@M41xM(p*9nN@#yX}H2GTHj_HBIR|s1WLgpeA?f zcU01JC9})Bx!uY6-T=g5H+2w0l#k$9bad#MLcVLA0p9tnuDnjy!^sO*lGVvb zB6;zaxwMDPQK42%@sc0qEL?a>`_Dg9PnW{GI--;wO(XX>15v4F@JQy*oxKtPXoi>B zJ2iTLl~_QUq+j%>MQ*!}NDkJ0q1`pKx3x7mtVtW4IqcS~a6zdC-B+Hh!ZJ(y^2Az=poGq1`BEq_IAET$|AGo=XFx7QZnL!hx+MwQyr=eum%bUZ} z5K*(P)#0{Y;SXVfa~dYoSv4Ix^r`E8CW$s1zWeF!kU`;OZ`Ht7nAq|q-lUVv!9vnQg3*@ zey+_ck*UKS1t8XvsD62O_GCMa9{zobocYvXtj$XieZc5QmUW|N+(FY1lZlYCjkx@G zJaV`^K7)_`TT8L!^0tVgXGZ;Pc*c>(=4Z7_HpePIkRiy7k8kUEB1vn{gd5XkH!D6k zBy+5%-kFv!iD~WDXyuxuVZ{ZbyQs%5>3Y7N`+Sa+MmfZA%+wqWEI3V6mqMQZX8i5# za`~}QXa*7M&&;&fvgf>D9}iCZj4uAD_bt3u1O6f8tf0NrrH}SVHOnA_zF7?_IkCQ7q>xvQWtXuG)^C zmW8AwOy;v2WLXYvD^yOJ4Y(^@>C$t>JomNv&RG2j5!LtlKa28sS6i`BiFq8{=2ZT|;NvIVv_%x(n^^IMXWh4Fo{rn@4A zMjr6RVB+*VPG|z6KTGk$O=kVU;TuepBL~C}q^_kaWYE1vZL;3xpLa9z-s46;RAS5k zUvKI~esZx5LN#@q+_(oESPB(J8$J%@7cs9+9*m_{`6G?}P7Pw}QjiH27MlJSgJy6r zMdDxbki-Gj>fo&Jmq-9HauH7uF|dZLKS^-{x8I`TgO#3y zvX_#4ES|xvDcfVxVA}HPB0DCynu4wpDyaxti4FO)aSto{M=rMrYmr4|##HEy)JS{! ziKy&&%xVzpcKJK2){p+}LD2QD3!TpnVGV~fKYnH3nNdd2mw38<7Sgg}D=g%rzCDZ5 z1G;p#SCEuG)|m)zvD8?YFz1SxIhr-}CXX|>q1~dA(a0E z9UwV5SQwcD*N{NN*qDKoTr@i!V^*?@&ZuA<4_f-(TjtGhmzBcbUJ>8ojpS_{9c6THUvIjsVwk#MGx+ z_t-t)WfI;*Mgpc_poo#10v0iKWKM<#A%-N~5oSJng|0f90Qn)b5Tl%YZqA(dFmW*uBgjJv^Hh9o< z(g{L)^4FkPfOzL9Y(tC3!%YwC*=#?BLBd%$#Ke&hKw8@U5nZbO-?bD`B9N3H4|)1sxJml5{56s{w$6*HAT9*=FgXX5HEYclteL4sqXB7RqiF z66yY9`~LO(Um*Ep(c2Er&b?z}BpMoAUESrq;Pmr+-n>@jf63#&rLwE`_~XOF*4|z@ z1dgZm#3sRKEZpCY*--rFOxQm|X5#Jr3VAdNhT|HH)XU{i1=Bu0H~Tl0@;=${R@&%2 z05OlFKcs0Nqh8bX2sDN}vDsh@EEgBo<@Vs!zzI8-ScU*tA$pp&(dl1RCfiaFx<3AL zn+au3+C1S^sJ<^U6re~V7-Qf-iY%3M0)ypLhiC>@v&%`BDQh$ra8=Z&tdZ83FC~+# zP(|74N(&JIq0;%o-_y9n>eVQqEu=kAlcr*sgTxBECVY8rqxjTPAoFM|PBd7zEqEd*Ep3lGyncJ+anl?`nw)>DuAVP1*k zVQwRBjEAN7KhBL@ABf#xvs4EQ30XcrKgUnUv6Ej=>_X`T#oOHG1wDV|G8fDE5_@1& zeW4Gz!C1y_zT{!ta^t`JGcl8pKsDrLDcIYKL9Xa4X$xbDi|Ot6@GBtyYne^J#&TO( zukXwz`JbDi5we+;i)-c~3+;fNa;F}L7nnr{(Y%sQDik-9&;L|NyT3w$kGVLOt$%Ek zk1?BXic+iUa0V;?n7FwFQ2Tb`B;&-kqa`eqrw`7u zX(I}mF%42NXL7`$Q&@5F+JDn_uUcH-6_%oB7g9<1XT5`SDIv>L_bWqth^klcHVVXM z+C*!u6TK8Tk$ZwGI@OV#+^+$vlct+d+h1GbB24IF2WP)Yr~H0Mu7Yg0zZZa_6k^l5t52H9EaWC(>frmR zsE2(?z>9oqi{9G(2sef&pJ;wlpjS?N@)T)e)BH0D1-jcKKK@1jw_+qigSiW}PS!_W zg@sPF;`rgU+ODNiR!r1h3Zf|wGr`gpfdVcL_Y8&p7N;VF#r?j{En-7!HwR9?`S4{WPjmT)q~gh zNK9-D3Vg-+cxi))CO2n0_0142X=0SbaSL*ZNcmX1`S--8IK+X_)Mh5%TF}+Y#sHs* z4pZgJ@aZ&d3bcYNlVhUo=&yjcEeM;D=$Wwvt;RDi)1HTL$;2^%He&sA>-|ZcFS=7? zL@b|5v#DK+K+B-YE2Ve|uUrjy%<@r}T`2P=rlm1?nm>4zE8>=8SaD#D@NOjeVn99V z7o)*>fBgO19Yb2mTyoVz)kg9HwnDq{Gzi295A6B$wrk$UQ7k>^xp3h)9A2#PZpVOl zFr4U~&Hu;@7=Al7KQ_=-E6HQ}S-aF}rI*}p7#&}4y6RtIh*%jZ~`z1O=Zlnb7 zo*<`QVvATg=+fm$tNns#wgC*ZH5i*MFrF^kKmy=Sly(v_I9UwspJ`NW$Ifz>9g7Wr zYyb5sbV@e3MwbH(ZtgW^Hpiz>tFDrLFe>41&;03V1EvUCaKMxSVw87Pw)9j z(!TL>xG{2~8dZk$+G}OtzsTYEvCRR6FK4r>D#Lgy(4-+?I;%v5qPd?l=i#0l%L~Li ztnNL$j?sUAKXdDO;cASF8hyRq)+jZ3)!|`|YyB3ScL?uvZ@FOHhOY!aT;*GXe~Ed%sULO=U0Ev2+OzxuV zw={Pd9y6k}%?)2FB0cY)PUe|oTg?BZt|dKi4bt&5Ju%!d%F4l$cX$ERzETEkSxT_>V;LbHPd+74&p!{BX189-Ks`xSWK30A#xZ|i#7_PB)WA#{ z&qixJO{gxjPrDB8W;n$S*sIOx;u%nqw`5~bnQ-30I1M(lx7egeSZwWzGPwDp4fjLY z6XTZfPaos<9YSs4nQ?3)!(eV>@+ES2aPxZtyjcr7J%R2lu+xL#sssOFOE(fDT(nN! z<-{)0_3}A>Ic_cdkcx2^sdwpL^@pu8$%dGa6Ec(*g*Zc}G(=2?B{(*e`M9N84E@Eb z(IlG{iaMd{xjW+7p_e#NcD?MRHwW zd~;5L?kJS?yp{SXSoB68+aW}=x8J~D`7nJY+;%StpGN09MYDSSW{{c0!78>k0lB$eq)hhs4&Yrp94CIe!>)QkP9S5e?2)Wb zh0P6RsI#rG{?!+pvj-BTx8wZeH7?h_>b{R=F58*(>c{}K<2>_}RN67-cSk&JcKe-k z4o9-6ZE@CbEMYYK6c^(lp$Vx;rmj*NJUFp>6eN}`v`}~GxLQVAB!UQx*vFO**0vF6H zFZbH<^y?`0)L%wMN1T(@1TZHgUX6Y3ZBbF^BIkbjYxq$v{shkBGl;kKfI^2=a{hWj z8L8q`yWfs=jK6k0nMhp`wB@jm z@KM0L<=RT^#+~ZD(yhV6bNP{u+oo@SbE(D<7~nM6@XxUtxFW5~K7IF>Sp|o)vEB&{ z$#GFGgjisYEj=>VktudVd8jVL0R=M-G_|}fy@9^9mb?{QJHqPY#z(0&o=kDSvP>R0 zDY%mDY>k%bp?Ly*jgro7&oR_iPsXy;<8 zLY7%>2I(^_^{3E71OG(Vu%D0X6(;zS57}X4H@K{`Y zT4QpW`$sd1Y4^TXAvl-YTSLuCO%!8PpKn1LRE{~lG*nRx3aI(^>a9y@O3OF8)AK(t zZe8ua!-TZAxkEuQWd}BH& zXlNQ7yCU<rp6?uu<5maGaoW6>)0JXHV^E~oljjl0LVD){X3t<27z%6o7t;pA)Y z-9En$@l`bwdCQ@q_BK28C=AXp+R?28tBRo2W1+VU%)3~DjK(nKk^*uVug;nr4$BL9 zTtUX>pKW64=Lm>h{g34ET`l`lJ#1nb{5F~O6uqnlW=nmoqO%Nsm_Hl>z=XGk;1%D` zT$t;Ug&K<(+|D>xI1QI`kwh)*C9L@GE^#QCw@_SO&gi^9(f5{NN&#b%Qg3OQ(Ts)f zDwy)P&rpuPmZq71j+t5@Vovv({0&yvNg@50wzbUE9+&2?jq~yh5fs5xEd=nX%Y%=` zD!h0KiHRgpT|su43%zKuxK3YW1nA&-(d}E_MY7>s9b6mgm;XXZOi~>Hk zZKr#I!Et8GH}&&QN7C_G2IEdsVd0=Bw!@NRk?3{C>w@!7XV;by{f(#7Xd^!lh8$2u zAPsyxeCx?-353J&BU#+LtnW0bAwE72Nm(){1yAl0p$oml z{b}vkC-m~B_HP$>zJB-F{$4g|$yQh9iC^`f8<3dz&~lw5^>s}c9k-Ln)p%#9%ZYy{ zXG5%?R#Pz8bi~hpYgx_6-8Iaj+0z$``O98&h!f4Vv_lIKIgcml?&vbdX%xBl$_ncD)1-A+cW$2o+J| zxChq&<+lS4TOKC~%Za9sxV%Ti;~vn&(S0!;QHWqj8 zol;f5J#1#)hy0dA+luROnukzN^o^Zh@t3+8+d(pjGcJuMAEFjF$MyvFW*gbP`d!Vm zxCuR_x5gpi5sk&&~0h+5n8xp4(G_fw}(wEm)JkJsu&|g-ir?DA;@j|7#iZC z!rY{cp^cugc^)pcu(M*!@nc(F^eV4{XyNnjVM1v6z`PuW(}CD=r`N3R;0qHT6_0Z= zoJKUjbP3x2^#eA;=<4>DrFgQFqi9G0tC*#?AZRc(wJ|0Q{?0QR;xMBK^ldcc%JaNr zf~H2z0dpmW)D8bg)!_U|0O-Crtvp*4GICjafemcn0ph_0>M75K16&BE1i(ChV^XQ& z>7Ta`zZ0rx`&H-pIvOlPjK`wq8O|Xz#|0|+sOrTteOV#=1|#Nd#Tw;#kgEKUSKa?R z?1KcX(L(Cd)Z#-1s`C+m(e4ufi9AEX^UwCzU|o|^K(c%H^TGbQoxZ9EEf$>_-kg;r_28U zz37@T$ime-Okuby{3qwbUycI~WQr`-2%`eHoW{$vOp5}Hf*x5xOvc@ zMx{Ac>99*A>{Op6b3+v36V@{u{vpzKZKP~`%Cu-Xy6NFKV`2Pp`Oo|MuKC&rv|AE3 zl_EXNpJ|zs+_dL!8cjysJpbltzO<(zA_H#WJ;QB*;C_*k#gX(!8GZ%vUB0sT(mC9r z0y7so(gsbExX%6cy?)gT?Rp;=;_KPy~ifE9p1>P+_5M|3G-?}fXO;h#!IzNBPlXct>*N& zT%Mihm6>b+@S5m6EAW$Zq}P%9)t=T-^#W6`)`P19+D|8V#nT%S?sN-o^;fO=M9%V? z^%eZ3#+3aBdDB$U)xG@Tp$CKhDZl`YpiWDaJ-wl>(aY1!U}iC1XQD}1o%RmR&!%UfgQ}l9(!ubdvW`8H4i$Xea$}~DYq`vWZl)> zbb5fxLV}yf7XJ3^*)o_=&MrrNM)hd<+!n3RzwHV)e34v3qZ!P(mL{AC^)E3M?MBsN zg=YBt!-f4rq414(Ia?X<3(?pKz~}h|dyAkw@~~UuEzabg%a3O7e4m`49_UE_s1TTN z0Xa9&Xqg_eJow#%VqTWIe_$S7yao@`(>=`&ullxco>T3cM?QPbX?!nN`MKP-T3D^Q zADumVIhTU{>_7!;C#0ot?yg6|&)-5LXu5>C3KMR+*N}K{J>htp5kZ#Ys-9*iOl1JN zjVcsB?SgPlpO*Vm-w{`$cV?Mkx)+OKhwcnUDySPc;+Bf$jCN~Pym^pOw%VKA5zJ;H z?oIltJ>QO-iVjs0J*EXdcwSsy_pr&Rdpcof>z-)117=Yf`TGQ{WajU#-5%hVvj?#X-y~rl%Em3+T z237uC^S0r+nR&AYugqJ_RJvyXdhF!y=nB#LGH|4lB4}N{ik;aRkh%7GpqcrT4?k!v z!U0Jl@wtaM%7gujYcLx9!@7U0I&Au1#nrsHlIY za&@h74>ZuNLiA5ebuUBLX&CL}I*hQVz{UB9*c^5}rjR+EVOpw`ih%+@zh}YQ&VdA# zMai4D7&!?VKflw2<-yink&ii<#)UZJ*t~(`sXHpcRWHM9F&|yoK$y)av1I4-tRbY8g((&mozVR(F!b~_V zx;~%3%z2J4$9^)=nKB&!R^A1exUg0p@jdTe5U3=ShCtGn-XhY1G`Lnx#qKt1Qm@ds zjFCrB@$Ij`G9g7^HA<|g;U#v7HPqNM<>)p`Uy0LkXA-ji6sr}aBFje$9rccqB@4XHZaS)LB8 zKLpC057c=>RNWAn?eunq-QcVu?tKM!jvJhJkbQt@!~M{NRk!PANVNiXEfTbT4_#)3(sF(&+^orrqbU z_SR>6HMTr)Yoj`+6eDC~PS;~I{yb_q8e>Ihr=|u!?P4XmFNd+bYH9TqULwmoBdP%( z>UB*Y1}>iUf&6Nxub+E~Ob;s-h@vVt-CxOYmfh;)2u?m&+_B`$s^X-E<-=z7Kj=O@k6~mt~Y)?uR|s9%cHzxW^}hTTRMa6HGkW ztWfucP>sCVGK1DMqZ$V`yn+zI*o|11j1(y5Mv2V(le?cAt!Q-w^@f)dUouy3I*dCn zlhn=%FPv~&LFi#<{f3T(($#PaEIPtGFXgbz!S}YTBtVal3$@M1dcs64K?ed5Im$Zc zTcBP~s6o*))h}8ZT50(Ci=`|))Dsj+M4z{I`)TXrfRk zqMtMxldy~nypKWl3<~XH^zL7F%gWLGfdI!bbQ>@vG?`XdBUg+*`bZjKd3EEEje<$V z$9HVZ=;U~(?M*uo5k2@UE5x0cb8m7n_H4qkF=GSg<2i$ehWQoQC&F`NjcZeD^y-WU zNvXfxbUv-_d75XvY^=U>XNMfWnUpB($emHgt;Y{>lDJv{x{nEHc*OjV4WvxDb@*N% z!6P=Ft90JXCptvPIqO{SE|B;sI*JO{^AbDHzU;nPfhFyoY#D5UF1>%u?bq6rIIv>o zkO_75{t4ap>!yLmFgPs&jXzck#!*X-v%>cQNTN8Wef8F}S4U%450@*u_pdkTwneo& zM_UPiadA%40RV(|KE-EFAn+Cr&51wh=U%lUTZD?gFZ2_ z+kM@7Dw1nor0!I#X9lsG+x_Hs^UZjGq5WylYs(Fo)Gj83%$B-h|E`rt&?TugHIdb1 zS+hyqQR@3xk_ZwW+bNPD@guGEmJD0C{jJv>s%;S#X(A9J_+im6cke>#G+J8UR0wuQ ze+K0XafrWu^WULZbNwRmwJfG4-91S(j8zu(9xfrrW&u&Pa3y?{WYo;j8ZFY3FH!D7 zOt#PL^|=TaWq6x16^M$cX4O4Tg65=fl5@jfo$}D5zU&sixgwO^+0e)2SFFMhbZM!6 zjA?fJ)Fjtlu1sI~@iFZU;MV*MHTkBwmnK+g`~9^}0j%fFRtT=l;# z@tqYEMBYaEoSu@k7XA1<{}r|}#~2#Kq-hcs+E>=oQz--wZIeeE;_8R@r{`Ml?nRdqz_Xo3Ss%HjDM+aUv^>#P^ijrn7S2XgTt8cb1 zFqGK)*!wxOeGV-*!-=DX3O@1oD5g}Xf+$+1`y4yxIIQvupz0o{*!|PhNP6V zd+}FLV!CDa{pc`GB7(%Ahtn4nr8Re>Jr=)eo*$gi74ew+6D6GTxAG0{XeON@07@aW z0CH&DH%TCvF%OJHDn%$UKB7g$PS7{`pjW8b6J!lM1H66F@wm2)M1PNSvU{%P3LAvW zKzjV=v@Hhp^{b&N2h}J^fPVizLf-Y$32lfJ0)Hgp=9IYjXDSEBCDM%0KljkKWP5Wy z;%mFL4pqg$r6L^5sdf2pBVJ;jT}Z-I?&}tSsOn}ek#-c)x?lno;TE7%dw$0@M-*CDsQtHg)&-(Dl5JI(I5k8{mBn&>fUKx^&I z%-;4|DVF$wh^hS}H6#fTsA%@AqX0I@{k};`>j}@!U|cAKUw;XO&)V9Ck8vwQ-4*Pw z5{5sMMLctCTj^RR^)gFxHB5*A#J^Jbrdn&UIFATR#$>0vm4)+blG|w;IfS;5LX2qp zo*ez7yUP{T0a-y>>Ue{)Z_qE?(L``sWxZ2wf`*X!Nt2r4^8S|3YEJ8eslKLY{i6J| z%^PKwr5Jy*VMG{!t)->qJIR=IkDXq(Wvp+>qb)(im4VxUJ6_iv$wbm*EdlHk7*HRGX|!pX&$4}hSsN*#*KvQekUJVu@!E&JVtVep`wg7 zM<_GEv-dY@#V%i7Icm5n6%cPqP?Oa z%|j-qTM6QA=#9^g<&VBy&AV4iiigF_A|I|GSJngeGZH=4G?e%pZY_ofdh^|_?G~07 zIQ&-SbPG(agH%eXuC*~}^Pv)=|HpXn2{N3B7o(Q9x9Df3nu|0xk3zSJcmV=rRg_A- z_;lGPbRTq}16RY755+b^iP_IyXc3`8VMSK)DPUm2tk~^lvC7p?4p?m9j$B7`7Q$j} z5SxO;V}9il8Ao}JjdMCX6d6W%eF2KYWspmy`a~>iqIypR^{LSt8fp0sovx1XWO-kG zO0IWdYc@byCK@u?(hQQvKxs@W zNM^C+FewT}UfyV<$C=_nEU#Ko_9GT#{TO-p&9=2yeL<9oQ1VWyn8$-Wl1Rjw!5=GQ zJ6SPH^4+&kp`MzJHS~n6?8V<|HmOrsSIab!fy?9~FTt@AWY@9=8mKbXZ2oB)rXlYd zpIbdYctMXEa@w()nI$hU@?I*GHw93w`i3kGEnvOIn-&Ls7Mhq}8n=)u$s4%Z1uSBcIY|WR_bt(Qpi)h%G+hyi8 zEG`+^YCw;TcH=KsWv?FG=JtqqF4}%vJ4N2}=1tN{JwFT~_#=nD6<5_zHxd=HMDn}# z-1xLk4f*@fde;UUzkBrPib#J5V_i3WOiUc2TF^O|f-7773#hpGp||Fxmwkq`Fp3JZ?O-^Y498pT^R1)Wh{(QBF$2ZnFakpYAYK3aPzP2-?cR&$y~D zArvB7H?!)z~cVnw~|_srJmwo=`Rp`-AK6HIph>&E-V5i_KYk zw56{kBRqmFAD~a4Mx~80=$Rap{0C~`VuDEj5y3=#EvurKEk7mkT3uUbFGA_VB-pOL zW1HLP%PN3YOsObIHm+8^Z9;)V;WvB>ZEi5A(N_mThNR_Nu4akoQmyWhNQ2JFO|-#6 zx3uasNI4cU z`{V!k{oRYW4QV%slkn}{kHds0I87*A!4+3-4#lIC+>rzw-2-JxU#Qx4OuvrlDqm`d zG4Gmav=0P(T9;cZKG8NlCm(ajypyO0bF$La1(lFyz#TK@ax@uWefH87R|uN8z7O%D zmjBoy_+z3p?X7s(C0M%8%^k#cjVQK#K_*1 z4%++^`Bts4ic&@!eRt#c1B^sQIJVl1!*i?y`{;PfWZ})foOyUqc-8a;>BeU8w*K_U~4u^nUX6t#c zz3;)2v{1Z26!e*AmPM~FYWnQxk)`cz(QC-= zqVU9uL#%;-eHqiua624uka1@qznn~5T9iX{Ce0MWG?IP`J$7&*Z~8>=LzuXm0W9+)!dJA zt-Mc9nBG>cD3Y8~%G@eOF@` zs5!2%KHbKQ3^xj}qrckR;yvSidUKQ9$!KF|{PjskYsUm<QRR*FIlrn814w$H=3ptZlQRzk`hrzhn%w>@6Rbf=mUS#)|@j|@U?kD<4| zQ#wOu%B@f!y3-pJmrGQSSRv(UN0d$QEKnO4ljM>{<-M8!m5snqNIO}>r|Pf`moCt# z(7X5QU~OdySmjX;MzOtUe3$TQCRMi@pbVuLW3+@qOdKePevUO7MucC-UD*OJ;iw~~ zfN#0+Q6@j#BH5Q~q&|4TEO0i8 zZ0Vc!R>96{9VLue&DN5w(B@n%%E_bWP)fw>bVF?tR6m_ z4{PZVg6a|T&$dr3mQyAWf>29;s!O71GcYxrT+;Q6{jw~ja(4oAu9nzm44?j!$FHw9xwkHjGzr8caBvm{Tf}y`h(C&CJee zd~JLx8@m(YC721>BeHJjjK5IE{$e;>*eeHuBRJDrEmyNY31~MP(oj|aA6%51 zM)v01Uo1}+!pmY4S@XR(v%dM@2Pxpg+Y)&a+|p_RP(nwHR(^kZc#n7) zGfM#L9`#gZ|HlkOqW$#UT(ZNMieq5&y7I~t%Gm1tU^Z-7>Eec3J8_0!>%q$P+m`PQ z>;D?CA7QY5`cs{?rw2(>Y;3}wBGsN3JZR(n?&zu&$b7DN@=_VyE#{%r!?ztp!Z^(^ zn_B*p))$eXXPu3;HD7H?Kd6UOwG#$ab9l+-;a&@Y8vo7+zvW1rYZ^Fa+)`9cj5F_5h89p= zj_FR0CN)I0nYuf#qhqqg8|oE8W|8^vXWzCfkY;{-D4#eKUZXMN zA`*lZ+gQ<;DOgIVSgeh0#h%pT`#n1Cc-b{q9lWu7v$QR^HY71~Tv=b=zq;ycZeT#h zMIYI4&HAV+OZ`O-21}FYfz?IRR@I$rzQ%&lm(L`6$Q7{st7%@7=Bq3GJ?ap$|Hr zbwDa4;16tWel+7D03Z&2oa}x@kPyk9_>AGR$(;CwccQ$MCk1}4v2c{sGlLrS_Q6HP z`9nLkb+Au{Vv&`)G;co%AeM!bjqG4vJrZjyQ)YF>5S8C0FCW-V)+vHrmdRjl{_;e` zozUGO231q*8RfYZGZ5AQS_a*RPurj)ADs_IDsrN_&`o4n+lis4lau1r{mulMy+n3k zj#$p;a?*X3mD~J;FXH}CUf(=d03p1k8O;8PsP{NEOZIkoP?#I`yMjT%kJRMA7?zf_ z_~G%%Ic<>r_h@kCbwLT={G;$kOSf`?k|c_&<9m!0 z#N+V9nn+@aj(oValHK5g3)3;+m**_^@uQTSyx4U2n}v{ln<{S&psxsAg0e3^qr91{ zZ2ZCLxjtsfht}6G4H0I?F}TeGb8sa{9rjWnv3N&L=EF>TbuXZ`MKjE4U83;F*XS7)2BMua#OjQe=`-CaCh zZiGUftYNMA90x1aW!_K2XTG-wM(S{OJL7{YM62v!=xgb{El-+{YDzzD8R6$%Mk}v0 z8i}zljMmZ#Iib12@E_V~nTr?v#=H%Bt^;>I-*L{_wULp#BQs+6wH1i~;p4V_vk z&b5zak5(b(CqmLxgn02KcFN=Dxcs@86nH_ z@(ZnBdz)M;6HY~~9LI=$`rh7#lbfQrfD}yp6HFZtFVHFLiMg|Ge~ldy zMOBtvKTD1N3s08MbDxuHgDk!TfKVhG4EswT;tfHfv{+tE^Gi(?v7|0NP|4%guF-x6 z%P9xP*0t33^<3IlEOv-SdZf6xqs|IjHsHggBN>kqDdt+;qcs!~c{X^BNS$WYh~_6L zYJ91rN6Odfsxhqz7p6>XrJ?xXTq->j^T<3JMI}(Kklvj4=veh;qffQq)?7^=3<4XH zieml|R*J_weqhG+owK>q08|;cj@-%zMt!Az!#4DkLBI27S(xyBmDPJ_B_6=;VxV88AQpaHwN9vE1g+ z5ov>>ZTY@2pY_adoX-1e{dlJ={>Z0dkL!UeM*90B%&)4!QQAK-Mk3~~4sisy#j~2Z;o^y&s_(=HFd-(TeD>LWMneoNsgm^(U6j>%>bv6RSkq=s; zd(s8`i<>HqHLqCA%nU(4{AofZAMZ&n9(>ph5<=q!UU}H&f$VANSIvr9Nhs}grx-Li9t#* zc31uGw1HS(mGg7@u-Mq3I-8JGo{PpUZ{|iy-H6QE<&ev=zYcKr>n4Zsk`u19+>yQ% zK9y6@IuO+a>b;8LvCp7YlF#i|+{HFFbc2JpHzO+>XeeO% zS4$9^hbqMcSa|NBp;O#gZI+9)^NM_WAjYoW6x~F$tusl>p7%B?=`hg)%@77(GS;hdBpDaXUytb`RUm* ziQYDc(W9sHYh*^r8+n2fo2nkv^{7jW(JNRxx;$sT9M6gyx+|!vH=wS0Iv37ef1C16 zN+bO2HN>j*@URrX*aTy;$WRw-m4T9O`7W+0uZT3Zi55?o!+m|~3^eZa>2$(dP!QyeXFd&yZ>u0h2T1gKLR5d-Q zWgsBsgt24m4KlLTJb9M(-l|E3g(VgWkg{XA`9POiK$EAz>|bwp01`<@_YkU_G#l+lBSQgD;?Qp#zIFX%1Q+F@T&0a6NAFW#fXr=v+DF}EiD=WgIc)Fqa7u|m+qaU>*? z+z*<63~AiK8*7C`&ZVQAOjb~{TX$-!jA+rTGZlp_!jGTjg`~u}Z981`jWO!sf0TN) zxA^=e#+~Hn$$m_jAHt(z87m|b4+bzV#GH|VU*c0t$!v&612_2l3}MM{EjsV+S Date: Wed, 14 Jun 2023 14:50:28 +0200 Subject: [PATCH 10/59] Add device diagnostics messages (#117) * Diagnostic msgs publisher for proxy diver * diagnostic msgs for cia402 and motor interface * Restructured message constructor * Added option to enable/disable diagnostic * Add diagnostic test in proxy driver * Diagnostics test launch for proxy and cia402 * proxy driver switched to diagnostic updater * Complete diagnostic implementation for cia402 driver * Update formating * include code documentation --- .../include/canopen_402_driver/motor.hpp | 19 + .../node_canopen_402_driver.hpp | 1 + .../node_canopen_402_driver_impl.hpp | 15 + canopen_402_driver/src/motor.cpp | 60 +- canopen_base_driver/CMakeLists.txt | 2 + .../diagnostic_collector.hpp | 158 ++ .../node_canopen_base_driver.hpp | 9 + .../node_canopen_base_driver_impl.hpp | 112 +- canopen_base_driver/package.xml | 1 + .../node_canopen_proxy_driver.hpp | 1 + .../node_canopen_proxy_driver_impl.hpp | 26 + canopen_tests/CMakeLists.txt | 2 + .../config/cia402_diagnostics/bus.yml | 62 + .../cia402_diagnostics/cia402_slave.eds | 1441 +++++++++++++++++ canopen_tests/config/simple/bus.yml | 4 + .../config/simple_diagnostics/bus.yml | 23 + .../config/simple_diagnostics/simple.eds | 282 ++++ .../analyzers/cia402_diagnostic_analyzer.yaml | 15 + .../analyzers/proxy_diagnostic_analyzer.yaml | 11 + .../launch/cia402_diagnostics_setup.launch.py | 106 ++ .../launch/proxy_diagnostics_setup.launch.py | 92 ++ canopen_tests/launch/proxy_setup.launch.py | 1 + 22 files changed, 2435 insertions(+), 8 deletions(-) create mode 100644 canopen_base_driver/include/canopen_base_driver/diagnostic_collector.hpp create mode 100644 canopen_tests/config/cia402_diagnostics/bus.yml create mode 100644 canopen_tests/config/cia402_diagnostics/cia402_slave.eds create mode 100644 canopen_tests/config/simple_diagnostics/bus.yml create mode 100644 canopen_tests/config/simple_diagnostics/simple.eds create mode 100644 canopen_tests/launch/analyzers/cia402_diagnostic_analyzer.yaml create mode 100644 canopen_tests/launch/analyzers/proxy_diagnostic_analyzer.yaml create mode 100644 canopen_tests/launch/cia402_diagnostics_setup.launch.py create mode 100644 canopen_tests/launch/proxy_diagnostics_setup.launch.py diff --git a/canopen_402_driver/include/canopen_402_driver/motor.hpp b/canopen_402_driver/include/canopen_402_driver/motor.hpp index 871a2794..a3d8537c 100644 --- a/canopen_402_driver/include/canopen_402_driver/motor.hpp +++ b/canopen_402_driver/include/canopen_402_driver/motor.hpp @@ -15,6 +15,7 @@ #include "canopen_402_driver/default_homing_mode.hpp" #include "canopen_402_driver/mode_forward_helper.hpp" #include "canopen_402_driver/profiled_position_mode.hpp" +#include "canopen_base_driver/diagnostic_collector.hpp" #include "canopen_base_driver/lely_driver_bridge.hpp" namespace ros2_canopen @@ -53,6 +54,14 @@ class Motor402 : public MotorBase virtual bool isModeSupported(uint16_t mode); virtual uint16_t getMode(); bool readState(); + + /** + * @brief Updates the device diagnostic information + * + * This function updates the diagnostic information of the device by updating the diagnostic + * status message + * @ref diagnostic_status_ and publishing it. + */ void handleDiag(); /** * @brief Initialise the drive @@ -154,6 +163,12 @@ class Motor402 : public MotorBase return (double)this->driver->universal_get_value(0x6064, 0); } + void set_diagnostic_status_msgs(std::shared_ptr status, bool enable) + { + this->enable_diagnostics_.store(enable); + this->diag_collector_ = status; + } + private: virtual bool isModeSupportedByDevice(uint16_t mode); void registerMode(uint16_t id, const ModeSharedPtr & m); @@ -190,6 +205,10 @@ class Motor402 : public MotorBase const uint16_t op_mode_display_index = 0x6061; const uint16_t op_mode_index = 0x6060; const uint16_t supported_drive_modes_index = 0x6502; + + // Diagnostic components + std::atomic enable_diagnostics_; + std::shared_ptr diag_collector_; }; } // namespace ros2_canopen diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp index 823cd7ad..9142f825 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp @@ -42,6 +42,7 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver void publish(); virtual void poll_timer_callback() override; + void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat) override; public: NodeCanopen402Driver(NODETYPE * node); diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index 120e4a84..408261e2 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -255,6 +255,7 @@ void NodeCanopen402Driver::activate(bool called_from_base) { NodeCanopenProxyDriver::activate(false); motor_->registerDefaultModes(); + motor_->set_diagnostic_status_msgs(this->diagnostic_collector_, this->diagnostic_enabled_); } template @@ -602,4 +603,18 @@ bool NodeCanopen402Driver::set_target(double target) } } +template +void NodeCanopen402Driver::diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + this->motor_->handleDiag(); + + stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage()); + stat.add("device_state", this->diagnostic_collector_->getValue("DEVICE")); + stat.add("nmt_state", this->diagnostic_collector_->getValue("NMT")); + stat.add("emcy_state", this->diagnostic_collector_->getValue("EMCY")); + stat.add("cia402_mode", this->diagnostic_collector_->getValue("cia402_mode")); + stat.add("cia402_state", this->diagnostic_collector_->getValue("cia402_state")); +} + #endif diff --git a/canopen_402_driver/src/motor.cpp b/canopen_402_driver/src/motor.cpp index b2a98889..ecddda93 100644 --- a/canopen_402_driver/src/motor.cpp +++ b/canopen_402_driver/src/motor.cpp @@ -70,6 +70,10 @@ bool Motor402::switchMode(uint16_t mode) catch (...) { } + if (enable_diagnostics_.load()) + { + this->diag_collector_->addf("cia402_mode", "No mode selected: %d", mode); + } return true; } @@ -130,11 +134,19 @@ bool Motor402::switchMode(uint16_t mode) { selected_mode_ = next_mode; okay = true; + if (enable_diagnostics_.load()) + { + this->diag_collector_->addf("cia402_mode", "Mode switched to: %d", mode); + } } else { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode switch timed out."); driver->universal_set_value(op_mode_index, 0x0, mode_id_); + if (enable_diagnostics_.load()) + { + this->diag_collector_->addf("cia402_mode", "Mode switch timed out: %d", mode); + } } } @@ -159,10 +171,19 @@ bool Motor402::switchState(const State402::InternalState & target) RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Could not set transition."); return false; } + else if (enable_diagnostics_.load() && success) + { + this->diag_collector_->addf("cia402_state", "State switched to: %d", next); + } lock.unlock(); if (state != next && !state_handler_.waitForNewState(abstime, state)) { RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Transition timed out."); + if (enable_diagnostics_.load()) + { + this->diag_collector_->addf( + "cia402_state", "State transition timed out: %d -> %d", state, next); + } return false; } } @@ -256,34 +277,59 @@ void Motor402::handleDiag() switch (state) { case State402::Not_Ready_To_Switch_On: + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Not ready to switch on"); + break; case State402::Switch_On_Disabled: + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Switch on disabled"); + break; case State402::Ready_To_Switch_On: + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, "Ready to switch on"); + break; case State402::Switched_On: - std::cout << "Motor operation is not enabled" << std::endl; + // std::cout << "Motor operation is not enabled" << std::endl; + this->diag_collector_->summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Switched on"); + break; case State402::Operation_Enable: + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, "Operation enabled"); break; - case State402::Quick_Stop_Active: - std::cout << "Quick stop is active" << std::endl; + // std::cout << "Quick stop is active" << std::endl; + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Quick stop active"); break; case State402::Fault: + this->diag_collector_->summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Fault"); + break; case State402::Fault_Reaction_Active: - std::cout << "Motor has fault" << std::endl; + // std::cout << "Motor has fault" << std::endl; + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Fault reaction active"); break; case State402::Unknown: - std::cout << "State is unknown" << std::endl; + // std::cout << "State is unknown" << std::endl; + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Unknown state"); break; } if (sw & (1 << State402::SW_Warning)) { - std::cout << "Warning bit is set" << std::endl; + // std::cout << "Warning bit is set" << std::endl; + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Warning bit is set"); } if (sw & (1 << State402::SW_Internal_limit)) { - std::cout << "Internal limit active" << std::endl; + // std::cout << "Internal limit active" << std::endl; + this->diag_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Internal limit active"); } } + bool Motor402::handleInit() { for (std::unordered_map::iterator it = mode_allocators_.begin(); diff --git a/canopen_base_driver/CMakeLists.txt b/canopen_base_driver/CMakeLists.txt index 7b850aff..76f5fc49 100644 --- a/canopen_base_driver/CMakeLists.txt +++ b/canopen_base_driver/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(diagnostic_updater REQUIRED) set(dependencies canopen_core @@ -26,6 +27,7 @@ set(dependencies rclcpp_lifecycle std_msgs std_srvs + diagnostic_updater ) diff --git a/canopen_base_driver/include/canopen_base_driver/diagnostic_collector.hpp b/canopen_base_driver/include/canopen_base_driver/diagnostic_collector.hpp new file mode 100644 index 00000000..dbd5953b --- /dev/null +++ b/canopen_base_driver/include/canopen_base_driver/diagnostic_collector.hpp @@ -0,0 +1,158 @@ +#ifndef DIAGNOSTICS_COLLECTOR_HPP_ +#define DIAGNOSTICS_COLLECTOR_HPP_ + +#include +#include +#include +#include +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" + +namespace ros2_canopen +{ + +/** + * @brief A class to collect diagnostic information + * + */ +class DiagnosticsCollector +{ +public: + DiagnosticsCollector() : level_(diagnostic_msgs::msg::DiagnosticStatus::OK) {} + + /** + * @brief Get the Level + * Returns the current level (OK, WARN, ERROR or STALE) of the diagnostic + * @return unsigned char + */ + unsigned char getLevel() const + { + return static_cast(level_.load(std::memory_order_relaxed)); + } + + /** + * @brief Get the Message object + * Returns the current message of the diagnostic + * @return std::string + */ + std::string getMessage() const { return message_; } + + /** + * @brief Get the Value object + * Returns the current different device state values of the diagnostic. + * @param key Search device state by key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", + * "cia402_mode" etc. + * @return std::string + */ + std::string getValue(const std::string & key) const + { + std::lock_guard lock(mutex_); + auto it = values_.find(key); + if (it != values_.end()) + return it->second; + else + return ""; // Return an empty string if key not found + } + + /** + * @brief Store current device summary + * + * @param lvl Operation level (OK, WARN, ERROR or STALE) + * @param message Device summary message + */ + void summary(unsigned char lvl, const std::string & message) + { + std::lock_guard lock(mutex_); + this->setLevel(lvl); + this->setMessage(message); + } + + /** + * @brief Store current device summary + * + * @param lvl Operation level (OK, WARN, ERROR or STALE) + * @param format Device summary message format + * @param ... + */ + void summaryf(unsigned char lvl, const char * format, ...) + { + va_list args; + va_start(args, format); + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, args); + va_end(args); + summary(lvl, std::string(buffer)); + } + + /** + * @brief Add a device state value + * + * @param key Device state key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", "cia402_mode" etc. + * @param value Current device state value + */ + void add(const std::string & key, const std::string & value) + { + std::lock_guard lock(mutex_); + this->setValue(key, value); + } + + /** + * @brief Add a device state value + * + * @param key Device state key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", "cia402_mode" etc. + * @param format Current device state value format + * @param ... + */ + void addf(const std::string & key, const char * format, ...) + { + va_list args; + va_start(args, format); + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, args); + va_end(args); + add(key, std::string(buffer)); + } + + /** + * @brief Update all diagnostic information + * + * @param lvl Operation level (OK, WARN, ERROR or STALE) + * @param message Device summary message + * @param key Device state key. Eg. "DEVICE", "NMT", "EMCY", "cia402_state", "cia402_mode" etc. + * @param value Current device state value + */ + void updateAll( + unsigned char lvl, const std::string & message, const std::string & key, + const std::string & value) + { + std::lock_guard lock(mutex_); + this->setLevel(lvl); + this->setMessage(message); + this->setValue(key, value); + } + +private: + std::atomic level_; + std::string message_; + std::unordered_map values_; + mutable std::mutex mutex_; + + void setLevel(unsigned char lvl) + { + level_.store(static_cast(lvl), std::memory_order_relaxed); + } + + void setMessage(const std::string & message) { message_ = message; } + + void setValue(const std::string & key, const std::string & value) { values_[key] = value; } + + // std::unordered_map getValues() const + // { + // std::lock_guard lock(mutex_); + // return values_; + // } +}; +} // namespace ros2_canopen + +#endif // DIAGNOSTICS_COLLECTOR_HPP_ diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp index d914b17c..a4087460 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp @@ -15,6 +15,7 @@ #ifndef NODE_CANOPEN_BASE_DRIVER #define NODE_CANOPEN_BASE_DRIVER +#include "canopen_base_driver/diagnostic_collector.hpp" #include "canopen_base_driver/lely_driver_bridge.hpp" #include "canopen_core/node_interfaces/node_canopen_driver.hpp" #include "std_msgs/msg/string.hpp" @@ -55,6 +56,13 @@ class NodeCanopenBaseDriver : public NodeCanopenDriver std::shared_ptr> emcy_queue_; std::shared_ptr> rpdo_queue_; rclcpp::TimerBase::SharedPtr poll_timer_; + + // Diagnostic components + std::atomic diagnostic_enabled_; + uint32_t diagnostic_period_ms_; + std::shared_ptr diagnostic_updater_; + std::shared_ptr diagnostic_collector_; + virtual void poll_timer_callback(); void nmt_listener(); virtual void on_nmt(canopen::NmtState nmt_state); @@ -62,6 +70,7 @@ class NodeCanopenBaseDriver : public NodeCanopenDriver virtual void on_rpdo(COData data); void emcy_listener(); virtual void on_emcy(COEmcy emcy); + virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); public: NodeCanopenBaseDriver(NODETYPE * node); diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index 8189d23b..fdddb49b 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -21,7 +21,9 @@ using namespace ros2_canopen::node_interfaces; template NodeCanopenBaseDriver::NodeCanopenBaseDriver(NODETYPE * node) -: ros2_canopen::node_interfaces::NodeCanopenDriver(node) +: ros2_canopen::node_interfaces::NodeCanopenDriver(node), + diagnostic_enabled_(false), + diagnostic_collector_(new DiagnosticsCollector()) { } @@ -54,6 +56,36 @@ void NodeCanopenBaseDriver::configure(bool call period_ms_ = 10; } } + + // Diagnostic components + try + { + diagnostic_enabled_ = this->config_["diagnostics"]["enable"].as(); + } + catch (...) + { + RCLCPP_ERROR( + this->node_->get_logger(), + "Could not read enable diagnostics from config, setting to false."); + diagnostic_enabled_ = false; + } + if (diagnostic_enabled_.load()) + { + try + { + diagnostic_period_ms_ = this->config_["diagnostics"]["period"].as(); + } + catch (...) + { + RCLCPP_ERROR( + this->node_->get_logger(), + "Could not read diagnostics period from config, setting to 1000ms"); + diagnostic_period_ms_ = 1000; + } + + diagnostic_updater_ = std::make_shared(this->node_); + diagnostic_updater_->setHardwareID(std::to_string(this->node_id_)); + } } template <> void NodeCanopenBaseDriver::configure(bool called_from_base) @@ -79,6 +111,36 @@ void NodeCanopenBaseDriver::configure(bool called_from_base) period_ms_ = 10; } } + + // Diagnostic components + try + { + diagnostic_enabled_ = this->config_["diagnostics"]["enable"].as(); + } + catch (...) + { + RCLCPP_ERROR( + this->node_->get_logger(), + "Could not read enable diagnostics from config, setting to false."); + diagnostic_enabled_ = false; + } + if (diagnostic_enabled_.load()) + { + try + { + diagnostic_period_ms_ = this->config_["diagnostics"]["period"].as(); + } + catch (...) + { + RCLCPP_ERROR( + this->node_->get_logger(), + "Could not read diagnostics period from config, setting to 1000ms"); + diagnostic_period_ms_ = 1000; + } + + diagnostic_updater_ = std::make_shared(this->node_); + diagnostic_updater_->setHardwareID(std::to_string(this->node_id_)); + } } template @@ -101,6 +163,18 @@ void NodeCanopenBaseDriver::activate(bool called_from_base) this->lely_driver_->set_sync_function( std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this)); } + // poll_timer_ = this->node_->create_wall_timer( + // std::chrono::milliseconds(period_ms_), + // std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this), this->timer_cbg_); + this->lely_driver_->set_sync_function( + std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this)); + + if (diagnostic_enabled_.load()) + { + RCLCPP_INFO(this->node_->get_logger(), "Starting with diagnostics enabled."); + diagnostic_updater_->add( + "diagnostic updater", this, &NodeCanopenBaseDriver::diagnostic_callback); + } } template @@ -109,6 +183,10 @@ void NodeCanopenBaseDriver::deactivate(bool called_from_base) nmt_state_publisher_thread_.join(); poll_timer_->cancel(); this->lely_driver_->unset_sync_function(); + if (diagnostic_enabled_.load()) + { + diagnostic_updater_->removeByName("diagnostic updater"); + } } template @@ -161,6 +239,12 @@ void NodeCanopenBaseDriver::add_to_master() } } RCLCPP_INFO(this->node_->get_logger(), "Driver booted and ready."); + + if (diagnostic_enabled_.load()) + { + diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::OK, "Device ready", "DEVICE", "Added to master."); + } } template @@ -181,6 +265,12 @@ void NodeCanopenBaseDriver::remove_from_master() { throw DriverException("remove_from_master: removing timed out"); } + if (diagnostic_enabled_.load()) + { + diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Device removed", "DEVICE", + "Removed from master."); + } } template void NodeCanopenBaseDriver::nmt_listener() @@ -224,6 +314,20 @@ void NodeCanopenBaseDriver::on_rpdo(COData data) template void NodeCanopenBaseDriver::on_emcy(COEmcy emcy) { + diagnostic_collector_->summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Emergency message received"); + std::string emcy_msg = "Emergency message: "; + emcy_msg.append("eec: "); + emcy_msg.append(std::to_string(emcy.eec)); + emcy_msg.append(" er: "); + emcy_msg.append(std::to_string(emcy.er)); + emcy_msg.append(" msef: "); + for (auto & msef : emcy.msef) + { + emcy_msg.append(std::to_string(msef)); + emcy_msg.append(" "); + } + diagnostic_collector_->add("EMCY", emcy_msg); } template @@ -328,4 +432,10 @@ void NodeCanopenBaseDriver::emcy_listener() } } +template +void NodeCanopenBaseDriver::diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ +} + #endif diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index a47416a6..40858fe1 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -18,6 +18,7 @@ std_msgs std_srvs boost + diagnostic_updater ament_lint_auto diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp index b73759c1..df8a61ce 100644 --- a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp +++ b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver.hpp @@ -43,6 +43,7 @@ class NodeCanopenProxyDriver : public NodeCanopenBaseDriver virtual void on_nmt(canopen::NmtState nmt_state) override; virtual void on_rpdo(COData data) override; virtual void on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg); + virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat) override; void on_nmt_state_reset( const std_srvs::srv::Trigger::Request::SharedPtr request, diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp index c4fdf613..15444a31 100644 --- a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp +++ b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp @@ -119,28 +119,44 @@ void NodeCanopenProxyDriver::on_nmt(canopen::NmtState nmt_state) { case canopen::NmtState::BOOTUP: message.data = "BOOTUP"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT bootup", "NMT", "BOOTUP"); break; case canopen::NmtState::PREOP: message.data = "PREOP"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT preop", "NMT", "PREOP"); break; case canopen::NmtState::RESET_COMM: message.data = "RESET_COMM"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "NMT reset comm", "NMT", "RESET_COMM"); break; case canopen::NmtState::RESET_NODE: message.data = "RESET_NODE"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "NMT reset node", "NMT", "RESET_NODE"); break; case canopen::NmtState::START: message.data = "START"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT start", "NMT", "START"); break; case canopen::NmtState::STOP: message.data = "STOP"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT stop", "NMT", "STOP"); break; case canopen::NmtState::TOGGLE: message.data = "TOGGLE"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::OK, "NMT toggle", "NMT", "TOGGLE"); break; default: RCLCPP_ERROR(this->node_->get_logger(), "Unknown NMT State."); message.data = "ERROR"; + this->diagnostic_collector_->updateAll( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "NMT unknown state", "NMT", "ERROR"); break; } RCLCPP_INFO( @@ -317,4 +333,14 @@ bool NodeCanopenProxyDriver::sdo_write(ros2_canopen::COData & data) return false; } +template +void NodeCanopenProxyDriver::diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage()); + stat.add("device_state", this->diagnostic_collector_->getValue("DEVICE")); + stat.add("nmt_state", this->diagnostic_collector_->getValue("NMT")); + stat.add("emcy_state", this->diagnostic_collector_->getValue("EMCY")); +} + #endif diff --git a/canopen_tests/CMakeLists.txt b/canopen_tests/CMakeLists.txt index 33b0720d..ff995ec5 100644 --- a/canopen_tests/CMakeLists.txt +++ b/canopen_tests/CMakeLists.txt @@ -15,7 +15,9 @@ generate_dcf(canopen_system) cogen_dcf(cia402_system) cogen_dcf(cia402) generate_dcf(cia402_lifecycle) +cogen_dcf(cia402_diagnostics) generate_dcf(simple_lifecycle) +cogen_dcf(simple_diagnostics) generate_dcf(robot_control) install(DIRECTORY diff --git a/canopen_tests/config/cia402_diagnostics/bus.yml b/canopen_tests/config/cia402_diagnostics/bus.yml new file mode 100644 index 00000000..ad74740a --- /dev/null +++ b/canopen_tests/config/cia402_diagnostics/bus.yml @@ -0,0 +1,62 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + +master: + node_id: 1 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + sync_period: 10000 + +defaults: + dcf: "cia402_slave.eds" + driver: "ros2_canopen::Cia402Driver" + package: "canopen_402_driver" + diagnostics: + enable: true + period: 1000 # in milliseconds + revision_number: 0 + sdo: + - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms + - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s + - {index: 0x6081, sub_index: 0, value: 1000} + - {index: 0x6083, sub_index: 0, value: 2000} + tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation + 1: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6041, sub_index: 0} # status word + - {index: 0x6061, sub_index: 0} # mode of operation display + 2: + enabled: true + cob_id: "auto" + transmission: 0x01 + mapping: + - {index: 0x6064, sub_index: 0} # position actual value + - {index: 0x606c, sub_index: 0} # velocity actual position + 3: + enabled: false + 4: + enabled: false + rpdo: # RPDO needed controlword, target position, target velocity, mode of operation + 1: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x6040, sub_index: 0} # controlword + - {index: 0x6060, sub_index: 0} # mode of operation + 2: + enabled: true + cob_id: "auto" + mapping: + - {index: 0x607A, sub_index: 0} # target position + - {index: 0x60FF, sub_index: 0} # target velocity + +nodes: + cia402_device_1: + node_id: 2 + cia402_device_2: + node_id: 3 + cia402_device_3: + node_id: 4 diff --git a/canopen_tests/config/cia402_diagnostics/cia402_slave.eds b/canopen_tests/config/cia402_diagnostics/cia402_slave.eds new file mode 100644 index 00000000..24be3815 --- /dev/null +++ b/canopen_tests/config/cia402_diagnostics/cia402_slave.eds @@ -0,0 +1,1441 @@ +[DeviceInfo] +VendorName=ROS-Industrial +VendorNumber=0x555 +ProductName=CIA402VTD +BaudRate_10=0 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +LSS_Supported=0 +Granularity=8 +SimpleBootUpSlave=1 +SimpleBootUpMaster=0 +NrOfRXPDO=4 +NrOfTXPDO=4 + +[Comments] +Lines=0 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[ManufacturerObjects] +SupportedObjects=0 + +[OptionalObjects] +SupportedObjects=67 +1=0x1005 +2=0x1008 +3=0x1009 +4=0x100A +5=0x100C +6=0x100D +7=0x1010 +8=0x1011 +9=0x1014 +10=0x1015 +11=0x1016 +12=0x1017 +13=0x1023 +14=0x1029 +15=0x1400 +16=0x1401 +17=0x1402 +18=0x1403 +19=0x1600 +20=0x1601 +21=0x1602 +22=0x1603 +23=0x1800 +24=0x1801 +25=0x1802 +26=0x1803 +27=0x1A00 +28=0x1A01 +29=0x1A02 +30=0x1A03 +31=0x6040 +32=0x6041 +33=0x605A +34=0x605B +35=0x605C +36=0x605D +37=0x605E +38=0x6060 +39=0x6061 +40=0x6062 +41=0x6063 +42=0x6064 +43=0x6065 +44=0x6067 +45=0x6068 +46=0x606A +47=0x606C +48=0x607A +49=0x607C +50=0x607D +51=0x6081 +52=0x6082 +53=0x6083 +54=0x6084 +55=0x6085 +56=0x608F +57=0x6098 +58=0x6099 +59=0x609A +60=0x60B0 +61=0x60B1 +62=0x60C2 +63=0x60F2 +64=0x60FD +65=0x60FF +66=0x6502 +67=0x67FF + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +AccessType=const +DefaultValue=0xFFFF0192 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1010] +SubNumber=8 +ParameterName=Store Parameter Field +ObjectType=0x08 + +[1010sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x7 +PDOMapping=0 + +[1010sub1] +ParameterName=Save all Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub2] +ParameterName=Save Communication Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub3] +ParameterName=Save Device Profile Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub4] +ParameterName=Save Axis 0 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub5] +ParameterName=Save Axis 1 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub6] +ParameterName=Save Axis 2 Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1010sub7] +ParameterName=Save Device Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1011] +SubNumber=8 +ParameterName=Restore Default Parameters +ObjectType=0x08 + +[1011sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x7 +PDOMapping=0 + +[1011sub1] +ParameterName=Restore all Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub2] +ParameterName=Restore Communication Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub3] +ParameterName=Restore Device Profile Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub4] +ParameterName=Restore Axis 0 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub5] +ParameterName=Restore Axis 1 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1011sub6] +ParameterName=Restore Axis 2 Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1011sub7] +ParameterName=Restore Device Default Parameters +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1014] +ParameterName=COB-ID EMCY +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 +PDOMapping=0 + +[1015] +ParameterName=Inhibit Time Emergency +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1016] +SubNumber=2 +ParameterName=Heartbeat Consumer Entries +ObjectType=0x08 + +[1016sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x1 +PDOMapping=0 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x555 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x0 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x0 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1023] +SubNumber=4 +ParameterName=OS Command +ObjectType=0x09 + +[1023sub0] +ParameterName=NumOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x3 +PDOMapping=0 + +[1023sub1] +ParameterName=Command +ObjectType=0x07 +DataType=0x000A +AccessType=rw +PDOMapping=0 +ObjFlags=0x00000001 + +[1023sub2] +ParameterName=Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1023sub3] +ParameterName=Reply +ObjectType=0x07 +DataType=0x000A +AccessType=ro +PDOMapping=0 + +[1029] +SubNumber=3 +ParameterName=Error Behaviour +ObjectType=0x08 + +[1029sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=5 +AccessType=ro +DefaultValue=0x2 +PDOMapping=0 + +[1029sub1] +ParameterName=Communication Error +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1029sub2] +ParameterName=Application Error +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[1400] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 1 +ObjectType=0x09 + +[1400sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1400sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000200 +PDOMapping=0 + +[1400sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1401] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 2 +ObjectType=0x09 + +[1401sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1401sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000300 +PDOMapping=0 + +[1401sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1402] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 3 +ObjectType=0x09 + +[1402sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1402sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000400 +PDOMapping=0 + +[1402sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1403] +SubNumber=3 +ParameterName=Receive PDO Communication Parameter 4 +ObjectType=0x09 + +[1403sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[1403sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000500 +PDOMapping=0 + +[1403sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFE +PDOMapping=0 + +[1600] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 1 +ObjectType=0x09 + +[1600sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1600sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1600sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1600sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 2 +ObjectType=0x09 + +[1601sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1601sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1601sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60600008 +PDOMapping=0 + +[1601sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 3 +ObjectType=0x09 + +[1602sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1602sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1602sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x607A0020 +PDOMapping=0 + +[1602sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603] +SubNumber=4 +ParameterName=Receive PDO Mapping Parameter 4 +ObjectType=0x09 + +[1603sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1603sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 + +[1603sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60FF0020 +PDOMapping=0 + +[1603sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1800] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 1 +ObjectType=0x09 + +[1800sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000180 +PDOMapping=0 + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1801] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 2 +ObjectType=0x09 + +[1801sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000280 +PDOMapping=0 + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1801sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1802] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 3 +ObjectType=0x09 + +[1802sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1802sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000380 +PDOMapping=0 + +[1802sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[1802sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1802sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1802sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1803] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 4 +ObjectType=0x09 + +[1803sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1803sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80000480 +PDOMapping=0 + +[1803sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFE +PDOMapping=0 + +[1803sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1803sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1803sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1A00] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 1 +ObjectType=0x09 + +[1A00sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1A00sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A00sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 2 +ObjectType=0x09 + +[1A01sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A01sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A01sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60610008 +PDOMapping=0 + +[1A01sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 3 +ObjectType=0x09 + +[1A02sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A02sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A02sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60640020 +PDOMapping=0 + +[1A02sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03] +SubNumber=4 +ParameterName=Transmit PDO Mapping Parameter 4 +ObjectType=0x09 + +[1A03sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x02 +PDOMapping=0 + +[1A03sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 + +[1A03sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x606C0020 +PDOMapping=0 + +[1A03sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[6040] +ParameterName=Controlword 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rww +PDOMapping=1 + +[6041] +ParameterName=Statusword 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=ro +PDOMapping=1 + +[605A] +ParameterName=Quick Stop Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +DefaultValue=0x0002 +PDOMapping=0 + +[605B] +ParameterName=Shutdown Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 + +[605C] +ParameterName=Disable Operation Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0001 +PDOMapping=0 + +[605D] +ParameterName=Halt Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +DefaultValue=0x0001 +PDOMapping=0 + +[605E] +ParameterName=Fault Reaction Option Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0002 +PDOMapping=0 + +[6060] +ParameterName=Modes of Operation 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=rww +DefaultValue=0x00 +PDOMapping=1 + +[6061] +ParameterName=Modes of Operation Display 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=ro +PDOMapping=1 + +[6062] +ParameterName=Position Demand Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6063] +ParameterName=Position Actual Internal Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6064] +ParameterName=Position Actual Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[6065] +ParameterName=Following Error Window 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6067] +ParameterName=Position Window 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0xFFFFFFFF +PDOMapping=0 + +[6068] +ParameterName=Position Window Time 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[606A] +ParameterName=Sensor Selection Code 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=rw +PDOMapping=0 + +[606C] +ParameterName=Velocity Actual Value 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=ro +PDOMapping=1 + +[607A] +ParameterName=Target Position 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rww +DefaultValue=0x0 +PDOMapping=1 + +[607C] +ParameterName=Home Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[607D] +SubNumber=3 +ParameterName=Software Position Limit 1 +ObjectType=0x08 + +[607Dsub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[607Dsub1] +ParameterName=Min Software Position Limit +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=-2147483648 +PDOMapping=0 + +[607Dsub2] +ParameterName=Max Software Position Limit +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=2147483647 +PDOMapping=0 + +[6081] +ParameterName=Profile Velocity in pp-mode 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6082] +ParameterName=End velocity 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[6083] +ParameterName=Profile Acceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6084] +ParameterName=Profile Deceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6085] +ParameterName=Quick Stop Deceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[608F] +SubNumber=3 +ParameterName=Position Encoder Resolution 1 +ObjectType=0x08 + +[608Fsub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x00000002 +PDOMapping=0 + +[608Fsub1] +ParameterName=Encoder Increments +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[608Fsub2] +ParameterName=Motor Revolutions +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000001 +PDOMapping=0 + +[6098] +ParameterName=Homing Method 1 +ObjectType=0x07 +DataType=0x0002 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[6099] +SubNumber=3 +ParameterName=Homing Speeds 1 +ObjectType=0x08 + +[6099sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[6099sub1] +ParameterName=Fast Homing Speed +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[6099sub2] +ParameterName=Slow Homing Speed +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[609A] +ParameterName=Homing Acceleration 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[60B0] +ParameterName=Position Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[60B1] +ParameterName=Velocity Offset 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[60C2] +SubNumber=3 +ParameterName=Interpolation Time Period 1 +ObjectType=0x09 + +[60C2sub0] +ParameterName=NumOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[60C2sub1] +ParameterName=timeUnits +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x1 +PDOMapping=0 + +[60C2sub2] +ParameterName=timeIndex +ObjectType=0x07 +DataType=0x0002 +AccessType=rw +DefaultValue=-2 +PDOMapping=0 + +[60F2] +ParameterName=Positioning Option Code 1 +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[60FD] +ParameterName=Digital Inputs 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=1 + +[60FF] +ParameterName=Target Velocity 1 +ObjectType=0x07 +DataType=0x0004 +AccessType=rw +DefaultValue=0x0 +PDOMapping=1 + +[6502] +ParameterName=Supported Drive Modes 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x000000A5 +PDOMapping=0 + +[67FF] +ParameterName=Single Device Type 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00040192 +PDOMapping=0 diff --git a/canopen_tests/config/simple/bus.yml b/canopen_tests/config/simple/bus.yml index 4b9d35cb..8b26d4e8 100644 --- a/canopen_tests/config/simple/bus.yml +++ b/canopen_tests/config/simple/bus.yml @@ -11,9 +11,13 @@ proxy_device_1: dcf: "simple.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" + polling: true + period: 10 proxy_device_2: node_id: 3 dcf: "simple.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" + polling: true + period: 10 diff --git a/canopen_tests/config/simple_diagnostics/bus.yml b/canopen_tests/config/simple_diagnostics/bus.yml new file mode 100644 index 00000000..96ceac9e --- /dev/null +++ b/canopen_tests/config/simple_diagnostics/bus.yml @@ -0,0 +1,23 @@ +options: + dcf_path: "@BUS_CONFIG_PATH@" + +master: + node_id: 1 + driver: "ros2_canopen::MasterDriver" + package: "canopen_master_driver" + +defaults: + dcf: "simple.eds" + driver: "ros2_canopen::ProxyDriver" + package: "canopen_proxy_driver" + polling: true + period: 10 + diagnostics: + enable: true + period: 1000 # in milliseconds + +nodes: + proxy_device_1: + node_id: 2 + proxy_device_2: + node_id: 3 diff --git a/canopen_tests/config/simple_diagnostics/simple.eds b/canopen_tests/config/simple_diagnostics/simple.eds new file mode 100644 index 00000000..68d9bb2a --- /dev/null +++ b/canopen_tests/config/simple_diagnostics/simple.eds @@ -0,0 +1,282 @@ +[DeviceInfo] +VendorName=Lely Industries N.V. +VendorNumber=0x00000360 +ProductName= +ProductNumber=0x00000000 +RevisionNumber=0x00000000 +OrderCode= +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +NrOfRxPDO=1 +NrOfTxPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=1 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 +Dummy0010=1 +Dummy0011=1 +Dummy0012=1 +Dummy0013=1 +Dummy0014=1 +Dummy0015=1 +Dummy0016=1 +Dummy0018=1 +Dummy0019=1 +Dummy001A=1 +Dummy001B=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=11 +1=0x1003 +2=0x1005 +3=0x1014 +4=0x1015 +5=0x1016 +6=0x1017 +7=0x1029 +8=0x1400 +9=0x1600 +10=0x1800 +11=0x1A00 + +[ManufacturerObjects] +SupportedObjects=4 +1=0x4000 +2=0x4001 +3=0x4002 +4=0x4003 + +[1000] +ParameterName=Device type +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 + +[1001] +ParameterName=Error register +DataType=0x0005 +AccessType=ro + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x08 +DataType=0x0007 +AccessType=ro +CompactSubObj=254 + +[1005] +ParameterName=COB-ID SYNC message +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 + +[1014] +ParameterName=COB-ID EMCY +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 + +[1015] +ParameterName=Inhibit time EMCY +DataType=0x0006 +AccessType=rw +DefaultValue=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1017] +ParameterName=Producer heartbeat time +DataType=0x0006 +AccessType=rw + +[1018] +SubNumber=5 +ParameterName=Identity object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=4 + +[1018sub1] +ParameterName=Vendor-ID +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000360 + +[1018sub2] +ParameterName=Product code +DataType=0x0007 +AccessType=ro + +[1018sub3] +ParameterName=Revision number +DataType=0x0007 +AccessType=ro + +[1018sub4] +ParameterName=Serial number +DataType=0x0007 +AccessType=ro + +[1029] +ParameterName=Error behavior object +ObjectType=0x08 +DataType=0x0005 +AccessType=rw +CompactSubObj=1 + +[1400] +SubNumber=6 +ParameterName=RPDO communication parameter +ObjectType=0x09 + +[1400sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=5 + +[1400sub1] +ParameterName=COB-ID used by RPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x200 + +[1400sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1400sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1400sub4] +ParameterName=compatibility entry +DataType=0x0005 +AccessType=rw + +[1400sub5] +ParameterName=event-timer +DataType=0x0006 +AccessType=rw + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1600Value] +NrOfEntries=1 +1=0x40000020 + +[1800] +SubNumber=7 +ParameterName=TPDO communication parameter +ObjectType=0x09 + +[1800sub0] +ParameterName=highest sub-index supported +DataType=0x0005 +AccessType=const +DefaultValue=6 + +[1800sub1] +ParameterName=COB-ID used by TPDO +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 + +[1800sub2] +ParameterName=transmission type +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF + +[1800sub3] +ParameterName=inhibit time +DataType=0x0006 +AccessType=rw + +[1800sub4] +ParameterName=reserved +DataType=0x0005 +AccessType=rw + +[1800sub5] +ParameterName=event timer +DataType=0x0006 +AccessType=rw + +[1800sub6] +ParameterName=SYNC start value +DataType=0x0005 +AccessType=rw + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x08 +DataType=0x0007 +AccessType=rw +CompactSubObj=1 + +[1A00Value] +NrOfEntries=1 +1=0x40010020 + +[4000] +ParameterName=UNSIGNED32 received by slave +DataType=0x0007 +AccessType=rww +PDOMapping=1 + +[4001] +ParameterName=UNSIGNED32 sent from slave +DataType=0x0007 +AccessType=rwr +PDOMapping=1 + +[4002] +ParameterName=INTEGER32 test +DataType=0x0004 +AccessType=rw + +[4003] +ParameterName=INTEGER16 test +DataType=0x0003 +AccessType=rw diff --git a/canopen_tests/launch/analyzers/cia402_diagnostic_analyzer.yaml b/canopen_tests/launch/analyzers/cia402_diagnostic_analyzer.yaml new file mode 100644 index 00000000..4e68c47c --- /dev/null +++ b/canopen_tests/launch/analyzers/cia402_diagnostic_analyzer.yaml @@ -0,0 +1,15 @@ +analyzers: + ros__parameters: + path: Aggregation + cia402_device_1: + type: diagnostic_aggregator/GenericAnalyzer + path: Cia402Device1 + contains: [ 'cia402_device_1' ] + cia402_device_2: + type: diagnostic_aggregator/GenericAnalyzer + path: Cia402Device2 + contains: [ 'cia402_device_2' ] + cia402_device_3: + type: diagnostic_aggregator/GenericAnalyzer + path: Cia402Device3 + contains: [ 'cia402_device_3' ] diff --git a/canopen_tests/launch/analyzers/proxy_diagnostic_analyzer.yaml b/canopen_tests/launch/analyzers/proxy_diagnostic_analyzer.yaml new file mode 100644 index 00000000..31fb092e --- /dev/null +++ b/canopen_tests/launch/analyzers/proxy_diagnostic_analyzer.yaml @@ -0,0 +1,11 @@ +analyzers: + ros__parameters: + path: Aggregation + proxy_device_1: + type: diagnostic_aggregator/GenericAnalyzer + path: ProxyDevice1 + contains: [ 'proxy_device_1' ] + proxy_device_2: + type: diagnostic_aggregator/GenericAnalyzer + path: ProxyDevice2 + contains: [ 'proxy_device_2' ] diff --git a/canopen_tests/launch/cia402_diagnostics_setup.launch.py b/canopen_tests/launch/cia402_diagnostics_setup.launch.py new file mode 100644 index 00000000..c8937f6d --- /dev/null +++ b/canopen_tests/launch/cia402_diagnostics_setup.launch.py @@ -0,0 +1,106 @@ +import os +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +import launch +import launch_ros +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + slave_eds_path = os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "cia402_diagnostics", + "cia402_slave.eds", + ) + + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/cia402_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "2", + "node_name": "cia402_node_1", + "slave_config": slave_eds_path, + }.items(), + ) + slave_node_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/cia402_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "3", + "node_name": "cia402_node_2", + "slave_config": slave_eds_path, + }.items(), + ) + slave_node_3 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/cia402_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "4", + "node_name": "cia402_node_4", + "slave_config": slave_eds_path, + }.items(), + ) + master_bin_path = os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "cia402_diagnostics", + "master.bin", + ) + if not os.path.exists(master_bin_path): + master_bin_path = "" + device_container = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_core"), "launch"), + "/canopen.launch.py", + ] + ), + launch_arguments={ + "master_config": os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "cia402_diagnostics", + "master.dcf", + ), + "master_bin": master_bin_path, + "bus_config": os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "cia402_diagnostics", + "bus.yml", + ), + "can_interface_name": "vcan0", + }.items(), + ) + + diagnostics_analyzer_path = os.path.join( + get_package_share_directory("canopen_tests"), + "launch", + "analyzers", + "cia402_diagnostic_analyzer.yaml", + ) + + diagnostics_aggregator_node = launch_ros.actions.Node( + package="diagnostic_aggregator", + executable="aggregator_node", + output="screen", + parameters=[diagnostics_analyzer_path], + ) + + return LaunchDescription( + [slave_node_1, slave_node_2, slave_node_3, device_container, diagnostics_aggregator_node] + ) diff --git a/canopen_tests/launch/proxy_diagnostics_setup.launch.py b/canopen_tests/launch/proxy_diagnostics_setup.launch.py new file mode 100644 index 00000000..a0357f87 --- /dev/null +++ b/canopen_tests/launch/proxy_diagnostics_setup.launch.py @@ -0,0 +1,92 @@ +import os +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +import launch +import launch_ros +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + slave_eds_path = os.path.join( + get_package_share_directory("canopen_tests"), "config", "simple_diagnostics", "simple.eds" + ) + slave_node_1 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/basic_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "2", + "node_name": "slave_node_1", + "slave_config": slave_eds_path, + }.items(), + ) + + slave_node_2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_fake_slaves"), "launch"), + "/basic_slave.launch.py", + ] + ), + launch_arguments={ + "node_id": "3", + "node_name": "slave_node_2", + "slave_config": slave_eds_path, + }.items(), + ) + + print( + os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "proxy_write_sdo", + "master.dcf", + ) + ) + + device_container = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_core"), "launch"), + "/canopen.launch.py", + ] + ), + launch_arguments={ + "master_config": os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "simple_diagnostics", + "master.dcf", + ), + "master_bin": "", + "bus_config": os.path.join( + get_package_share_directory("canopen_tests"), + "config", + "simple_diagnostics", + "bus.yml", + ), + "can_interface_name": "vcan0", + }.items(), + ) + + diagnostics_analyzer_path = os.path.join( + get_package_share_directory("canopen_tests"), + "launch", + "analyzers", + "proxy_diagnostic_analyzer.yaml", + ) + + diagnostics_aggregator_node = launch_ros.actions.Node( + package="diagnostic_aggregator", + executable="aggregator_node", + output="screen", + parameters=[diagnostics_analyzer_path], + ) + + return LaunchDescription( + [slave_node_1, slave_node_2, device_container, diagnostics_aggregator_node] + ) diff --git a/canopen_tests/launch/proxy_setup.launch.py b/canopen_tests/launch/proxy_setup.launch.py index 867b35e0..ef6d1d3e 100644 --- a/canopen_tests/launch/proxy_setup.launch.py +++ b/canopen_tests/launch/proxy_setup.launch.py @@ -16,6 +16,7 @@ from ament_index_python import get_package_share_directory from launch import LaunchDescription import launch +import launch_ros from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource From 5efacc286319e13042abd75db0e8ba14015c7e95 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 14 Jun 2023 15:03:52 +0200 Subject: [PATCH 11/59] Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' --- .../sphinx/user-guide/how-to-create-a-configuration.rst | 2 +- .../user-guide/operation/managed-service-interface.rst | 2 +- canopen/sphinx/user-guide/operation/service-interface.rst | 2 +- .../node_interfaces/node_canopen_402_driver_impl.hpp | 3 ++- canopen_core/src/device_container.cpp | 1 + canopen_ros2_control/src/canopen_system.cpp | 2 +- canopen_ros2_control/src/cia402_system.cpp | 4 ++-- canopen_tests/launch/cia402_system.launch.py | 8 ++++---- .../urdf/cia402_system/cia402_system.ros2_control.xacro | 4 ++-- canopen_tests/urdf/cia402_system/cia402_system.urdf.xacro | 4 ++-- 10 files changed, 17 insertions(+), 15 deletions(-) diff --git a/canopen/sphinx/user-guide/how-to-create-a-configuration.rst b/canopen/sphinx/user-guide/how-to-create-a-configuration.rst index 4d5fb355..9caf231c 100644 --- a/canopen/sphinx/user-guide/how-to-create-a-configuration.rst +++ b/canopen/sphinx/user-guide/how-to-create-a-configuration.rst @@ -181,7 +181,7 @@ Add the following code: "{bus_config_name}", "bus.yml", ), - "can_interface_name": "{can_interface i.e. can0}", + "can_interface_name": "{can_interface_name i.e. can0}", }.items(), ) diff --git a/canopen/sphinx/user-guide/operation/managed-service-interface.rst b/canopen/sphinx/user-guide/operation/managed-service-interface.rst index c0c858a3..e7962b48 100644 --- a/canopen/sphinx/user-guide/operation/managed-service-interface.rst +++ b/canopen/sphinx/user-guide/operation/managed-service-interface.rst @@ -49,4 +49,4 @@ The device manager has the following configuration parameters. bus_conf, string, (Mandatory) Path to the bus configuration YAML-file master_dcf, string, (Mandatory) Path to the DCF file to be used by the master node. Usually generated by dcfgen as master.dcf. master_bin, string, (Optional) Path to the concise DCF (.bin) file to be used to configure the master. Usually generated by dcfgen as master.bin. (default: "") - can_interface, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) + can_interface_name, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) diff --git a/canopen/sphinx/user-guide/operation/service-interface.rst b/canopen/sphinx/user-guide/operation/service-interface.rst index f177420b..17d77bdb 100644 --- a/canopen/sphinx/user-guide/operation/service-interface.rst +++ b/canopen/sphinx/user-guide/operation/service-interface.rst @@ -51,4 +51,4 @@ The device manager has the following configuration parameters. bus_conf, string, (Mandatory) Path to the bus configuration YAML-file master_dcf, string, (Mandatory) Path to the DCF file to be used by the master node. Usually generated by dcfgen as master.dcf. master_bin, string, (Optional) Path to the concise DCF (.bin) file to be used to configure the master. Usually generated by dcfgen as master.bin. (default: "") - can_interface, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) + can_interface_name, string, (Mandatory) Name of the CAN interface to be used. (default: vcan0) diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index 408261e2..d85cf48e 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -7,7 +7,8 @@ #include using namespace ros2_canopen::node_interfaces; -using namespace std::placeholders; +using std::placeholders::_1; +using std::placeholders::_2; template NodeCanopen402Driver::NodeCanopen402Driver(NODETYPE * node) diff --git a/canopen_core/src/device_container.cpp b/canopen_core/src/device_container.cpp index 43590041..d4153a11 100644 --- a/canopen_core/src/device_container.cpp +++ b/canopen_core/src/device_container.cpp @@ -362,6 +362,7 @@ void DeviceContainer::init( bus_config_ = bus_config; RCLCPP_INFO(this->get_logger(), "Starting Device Container with:"); + RCLCPP_INFO(this->get_logger(), "\t can_interface_name %s", can_interface_name_.c_str()); RCLCPP_INFO(this->get_logger(), "\t master_config %s", dcf_txt_.c_str()); RCLCPP_INFO(this->get_logger(), "\t bus_config %s", bus_config_.c_str()); diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index eed266a9..d8d3ae24 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -146,7 +146,7 @@ void CanopenSystem::initDeviceContainer() proxy_driver->register_rpdo_cb(rpdo_cb); RCLCPP_INFO( - kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%u'", + kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%x'", // it->second->get_node_base_interface()->get_name(), it->first); } diff --git a/canopen_ros2_control/src/cia402_system.cpp b/canopen_ros2_control/src/cia402_system.cpp index 42879656..8c1cfbf4 100644 --- a/canopen_ros2_control/src/cia402_system.cpp +++ b/canopen_ros2_control/src/cia402_system.cpp @@ -53,7 +53,7 @@ void Cia402System::initDeviceContainer() : info_.hardware_parameters["master_bin"]; device_container_->init( - info_.hardware_parameters["can_interface"], info_.hardware_parameters["master_config"], + info_.hardware_parameters["can_interface_name"], info_.hardware_parameters["master_config"], info_.hardware_parameters["bus_config"], tmp_master_bin); auto drivers = device_container_->get_registered_drivers(); RCLCPP_INFO(kLogger, "Number of registered drivers: '%lu'", device_container_->count_drivers()); @@ -72,7 +72,7 @@ void Cia402System::initDeviceContainer() driver->register_rpdo_cb(rpdo_cb); RCLCPP_INFO( - kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%u'", + kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%x'", it->second->get_node_base_interface()->get_name(), it->first); } diff --git a/canopen_tests/launch/cia402_system.launch.py b/canopen_tests/launch/cia402_system.launch.py index 2a84159c..1e8201c8 100644 --- a/canopen_tests/launch/cia402_system.launch.py +++ b/canopen_tests/launch/cia402_system.launch.py @@ -66,7 +66,7 @@ def launch_setup(context, *args, **kwargs): ) # can interface name - can_interface = LaunchConfiguration("can_interface") + can_interface_name = LaunchConfiguration("can_interface_name") # robot description stuff description_package = LaunchConfiguration("description_package") @@ -91,8 +91,8 @@ def launch_setup(context, *args, **kwargs): "master_config:=", master_config, " ", - "can_interface:=", - can_interface, + "can_interface_name:=", + can_interface_name, " ", ] ) @@ -265,7 +265,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( - "can_interface", + "can_interface_name", default_value="vcan0", description="Interface name for can", ) diff --git a/canopen_tests/urdf/cia402_system/cia402_system.ros2_control.xacro b/canopen_tests/urdf/cia402_system/cia402_system.ros2_control.xacro index 0cf2c15d..c7ecf65b 100644 --- a/canopen_tests/urdf/cia402_system/cia402_system.ros2_control.xacro +++ b/canopen_tests/urdf/cia402_system/cia402_system.ros2_control.xacro @@ -6,7 +6,7 @@ prefix bus_config master_config - can_interface + can_interface_name master_bin"> @@ -14,7 +14,7 @@ canopen_ros2_control/Cia402System ${bus_config} ${master_config} - ${can_interface} + ${can_interface_name} "${master_bin}" diff --git a/canopen_tests/urdf/cia402_system/cia402_system.urdf.xacro b/canopen_tests/urdf/cia402_system/cia402_system.urdf.xacro index a328f9eb..eec26ac9 100644 --- a/canopen_tests/urdf/cia402_system/cia402_system.urdf.xacro +++ b/canopen_tests/urdf/cia402_system/cia402_system.urdf.xacro @@ -7,7 +7,7 @@ - + @@ -19,7 +19,7 @@ prefix="$(arg prefix)" bus_config="$(arg bus_config)" master_config="$(arg master_config)" - can_interface="$(arg can_interface)" + can_interface_name="$(arg can_interface_name)" master_bin="$(arg master_bin)" /> From d090887af33cccb643248160d8a21c40c18dd482 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 14 Jun 2023 16:43:58 +0200 Subject: [PATCH 12/59] Update changelofs Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 35 +++++++++++++++++++++++++- canopen_402_driver/CHANGELOG.rst | 23 +++++++++++++++++ canopen_base_driver/CHANGELOG.rst | 20 +++++++++++++++ canopen_core/CHANGELOG.rst | 16 ++++++++++++ canopen_fake_slaves/CHANGELOG.rst | 16 ++++++++++++ canopen_interfaces/CHANGELOG.rst | 11 +++++++- canopen_master_driver/CHANGELOG.rst | 15 +++++++++++ canopen_proxy_driver/CHANGELOG.rst | 21 ++++++++++++++++ canopen_ros2_control/CHANGELOG.rst | 18 +++++++++++-- canopen_ros2_controllers/CHANGELOG.rst | 12 ++++++++- canopen_tests/CHANGELOG.rst | 25 ++++++++++++++++++ canopen_utils/CHANGELOG.rst | 11 ++++++++ 12 files changed, 218 insertions(+), 5 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 77a7ff22..6395581b 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -4,7 +4,40 @@ Changelog for package canopen Forthcoming ----------- -* Adapt package xml +* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' +* Documentation for beta release (`#127 `_) + * Add documenation for cia301 + * Update ros2-control-interface.rst + Fix some format problems... Still have issue showing the example code + * Update ros2-control-interface.rst + Make the code rending okay.. Still have issues with the code blocks + * Update ros2-control-interface.rst + Minor changes + * Fix the format so that it builds + * Update ros2-control-interface.rst + * Update ros2-control-interface.rst + * Update configuration section + * Update configuration in documentation + * Update structure + * Application demos: trinamic stepper motor + * fix formatting and typo + * Integrate the documentation from the master branch + * Update ros2-control-interface.rst + * Application demos: PRBT robot with ros2_canopen + * Rename and add to index + --------- + Co-authored-by: Xi Huang + Co-authored-by: Vishnuprasad Prachandabhanu +* Use 'can_interface_name' consistently everywhere. +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Reduce processor load (`#111 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index e95a4d67..ebdb91be 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -4,6 +4,29 @@ Changelog for package canopen_402_driver Forthcoming ----------- +* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' +* Add device diagnostics messages (`#117 `_) + * Diagnostic msgs publisher for proxy diver + * diagnostic msgs for cia402 and motor interface + * Restructured message constructor + * Added option to enable/disable diagnostic + * Add diagnostic test in proxy driver + * Diagnostics test launch for proxy and cia402 + * proxy driver switched to diagnostic updater + * Complete diagnostic implementation for cia402 driver + * Update formating + * include code documentation +* Use 'can_interface_name' consistently everywhere. +* Fix interpolated mode switch in CiA402 (`#124 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Reduce processor load (`#111 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 718540a0..68eb6584 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -4,6 +4,26 @@ Changelog for package canopen_base_driver Forthcoming ----------- +* Add device diagnostics messages (`#117 `_) + * Diagnostic msgs publisher for proxy diver + * diagnostic msgs for cia402 and motor interface + * Restructured message constructor + * Added option to enable/disable diagnostic + * Add diagnostic test in proxy driver + * Diagnostics test launch for proxy and cia402 + * proxy driver switched to diagnostic updater + * Complete diagnostic implementation for cia402 driver + * Update formating + * include code documentation +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Reduce processor load (`#111 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index e9174b5e..fdd064a4 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -4,6 +4,22 @@ Changelog for package canopen_core Forthcoming ----------- +* Fix a number of build warnings (`#137 `_) + * Fix a number of build warnings + * Try rclcpp::QoS + * Set QoS in device_container + --------- +* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' +* Use 'can_interface_name' consistently everywhere. +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Reduce processor load (`#111 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 5c0f975b..bd55e5b5 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -4,6 +4,22 @@ Changelog for package canopen_fake_slaves Forthcoming ----------- +* Fix a number of build warnings (`#137 `_) + * Fix a number of build warnings + * Try rclcpp::QoS + * Set QoS in device_container + --------- +* Update package versions to 0.1.0 (`#133 `_) +* run interpolated mode in fake slave (`#126 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Add driver dictionaries (`#110 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 863c4d7f..23ef8901 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -4,7 +4,16 @@ Changelog for package canopen_interfaces Forthcoming ----------- -* Adapt package xml +* Update package versions to 0.1.0 (`#133 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Remove type indication from msg and srv interfaces (`#112 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 6b124810..75985b66 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -4,6 +4,21 @@ Changelog for package canopen_master_driver Forthcoming ----------- +* Fix a number of build warnings (`#137 `_) + * Fix a number of build warnings + * Try rclcpp::QoS + * Set QoS in device_container + --------- +* Update package versions to 0.1.0 (`#133 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Remove type indication from msg and srv interfaces (`#112 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index a1a3b3ac..55e556d2 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -4,6 +4,27 @@ Changelog for package canopen_proxy_driver Forthcoming ----------- +* Add device diagnostics messages (`#117 `_) + * Diagnostic msgs publisher for proxy diver + * diagnostic msgs for cia402 and motor interface + * Restructured message constructor + * Added option to enable/disable diagnostic + * Add diagnostic test in proxy driver + * Diagnostics test launch for proxy and cia402 + * proxy driver switched to diagnostic updater + * Complete diagnostic implementation for cia402 driver + * Update formating + * include code documentation +* Update package versions to 0.1.0 (`#133 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Remove type indication from msg and srv interfaces (`#112 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 150e69e2..b8607e61 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -4,7 +4,21 @@ Changelog for package canopen_ros2_control Forthcoming ----------- -* Add license files +* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' +* Fix formatting. +* Use 'can_interface_name' consistently everywhere. +* Bring ros2_control test launches to canopen_tests pkg (`#131 `_) +* Update package versions to 0.1.0 (`#133 `_) +* Fix interpolated mode switch in CiA402 (`#124 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Reduce processor load (`#111 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver @@ -155,4 +169,4 @@ Forthcoming * Print config paths on init. * Enable easy testing temporarily. * Introduce canopen system interface. -* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, livanov93 +* Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu, livanov93 diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index e5abfa5d..c3667d0e 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -4,7 +4,17 @@ Changelog for package canopen_ros2_controllers Forthcoming ----------- -* Add license files +* Update package versions to 0.1.0 (`#133 `_) +* Delete robot controller (`#132 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Reduce processor load (`#111 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 627798c5..e7469d7d 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -4,6 +4,31 @@ Changelog for package canopen_tests Forthcoming ----------- +* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' +* Add device diagnostics messages (`#117 `_) + * Diagnostic msgs publisher for proxy diver + * diagnostic msgs for cia402 and motor interface + * Restructured message constructor + * Added option to enable/disable diagnostic + * Add diagnostic test in proxy driver + * Diagnostics test launch for proxy and cia402 + * proxy driver switched to diagnostic updater + * Complete diagnostic implementation for cia402 driver + * Update formating + * include code documentation +* Use 'can_interface_name' consistently everywhere. +* Fix integration tests (`#136 `_) +* Bring ros2_control test launches to canopen_tests pkg (`#131 `_) +* Update package versions to 0.1.0 (`#133 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Enable simplified bus.yml format (`#115 `_) * Get slave eds and bin in node_canopen_driver * Add dictionary to base driver diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 0bb5a6cd..e89f81cb 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -4,6 +4,17 @@ Changelog for package canopen_utils Forthcoming ----------- +* Fix integration tests (`#136 `_) +* Update package versions to 0.1.0 (`#133 `_) +* Beta release preparations (`#120 `_) + * Improve lely compilation time + * Bump lely_core_librries to version 2.3.2 + * Add license files + * Adapt package xml + * Add changelogs - forthcoming for now. + * Update readme + * Add apacje-2.0 license notifications to files + --------- * Include rpdo/tpdo test in launch_test. (`#98 `_) * Implement rpdo/tpdo test * Removed unnecessary files From dfd5fc0b1e9fa909f4c0cb2f19c26947dfc997cd Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 14 Jun 2023 16:48:01 +0200 Subject: [PATCH 13/59] set versions Signed-off-by: Christoph Hellmann Santos --- canopen/package.xml | 2 +- canopen_402_driver/package.xml | 2 +- canopen_base_driver/package.xml | 2 +- canopen_core/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/canopen/package.xml b/canopen/package.xml index ed835de3..37377d41 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.0.1 + 0.1.0 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index d8980e21..a24b2b30 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.0.1 + 0.1.0 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index 40858fe1..b7b1e0cd 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.0.1 + 0.1.0 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/package.xml b/canopen_core/package.xml index 906695c4..3b0db52e 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.0.1 + 0.1.0 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 From 4a39f25558ea90a709e30601cdc088666ee4a703 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 14 Jun 2023 20:44:06 +0200 Subject: [PATCH 14/59] Update changelog Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 181 +------------------ canopen_402_driver/CHANGELOG.rst | 225 +---------------------- canopen_base_driver/CHANGELOG.rst | 177 +----------------- canopen_core/CHANGELOG.rst | 237 +------------------------ canopen_fake_slaves/CHANGELOG.rst | 64 +------ canopen_interfaces/CHANGELOG.rst | 59 +----- canopen_master_driver/CHANGELOG.rst | 44 +---- canopen_proxy_driver/CHANGELOG.rst | 143 +-------------- canopen_ros2_control/CHANGELOG.rst | 166 +---------------- canopen_ros2_controllers/CHANGELOG.rst | 103 +---------- canopen_tests/CHANGELOG.rst | 188 +------------------- canopen_utils/CHANGELOG.rst | 56 +----- lely_core_libraries/CHANGELOG.rst | 78 +------- 13 files changed, 13 insertions(+), 1708 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 6395581b..ced1e303 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -4,184 +4,5 @@ Changelog for package canopen Forthcoming ----------- -* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' -* Documentation for beta release (`#127 `_) - * Add documenation for cia301 - * Update ros2-control-interface.rst - Fix some format problems... Still have issue showing the example code - * Update ros2-control-interface.rst - Make the code rending okay.. Still have issues with the code blocks - * Update ros2-control-interface.rst - Minor changes - * Fix the format so that it builds - * Update ros2-control-interface.rst - * Update ros2-control-interface.rst - * Update configuration section - * Update configuration in documentation - * Update structure - * Application demos: trinamic stepper motor - * fix formatting and typo - * Integrate the documentation from the master branch - * Update ros2-control-interface.rst - * Application demos: PRBT robot with ros2_canopen - * Rename and add to index - --------- - Co-authored-by: Xi Huang - Co-authored-by: Vishnuprasad Prachandabhanu -* Use 'can_interface_name' consistently everywhere. -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Reduce processor load (`#111 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Correct repo link (`#94 `_) -* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) - * Add Interpolated Position Mode (linear only, no PT or PVT) - * add interpolated position mode to system interface - * Add interpolated position mode to controllers. - * Add to interpolated position mode to documentation - --------- -* Merge branch 'destogl-patch-2' -* Format updates -* Merge branch 'master' into patch-2 -* Merge branch 'bjsowa-master' -* add short documentation -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Documentation for streamlined design (`#67 `_) - * Add canopen_core tests (90% coverage) - * Restructure and add plantuml - * Changes to quickstart/configuration - * Revert "Add canopen_core tests (90% coverage)" as it is not needed. - This reverts commit 771c498347f190777fb28edfd5044b96cbfd7bf0. - * Create custom driver documentation - * Remove breathe api reference and use doxygen - * Update interface and naming information for drivers - * Update test documentation -* Update deployment -* undo renaming can_interface_name -> can_interface -* Integration with ros2_control -* Restructure documentation (`#37 `_) - * Document device container node - * Document lely_master_bridge - * Document Lifecycle Device Container - * Document Lifecycle Device Manager - * Document LifecyleMasterNode - * Document Master Node - * Fix error - * Document lifecycle base driver - * Document lely bridge - * Document canopen_proxy_driver - * Document canopen_402_driver - * Update sphinx documentation -* make documentation on test with ros2_control more detailed - Make the test documentation a small example with explanations of the functionality. -* Merge branch 'canopen-system-interface' into canopen-controller -* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface - Add generic system interface for ros2_control -* Add documentation about testing ros2_control generic interface. -* Update dcfgen cmake integration (`#41 `_) -* Update configuration-package.rst -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability -* Update running-configuration-package.rst (`#29 `_) - Add setup steps for candleLight USB-CAN adapter -* Add information about root rights (`#28 `_) - * add information about root rights - * Add information about running a configuration pkg -* Add configuration (`#17 `_) -* Further additions to documentation (`#16 `_) - * change section titles in configuration package.rst - * add system-interface graphic and other change to configure-package.rst - * add description to Proxy Driver - * Add ros2_canopen logo -* Configuration manager integration (`#14 `_) - * Add longer startup delay and test documentation - * Add speed and position publisher - * Create Configuration Manager - * make MasterNode a component and add configuration manager functionalities - * add configuration manager functionalities - * add configuration manger functionalities - * Add documentation for Configuration Manager - * add info messages and documentation - * update launch files and configuration fiels - * add can_utils package - * add info text - * simplify dependencies - * remove tests from can_utils - * avoid tests for canopen_utils - * changes info logging and adds nmt and sdo tests - * add tests - * remove launch_tests from cmake -* Update configuration package tutorial (`#5 `_) - * Update configuration package tutorial - * improve bus configuration, development objectives and other documentation and add css for tables - * Reviewed Configuration Package Guide - * Add alpha test description - * Add test decriptions - * Update device manager description with new graphics -* Update documentation -* Merge branch 'licenses' into 'master' - add licenses to each package - See merge request ipa326/ros-industrial/ros2_canopen!22 -* rename ros2_canopen package to canopen -* add licenses to each package -* Merge branch 'renaming' into 'master' - Update package names to fit ROS2 naming rules better - See merge request ipa326/ros-industrial/ros2_canopen!21 -* rename packages to fit ROS2 conventions better +* Created package * Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index ebdb91be..b718cc67 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -4,228 +4,5 @@ Changelog for package canopen_402_driver Forthcoming ----------- -* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' -* Add device diagnostics messages (`#117 `_) - * Diagnostic msgs publisher for proxy diver - * diagnostic msgs for cia402 and motor interface - * Restructured message constructor - * Added option to enable/disable diagnostic - * Add diagnostic test in proxy driver - * Diagnostics test launch for proxy and cia402 - * proxy driver switched to diagnostic updater - * Complete diagnostic implementation for cia402 driver - * Update formating - * include code documentation -* Use 'can_interface_name' consistently everywhere. -* Fix interpolated mode switch in CiA402 (`#124 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Reduce processor load (`#111 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Add driver dictionaries (`#110 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - --------- -* Motor Profile Updates (`#101 `_) - * Extend and fix info statement. - * Fix service handler overwriting. - * Consider enum 3 as profiled velocity. Remove some code duplication by reusing private setters in service cbs. Create setter for interpolated position mode. - * Fix cyclic position mode. - * Simplify write method cases defined by mode of op. -* Merge pull request `#100 `_ from ipa-vsp/bugfix/fix-vel-pos-scaling - Proper scaling from the device -* proper vel and pos scaling from device -* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) - * Boost lock free queue implemetation - * include boost libraries in CMakelists - * Testing rpdo/tpdo ping pond - * pre-commit changes - * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking - * Fixed typo - * pre-commit update - * FIxed: properly export Boost libraries - * Update code documentation -* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) - * Add Interpolated Position Mode (linear only, no PT or PVT) - * add interpolated position mode to system interface - * Add interpolated position mode to controllers. - * Add to interpolated position mode to documentation - --------- -* Simplify 402 driver (`#89 `_) - * Split motor.hpp and motor.cpp into different files - * Fix missing symbol error - --------- -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Remove false license statements (`#76 `_) - * Remove false license statements -* Scaling factors for position and velocity (`#74 `_) - * Introduce scaling factors -* Merge branch 'livanov93-motor-profile' -* Set target based on condition. -* Expose 402 main functionalities to ros2_control system interface. -* Extend 402 functions via public methods - same as callback based ones. -* Adapt 402 hardware interface to device container getter. -* Add position and speed getter. -* Add unit tests for canopen_core (`#64 `_) - * Testing changes to canopen_core - * Testing changes to canopen_base_driver and canopen_402_driver - * Add canopen_core tests (90% coverage) - * Fix DriverException error in canopen_402_driver - * Catch errors in nmt and rpdo listeners - * Fix naming issues - * Fix deactivate transition - * Fix unclean shutdown -* Publish to joint_states, not joint_state (`#63 `_) - Co-authored-by: G.A. vd. Hoorn - Co-authored-by: Christoph Hellmann Santos -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* Fix 402 issues from testing -* Streamline logging -* Fix node_canopen_402_drivers add_to_master and handlers -* Fix get speed and get position -* Feature parity for lifecycle nodes -* Add 402 driver functions for ros2_control -* Add master dcfs and remove from gitignore -* Add device container and general changes to make things work. -* canopen_402_driver adaption to new framework -* Add in code documentation for canopen_core (`#53 `_) - * Document device container node - * Document lely_master_bridge - * Document Lifecycle Device Container - * Document Lifecycle Device Manager - * Document LifecyleMasterNode - * Document Master Node - * Fix error - * Document lifecycle base driver - * Document lely bridge - * Document canopen_proxy_driver - * Document canopen_402_driver -* Add configuration parameter passthrough (`#52 `_) -* Publish joint state instead of velocity topics (`#47 `_) - * disable loader service - * add custom target/command and install to macro - * publish jointstate - * correct variable name squiggle - * Minor changes to driver and slave - * Update lely core library - * Add sensor_msgs to dependencies - * Remove artifacts - * Remove some artifacts -* Solve Boot Error (`#49 `_) -* Remove some unecessary changes. -* Remove pedantic cmake flags. -* fix ament_export_libraries (`#45 `_) -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability -* Fix remote object bug in canopen_402_driver. (`#40 `_) -* Merge pull request `#1 `_ from livanov93/canopen-system-interface - [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities -* Remove pedantic cmake flags. -* Configuration manager integration (`#14 `_) - * Add longer startup delay and test documentation - * Add speed and position publisher - * Create Configuration Manager - * make MasterNode a component and add configuration manager functionalities - * add configuration manager functionalities - * add configuration manger functionalities - * Add documentation for Configuration Manager - * add info messages and documentation - * update launch files and configuration fiels - * add can_utils package - * add info text - * simplify dependencies - * remove tests from can_utils - * avoid tests for canopen_utils - * changes info logging and adds nmt and sdo tests - * add tests - * remove launch_tests from cmake -* Merge branch 'licenses' into 'master' - add licenses to each package - See merge request ipa326/ros-industrial/ros2_canopen!22 -* update package descriptions -* add licenses to each package -* Merge branch 'renaming' into 'master' - Update package names to fit ROS2 naming rules better - See merge request ipa326/ros-industrial/ros2_canopen!21 -* add missing variable for cyclic position mode -* store tests of proxy driver in canopen_proxy_driver -* rename packages to fit ROS2 conventions better +* Created package * Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, G.A. vd. Hoorn, Lovro, Vishnuprasad Prachandabhanu, livanov93 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 68eb6584..e32a1841 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -4,180 +4,5 @@ Changelog for package canopen_base_driver Forthcoming ----------- -* Add device diagnostics messages (`#117 `_) - * Diagnostic msgs publisher for proxy diver - * diagnostic msgs for cia402 and motor interface - * Restructured message constructor - * Added option to enable/disable diagnostic - * Add diagnostic test in proxy driver - * Diagnostics test launch for proxy and cia402 - * proxy driver switched to diagnostic updater - * Complete diagnostic implementation for cia402 driver - * Update formating - * include code documentation -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Reduce processor load (`#111 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Add driver dictionaries (`#110 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - --------- -* Fix stack smashing (`#103 `_) -* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) - * Boost lock free queue implemetation - * include boost libraries in CMakelists - * Testing rpdo/tpdo ping pond - * pre-commit changes - * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking - * Fixed typo - * pre-commit update - * FIxed: properly export Boost libraries - * Update code documentation -* Add EMCY callback to base driver (`#91 `_) -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Add unit tests for canopen_core (`#64 `_) - * Testing changes to canopen_core - * Testing changes to canopen_base_driver and canopen_402_driver - * Add canopen_core tests (90% coverage) - * Fix DriverException error in canopen_402_driver - * Catch errors in nmt and rpdo listeners - * Fix naming issues - * Fix deactivate transition - * Fix unclean shutdown -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* Streamline logging -* Fix tests base driver -* Feature parity for lifecycle nodes -* Add master dcfs and remove from gitignore -* Integration with ros2_control -* Add device container and general changes to make things work. -* Change canopen_base_driver for templating problems -* Make changes to canopen_base_driver for new structure -* Add in code documentation for canopen_core (`#53 `_) - * Document device container node - * Document lely_master_bridge - * Document Lifecycle Device Container - * Document Lifecycle Device Manager - * Document LifecyleMasterNode - * Document Master Node - * Fix error - * Document lifecycle base driver - * Document lely bridge - * Document canopen_proxy_driver - * Document canopen_402_driver -* Add configuration parameter passthrough (`#52 `_) -* Solve Boot Error (`#49 `_) -* Remove some unecessary changes. -* Remove pedantic cmake flags. -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability -* Merge pull request `#1 `_ from livanov93/canopen-system-interface - [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities -* Remove pedantic cmake flags. -* Configuration manager integration (`#14 `_) - * Add longer startup delay and test documentation - * Add speed and position publisher - * Create Configuration Manager - * make MasterNode a component and add configuration manager functionalities - * add configuration manager functionalities - * add configuration manger functionalities - * Add documentation for Configuration Manager - * add info messages and documentation - * update launch files and configuration fiels - * add can_utils package - * add info text - * simplify dependencies - * remove tests from can_utils - * avoid tests for canopen_utils - * changes info logging and adds nmt and sdo tests - * add tests - * remove launch_tests from cmake -* Merge branch 'licenses' into 'master' - add licenses to each package - See merge request ipa326/ros-industrial/ros2_canopen!22 -* update package descriptions -* add licenses to each package -* Merge branch 'renaming' into 'master' - Update package names to fit ROS2 naming rules better - See merge request ipa326/ros-industrial/ros2_canopen!21 -* rename packages to fit ROS2 conventions better +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index fdd064a4..aa430b4f 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -4,240 +4,5 @@ Changelog for package canopen_core Forthcoming ----------- -* Fix a number of build warnings (`#137 `_) - * Fix a number of build warnings - * Try rclcpp::QoS - * Set QoS in device_container - --------- -* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' -* Use 'can_interface_name' consistently everywhere. -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Reduce processor load (`#111 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Add driver dictionaries (`#110 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - --------- -* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) - * Boost lock free queue implemetation - * include boost libraries in CMakelists - * Testing rpdo/tpdo ping pond - * pre-commit changes - * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking - * Fixed typo - * pre-commit update - * FIxed: properly export Boost libraries - * Update code documentation -* Add EMCY callback to base driver (`#91 `_) -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge branch 'bjsowa-master' -* Don't treat options as driver -* Merge branch 'master' of github.com:bjsowa/ros2_canopen into bjsowa-master -* Don't treat options section as another device -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Disable device container tests (`#77 `_) -* Handle demand set master failure (`#70 `_) - * adapt fake cia402 slave - * Add retries for demand_set_master in case of failure -* Doxygen documentation for canopen_core (`#78 `_) - * canopen_core in code documentation - * Some more documentation -* intra_process_comms -* Doxygen documentation for canopen_core (`#78 `_) - * canopen_core in code documentation - * Some more documentation -* intra_process_comms -* Add unit tests for canopen_core (`#64 `_) - * Testing changes to canopen_core - * Testing changes to canopen_base_driver and canopen_402_driver - * Add canopen_core tests (90% coverage) - * Fix DriverException error in canopen_402_driver - * Catch errors in nmt and rpdo listeners - * Fix naming issues - * Fix deactivate transition - * Fix unclean shutdown -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* undo renaming can_interface_name -> can_interface -* Undo formatting in ros2_control -* Add lifecycle manager to device_container library -* Remove canopen_lifecycle.launch.py as it i no longer needed. -* Streamline logging -* Integrate lifecycle manager -* Fix tests canopen_core -* Fix canopen_master_driver for explicit instantiation -* Add CanopenDriverInterface Documentation -* Add master dcfs and remove from gitignore -* add node interface accessor and lifecycle information to drivers -* Add type accessor functions to device_container -* Integration with ros2_control -* Add function to device container -* Add device container and general changes to make things work. -* Update CmakeFile of canopen core -* Add tests to canopen core -* Remove device and do some renaming -* Add node base classes -* Add errors -* Add core node interfaces -* Add in code documentation for canopen_core (`#53 `_) - * Document device container node - * Document lely_master_bridge - * Document Lifecycle Device Container - * Document Lifecycle Device Manager - * Document LifecyleMasterNode - * Document Master Node - * Fix error - * Document lifecycle base driver - * Document lely bridge - * Document canopen_proxy_driver - * Document canopen_402_driver -* Add configuration parameter passthrough (`#52 `_) -* Publish joint state instead of velocity topics (`#47 `_) - * disable loader service - * add custom target/command and install to macro - * publish jointstate - * correct variable name squiggle - * Minor changes to driver and slave - * Update lely core library - * Add sensor_msgs to dependencies - * Remove artifacts - * Remove some artifacts -* Disable dynamic loading for containers (`#50 `_) - * disable loader service - * Remove artifacts -* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface - Add generic system interface for ros2_control -* Update canopen_core/CMakeLists.txt -* Fix merging issues. -* Merge remote-tracking branch 'livanov/fix-dependencies' into canopen-system-interface -* Remove some unecessary changes. -* Apply suggestions from code review -* Add nmt and rpdo callbacks. -* Start device manager in system interface. -* fix-ifndef-directive (`#43 `_) -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability -* Merge pull request `#1 `_ from livanov93/canopen-system-interface - [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities -* Apply suggestions from code review -* Remove pedantic cmake flags. -* Add nmt and rpdo callbacks. -* Start device manager in system interface. -* Add yaml_cpp_vendor as dependency for master_node (`#18 `_) - * commented out the yaml_cpp vendor since it doesnt let it build on the 20.04 machine - * Update CMakeLists.txt - * Update CMakeLists.txt - * Update CMakeLists.txt - Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> -* Configuration manager integration (`#14 `_) - * Add longer startup delay and test documentation - * Add speed and position publisher - * Create Configuration Manager - * make MasterNode a component and add configuration manager functionalities - * add configuration manager functionalities - * add configuration manger functionalities - * Add documentation for Configuration Manager - * add info messages and documentation - * update launch files and configuration fiels - * add can_utils package - * add info text - * simplify dependencies - * remove tests from can_utils - * avoid tests for canopen_utils - * changes info logging and adds nmt and sdo tests - * add tests - * remove launch_tests from cmake -* Add Industrial CI for foxy and galactic (`#10 `_) - * Add ci - * remove rclcpp_lifecycle stuff from CMAKElists.txt - * add rclcpp_lifecycle and msgs - * add galactic to ci and limit ci to prs and pushes to master -* Merge branch 'licenses' into 'master' - add licenses to each package - See merge request ipa326/ros-industrial/ros2_canopen!22 -* add header guards -* update package descriptions -* add license in files -* Merge branch 'renaming' into 'master' - Update package names to fit ROS2 naming rules better - See merge request ipa326/ros-industrial/ros2_canopen!21 -* store tests of proxy driver in canopen_proxy_driver -* rename packages to fit ROS2 conventions better +* Created package * Contributors: Aulon Bajrami, Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index bd55e5b5..ba67cd44 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -4,67 +4,5 @@ Changelog for package canopen_fake_slaves Forthcoming ----------- -* Fix a number of build warnings (`#137 `_) - * Fix a number of build warnings - * Try rclcpp::QoS - * Set QoS in device_container - --------- -* Update package versions to 0.1.0 (`#133 `_) -* run interpolated mode in fake slave (`#126 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Add driver dictionaries (`#110 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - --------- -* Include rpdo/tpdo test in launch_test. (`#98 `_) - * Implement rpdo/tpdo test - * Removed unnecessary files -* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) - * Boost lock free queue implemetation - * include boost libraries in CMakelists - * Testing rpdo/tpdo ping pond - * pre-commit changes - * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking - * Fixed typo - * pre-commit update - * FIxed: properly export Boost libraries - * Update code documentation -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Remove references to sympy.true (`#84 `_) - Co-authored-by: James Ward -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Clean cia402 fake shutdown (`#72 `_) - * adapt fake cia402 slave -* intra_process_comms -* intra_process_comms -* Rename canopen_mock_slave package to canopen_fake_slaves (`#66 `_) - * Testing changes to canopen_core - * Testing changes to canopen_base_driver and canopen_402_driver - * Add canopen_core tests (90% coverage) - * Fix DriverException error in canopen_402_driver - * Catch errors in nmt and rpdo listeners - * Fix naming issues - * Fix deactivate transition - * Fix unclean shutdown - * Rename canopen_mock_slave to canopen_fake_slaves - * Build flage CANOPEN_ENABLED for disabling tests on CI. +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, James Ward, Vishnuprasad Prachandabhanu diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 23ef8901..ab5349c5 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -4,62 +4,5 @@ Changelog for package canopen_interfaces Forthcoming ----------- -* Update package versions to 0.1.0 (`#133 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Apply suggestions from code review -* Remove pedantic cmake flags. -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability -* Apply suggestions from code review -* Remove pedantic cmake flags. -* Merge branch 'licenses' into 'master' - add licenses to each package - See merge request ipa326/ros-industrial/ros2_canopen!22 -* rename canopen -* add licenses to each package -* Merge branch 'renaming' into 'master' - Update package names to fit ROS2 naming rules better - See merge request ipa326/ros-industrial/ros2_canopen!21 -* rename packages to fit ROS2 conventions better +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 75985b66..ed0a8dbd 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -4,47 +4,5 @@ Changelog for package canopen_master_driver Forthcoming ----------- -* Fix a number of build warnings (`#137 `_) - * Fix a number of build warnings - * Try rclcpp::QoS - * Set QoS in device_container - --------- -* Update package versions to 0.1.0 (`#133 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* undo renaming can_interface_name -> can_interface -* Streamline logging -* Fix canopen_master_driver tests -* Fix canopen_master_driver for explicit instantiation -* Feature parity for lifecycle nodes -* Add master dcfs and remove from gitignore -* Add device container and general changes to make things work. -* Add canopen_master_driver package and contents +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 55e556d2..eb607e68 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -4,146 +4,5 @@ Changelog for package canopen_proxy_driver Forthcoming ----------- -* Add device diagnostics messages (`#117 `_) - * Diagnostic msgs publisher for proxy diver - * diagnostic msgs for cia402 and motor interface - * Restructured message constructor - * Added option to enable/disable diagnostic - * Add diagnostic test in proxy driver - * Diagnostics test launch for proxy and cia402 - * proxy driver switched to diagnostic updater - * Complete diagnostic implementation for cia402 driver - * Update formating - * include code documentation -* Update package versions to 0.1.0 (`#133 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Add driver dictionaries (`#110 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - --------- -* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) - * Boost lock free queue implemetation - * include boost libraries in CMakelists - * Testing rpdo/tpdo ping pond - * pre-commit changes - * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking - * Fixed typo - * pre-commit update - * FIxed: properly export Boost libraries - * Update code documentation -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* Feature parity for lifecycle nodes -* Add master dcfs and remove from gitignore -* Integration with ros2_control -* Add device container and general changes to make things work. -* Update header guards -* Add canopen_proxy_driver with new framework -* Add in code documentation for canopen_core (`#53 `_) - * Document device container node - * Document lely_master_bridge - * Document Lifecycle Device Container - * Document Lifecycle Device Manager - * Document LifecyleMasterNode - * Document Master Node - * Fix error - * Document lifecycle base driver - * Document lely bridge - * Document canopen_proxy_driver - * Document canopen_402_driver -* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface - Add generic system interface for ros2_control -* Merge remote-tracking branch 'livanov/fix-dependencies' into canopen-system-interface -* Remove unneccesary deps. -* Apply suggestions from code review -* Expose necessary stuff from proxy driver. -* Add nmt and rpdo callbacks. -* Start device manager in system interface. -* Enable easy testing temporarily. -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability -* Merge pull request `#1 `_ from livanov93/canopen-system-interface - [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities -* Apply suggestions from code review -* Expose necessary stuff from proxy driver. -* Add nmt and rpdo callbacks. -* Enable easy testing temporarily. -* Configuration manager integration (`#14 `_) - * Add longer startup delay and test documentation - * Add speed and position publisher - * Create Configuration Manager - * make MasterNode a component and add configuration manager functionalities - * add configuration manager functionalities - * add configuration manger functionalities - * Add documentation for Configuration Manager - * add info messages and documentation - * update launch files and configuration fiels - * add can_utils package - * add info text - * simplify dependencies - * remove tests from can_utils - * avoid tests for canopen_utils - * changes info logging and adds nmt and sdo tests - * add tests - * remove launch_tests from cmake -* Merge branch 'licenses' into 'master' - add licenses to each package - See merge request ipa326/ros-industrial/ros2_canopen!22 -* update package descriptions -* add licenses to each package -* Merge branch 'renaming' into 'master' - Update package names to fit ROS2 naming rules better - See merge request ipa326/ros-industrial/ros2_canopen!21 -* use proxy.yml instead simple.yml in CMakeLists.txt -* store tests of proxy driver in canopen_proxy_driver -* rename packages to fit ROS2 conventions better +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index b8607e61..08ede8b6 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -4,169 +4,5 @@ Changelog for package canopen_ros2_control Forthcoming ----------- -* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' -* Fix formatting. -* Use 'can_interface_name' consistently everywhere. -* Bring ros2_control test launches to canopen_tests pkg (`#131 `_) -* Update package versions to 0.1.0 (`#133 `_) -* Fix interpolated mode switch in CiA402 (`#124 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Reduce processor load (`#111 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Motor Profile Updates (`#101 `_) - * Extend and fix info statement. - * Fix service handler overwriting. - * Consider enum 3 as profiled velocity. Remove some code duplication by reusing private setters in service cbs. Create setter for interpolated position mode. - * Fix cyclic position mode. - * Simplify write method cases defined by mode of op. -* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) - * Add Interpolated Position Mode (linear only, no PT or PVT) - * add interpolated position mode to system interface - * Add interpolated position mode to controllers. - * Add to interpolated position mode to documentation - --------- -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Merge branch 'livanov93-motor-profile' -* Remove scalers -* Fix internal launch test. -* Add virtual can example for cia 402. -* Fix joint states scaling. -* Update runtime deps. -* State and command interfaces. -* Fix feedback for services for proxy driver and controlller. -* Handle init, recover, halt. Switch modes. -* Set target based on condition. -* Duplicate some code for configure, init, write phase from proxy driver. -* Add basic read and write. Divide targets into position, velocity, effort interfaces. -* Update proxy canopen system. -* Add vel and pos interfaves. -* Expose 402 main functionalities to ros2_control system interface. -* Prepare read/write/ -* Adapt 402 hardware interface to device container getter. -* Fix public fcn visibility. -* To protected members for easier inheritance policy. -* Update dependencies. -* State and command interfaces. -* Add position and speed getter. -* Add bare-bone 402 profile system interface. -* Rename canopen_mock_slave package to canopen_fake_slaves (`#66 `_) - * Testing changes to canopen_core - * Testing changes to canopen_base_driver and canopen_402_driver - * Add canopen_core tests (90% coverage) - * Fix DriverException error in canopen_402_driver - * Catch errors in nmt and rpdo listeners - * Fix naming issues - * Fix deactivate transition - * Fix unclean shutdown - * Rename canopen_mock_slave to canopen_fake_slaves - * Build flage CANOPEN_ENABLED for disabling tests on CI. -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* undo renaming can_interface_name -> can_interface -* Undo formatting in ros2_control -* Integration with ros2_control -* Merge pull request `#54 `_ from StoglRobotics-forks/canopen-system-interface - Add generic Controller for Canopen -* Apply suggestions from code review - Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> -* Merge pull request `#2 `_ from livanov93/canopen-controller - Canopen controller -* Merge branch 'canopen-system-interface' into canopen-controller -* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface - Add generic system interface for ros2_control -* Integrate launch files into existing launch files in canopen_mock_slaves & canopen_tests - Merge pull request `#6 `_ from ipa-cmh/canopen-system-interface -* Adapt canopen_system.launch.py for 2 nodes -* Remove bus.yml -* further changes -* update package.xml -* Reorganise test launch system -* Apply suggestions from code review -* Add service one shot mechanisms. -* Expose controller plugin. Start canopen proxy controller instance in example. -* Simplify configuration folder and use existing .eds, .dcf file. Improve test launch file. Update runtime deps. -* Disable test because they can not be eaisly tested. -* Update visibility-control macros. -* Merge remote-tracking branch 'livanov/from-init-to-configure' into canopen-system-interface -* Merge remote-tracking branch 'livanov/fix-dependencies' into canopen-system-interface -* Rename "device manager" to "device container" and disable test because it is now working in the current setup. -* Remove unneccesary deps. -* Add missig dependencies and execute tests only when testing. -* Apply suggestions from code review -* Add internal caching structures for canopen nodes. -* Add nmt and rpdo callbacks. -* Start device manager in system interface. -* Add device manager and executor. -* Print config paths on init. -* Enable easy testing temporarily. -* Introduce canopen system interface. -* Move device manager instantation into on_config. -* Merge pull request `#4 `_ from livanov93/fix-dependencies - [canopen_ros2_control] Dependency fix -* Fix dependencies for canopen_ros2_control. -* Merge pull request `#1 `_ from livanov93/canopen-system-interface - [WIP] Add ros2_control system interface wrapper for ros2_canopen functionalities -* Apply suggestions from code review -* Add internal caching structures for canopen nodes. -* Add nmt and rpdo callbacks. -* Start device manager in system interface. -* Add device manager and executor. -* Print config paths on init. -* Enable easy testing temporarily. -* Introduce canopen system interface. +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu, livanov93 diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index c3667d0e..6abbe848 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -4,106 +4,5 @@ Changelog for package canopen_ros2_controllers Forthcoming ----------- -* Update package versions to 0.1.0 (`#133 `_) -* Delete robot controller (`#132 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Reduce processor load (`#111 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Remove type indication from msg and srv interfaces (`#112 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - --------- -* Motor Profile Updates (`#101 `_) - * Extend and fix info statement. - * Fix service handler overwriting. - * Consider enum 3 as profiled velocity. Remove some code duplication by reusing private setters in service cbs. Create setter for interpolated position mode. - * Fix cyclic position mode. - * Simplify write method cases defined by mode of op. -* Add Interpolated Position Mode (linear only, no PT or PVT) (`#90 `_) - * Add Interpolated Position Mode (linear only, no PT or PVT) - * add interpolated position mode to system interface - * Add interpolated position mode to controllers. - * Add to interpolated position mode to documentation - --------- -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Merge branch 'livanov93-motor-profile' -* Fix proxy test. -* Fix internal launch test. -* Fix joint states scaling. -* Update runtime deps. -* Better handling of base class on_methods. -* Add services for one shot interfaces in cia402 profile. -* State and command interfaces. -* Add base function ret values first. -* Prepare cia 402 device controller. -* Fix feedback for services for proxy driver and controlller. -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* Integration with ros2_control -* Merge pull request `#54 `_ from StoglRobotics-forks/canopen-system-interface - Add generic Controller for Canopen -* Implement review requests regarding tests. -* Introduce tests. Adapt proxy controller for easier testing. -* Add service qos specific profile. -* Correct macro names. -* Apply suggestions from code review - Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> -* Merge pull request `#2 `_ from livanov93/canopen-controller - Canopen controller -* Add service one shot mechanisms. -* Add publishing of rpdo and nmt state. -* Expose controller plugin. Start canopen proxy controller instance in example. -* Add dummy services, rt publishers and subscribers to proxy controller. -* Add template for canopen proxy controller. +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Dr.-Ing. Denis Štogl, Lovro, livanov93 diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index e7469d7d..5a23a4d7 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -4,191 +4,5 @@ Changelog for package canopen_tests Forthcoming ----------- -* Merge branch 'StoglRobotics-forks-use-can-interface-name-consistently' -* Add device diagnostics messages (`#117 `_) - * Diagnostic msgs publisher for proxy diver - * diagnostic msgs for cia402 and motor interface - * Restructured message constructor - * Added option to enable/disable diagnostic - * Add diagnostic test in proxy driver - * Diagnostics test launch for proxy and cia402 - * proxy driver switched to diagnostic updater - * Complete diagnostic implementation for cia402 driver - * Update formating - * include code documentation -* Use 'can_interface_name' consistently everywhere. -* Fix integration tests (`#136 `_) -* Bring ros2_control test launches to canopen_tests pkg (`#131 `_) -* Update package versions to 0.1.0 (`#133 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Enable simplified bus.yml format (`#115 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - * Add cogen - * Add example usage for cogen - * Remove explicit path - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Reduce processor load (`#111 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Add driver dictionaries (`#110 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - --------- -* Include rpdo/tpdo test in launch_test. (`#98 `_) - * Implement rpdo/tpdo test - * Removed unnecessary files -* Implemented thread-safe queue for rpdo and emcy listener (`#97 `_) - * Boost lock free queue implemetation - * include boost libraries in CMakelists - * Testing rpdo/tpdo ping pond - * pre-commit changes - * Bugfix: implemented timeout for wait_and_pop to avoid thread blocking - * Fixed typo - * pre-commit update - * FIxed: properly export Boost libraries - * Update code documentation -* Better organize dependencies (`#88 `_) -* Merge branch 'master' into patch-2 -* Merge branch 'bjsowa-master' -* Add dcf_path to bus.ymls -* Merge branch 'master' of github.com:bjsowa/ros2_canopen into bjsowa-master -* Remove references to sympy.true (`#84 `_) - Co-authored-by: James Ward -* Use options section in test bus config files -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Use @BUS_CONFIG_PATH@ variable in bus configuration files -* intra_process_comms -* intra_process_comms -* Rename canopen_mock_slave package to canopen_fake_slaves (`#66 `_) - * Testing changes to canopen_core - * Testing changes to canopen_base_driver and canopen_402_driver - * Add canopen_core tests (90% coverage) - * Fix DriverException error in canopen_402_driver - * Catch errors in nmt and rpdo listeners - * Fix naming issues - * Fix deactivate transition - * Fix unclean shutdown - * Rename canopen_mock_slave to canopen_fake_slaves - * Build flage CANOPEN_ENABLED for disabling tests on CI. -* Merge pull request `#60 `_ from ipa-cmh/merge-non-lifecycle-and-lifecycle-drivers - Streamline driver and master infrastructure -* undo renaming can_interface_name -> can_interface -* Fix integration tests -* Integration with ros2_control -* Add device container and general changes to make things work. -* Merge branch 'canopen-system-interface' into canopen-controller -* Add configuration parameter passthrough (`#52 `_) -* Publish joint state instead of velocity topics (`#47 `_) - * disable loader service - * add custom target/command and install to macro - * publish jointstate - * correct variable name squiggle - * Minor changes to driver and slave - * Update lely core library - * Add sensor_msgs to dependencies - * Remove artifacts - * Remove some artifacts -* Merge pull request `#33 `_ from StoglRobotics-forks/canopen-system-interface - Add generic system interface for ros2_control -* Integrate launch files into existing launch files in canopen_mock_slaves & canopen_tests - Merge pull request `#6 `_ from ipa-cmh/canopen-system-interface -* further changes -* Reorganise test launch system -* Add tests to canopen_tests -* Add tests for lifecycle and normal operation (`#44 `_) -* Update dcfgen cmake integration (`#41 `_) -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, James Ward, Vishnuprasad Prachandabhanu diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index e89f81cb..d0895d07 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -4,59 +4,5 @@ Changelog for package canopen_utils Forthcoming ----------- -* Fix integration tests (`#136 `_) -* Update package versions to 0.1.0 (`#133 `_) -* Beta release preparations (`#120 `_) - * Improve lely compilation time - * Bump lely_core_librries to version 2.3.2 - * Add license files - * Adapt package xml - * Add changelogs - forthcoming for now. - * Update readme - * Add apacje-2.0 license notifications to files - --------- -* Include rpdo/tpdo test in launch_test. (`#98 `_) - * Implement rpdo/tpdo test - * Removed unnecessary files -* Merge branch 'master' into patch-2 -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Add lifecycle to service-based operation (`#34 `_) - * Add check if remote object already exists to avoid multiple objects with same target. - * Renaming and changes to MasterNode - * restrucutring for lifecycle support - * changes to build - * Add lifecycle to drivers, masters and add device manager - * Add lifecycled operation canopen_core - * Added non lifecycle stuff to canopen_core - * Add lifecyle to canopen_base_driver - * Add lifecycle to canopen_proxy_driver - * restructured canopen_core for lifecycle support - * restructured canopen_base_driver for lifecycle support - * Restrucutured canopen_proxy_driver for lifecycle support - * Restructured canopen_402_driver for lifecycle support - * Add canopen_mock_slave add cia402 slave - * add canopen_tests package for testing canopen stack - * Disable linting for the moment and some foxy compat changes - * Further changes for foxy compatability -* Configuration manager integration (`#14 `_) - * Add longer startup delay and test documentation - * Add speed and position publisher - * Create Configuration Manager - * make MasterNode a component and add configuration manager functionalities - * add configuration manager functionalities - * add configuration manger functionalities - * Add documentation for Configuration Manager - * add info messages and documentation - * update launch files and configuration fiels - * add can_utils package - * add info text - * simplify dependencies - * remove tests from can_utils - * avoid tests for canopen_utils - * changes info logging and adds nmt and sdo tests - * add tests - * remove launch_tests from cmake +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Vishnuprasad Prachandabhanu diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 57c85e7b..fcb556fd 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -4,81 +4,5 @@ Changelog for package lely_core_libraries Forthcoming ----------- -* Bump lely_core_librries to version 2.3.2 -* Improve lely compilation time -* Enable simplified bus.yml format (`#115 `_) - * Get slave eds and bin in node_canopen_driver - * Add dictionary to base driver - * Enable dictionary in proxy drivers - * Add a few test objects - * Add pdo checks - * Adjust 402 driver - * Fix tests - * rename to get_xx_queue - * Add typed sdo operations - * Remove object datatype where possible - * Add plain operation mode setting + switchingstate - * Add robot system interface - * Add robot system controller - * Add robot_system_tests - * Add a bit of documentation - * Add in code documentation - * Fix bug - * Add examples section - * Fix set_target for interpolated mode - * Switch to rclcpp::sleep_for - * Fix initialization for state and command interface variables - * Add remade robot system interfce - * Add copyright info - * Fix missing return statement - * processing behavior improvement - * Minor changes to make things work - * Add poll_timer_callback - * Fix format - * Add polling mode variable for config. - * Add cogen - * Add example usage for cogen - * Remove explicit path - --------- - Co-authored-by: Vishnuprasad Prachandabhanu -* Merge branch 'master' into patch-2 -* Merge branch 'bjsowa-master' -* Merge branch 'master' of github.com:bjsowa/ros2_canopen into bjsowa-master -* Merge remote-tracking branch 'ros/master' -* Precommit changes (`#79 `_) - * Precommit changes - * Update to clang-format-14 -* Substitute @BUS_CONFIG_PATH@ in bus configuration file -* Publish joint state instead of velocity topics (`#47 `_) - * disable loader service - * add custom target/command and install to macro - * publish jointstate - * correct variable name squiggle - * Minor changes to driver and slave - * Update lely core library - * Add sensor_msgs to dependencies - * Remove artifacts - * Remove some artifacts -* Update dcfgen cmake integration (`#41 `_) -* Merge branch 'licenses' into 'master' - add licenses to each package - See merge request ipa326/ros-industrial/ros2_canopen!22 -* update package descriptions -* add licenses to each package -* Merge branch 'renaming' into 'master' - Update package names to fit ROS2 naming rules better - See merge request ipa326/ros-industrial/ros2_canopen!21 -* store tests of proxy driver in canopen_proxy_driver -* Merge branch 'cmh/restructured_master_and_slaves' into 'master' - Complete Restructuring - See merge request ipa326/ros-industrial/ros2_canopen!15 -* make lely_core_library tools available -* Add automake etc. -* add autotools-dev to lelycore build dependencies -* change false dependencies -* Merge branch 'ros_canopen-merge' into 'master' - Merge in canopen_402 from ros_canopen - See merge request ipa326/ros-industrial/ros2_canopen!6 -* Merge in canopen_402 from ros_canopen -* lely_core_libraries wrapper tested and building +* Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos From 21141c7b827be47999d70121412c6ec52a90c8d5 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 14 Jun 2023 20:51:57 +0200 Subject: [PATCH 15/59] 0.2.0 --- canopen/CHANGELOG.rst | 4 ++-- canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 4 ++-- canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 4 ++-- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 4 ++-- canopen_core/package.xml | 2 +- canopen_fake_slaves/CHANGELOG.rst | 4 ++-- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 4 ++-- canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 4 ++-- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/CHANGELOG.rst | 4 ++-- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 4 ++-- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 4 ++-- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/CHANGELOG.rst | 4 ++-- canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 4 ++-- canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- lely_core_libraries/package.xml | 2 +- 27 files changed, 40 insertions(+), 40 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index ced1e303..997051b9 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu diff --git a/canopen/package.xml b/canopen/package.xml index 37377d41..aa172a86 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.1.0 + 0.2.0 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index b718cc67..fab1a3ee 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, G.A. vd. Hoorn, Lovro, Vishnuprasad Prachandabhanu, livanov93 diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index a24b2b30..be87a95a 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.1.0 + 0.2.0 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index e32a1841..28fb3782 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index b7b1e0cd..2b5c0ac2 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.1.0 + 0.2.0 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index aa430b4f..30783ca6 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Aulon Bajrami, Borong Yuan, Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_core/package.xml b/canopen_core/package.xml index 3b0db52e..c611b95e 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.1.0 + 0.2.0 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index ba67cd44..720899cf 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, James Ward, Vishnuprasad Prachandabhanu diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index bf828470..236b2aff 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.1.0 + 0.2.0 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index ab5349c5..ffa87bbc 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 8eea7d0f..80b544ce 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.1.0 + 0.2.0 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index ed0a8dbd..dab57468 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index c9151ae6..3799353e 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.1.0 + 0.2.0 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index eb607e68..84f4e97a 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index b7ae24cc..c840fd23 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.1.0 + 0.2.0 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 08ede8b6..64735a92 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Lovro, Vishnuprasad Prachandabhanu, livanov93 diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index 8f695b06..5b584997 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.1.0 + 0.2.0 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index 6abbe848..b89246ad 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, Dr.-Ing. Denis Štogl, Lovro, livanov93 diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index d28096b9..6c7cb291 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.1.0 + 0.2.0 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 5a23a4d7..cb80b1cf 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Denis Štogl, James Ward, Vishnuprasad Prachandabhanu diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 69ff7e29..9fe51c70 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.1.0 + 0.2.0 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index d0895d07..90216616 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos, Vishnuprasad Prachandabhanu diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index 13e8badd..df4a62be 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.1.0 + 0.2.0 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index faff5556..41e8aabb 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.0.0", + version="0.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index fcb556fd..6f8a9bd0 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.0 (2023-06-14) +------------------ * Created package * Contributors: Błażej Sowa, Christoph Hellmann Santos diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index ddf5db9d..9eddf087 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.1.0 + 0.2.0 ROS wrapper for lely-core-libraries From c2c03757ba7465914e253bc0968b987479647b1a Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Mon, 19 Jun 2023 08:27:57 +0200 Subject: [PATCH 16/59] Merge pull request #147 from Continuum-UWr/pr-polling Fix node polling mode in base driver --- .../node_interfaces/node_canopen_base_driver_impl.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index fdddb49b..1ea6b2b1 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -163,11 +163,6 @@ void NodeCanopenBaseDriver::activate(bool called_from_base) this->lely_driver_->set_sync_function( std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this)); } - // poll_timer_ = this->node_->create_wall_timer( - // std::chrono::milliseconds(period_ms_), - // std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this), this->timer_cbg_); - this->lely_driver_->set_sync_function( - std::bind(&NodeCanopenBaseDriver::poll_timer_callback, this)); if (diagnostic_enabled_.load()) { From abc2ba71936e4b25a4f565a349e5a27cf2c1d224 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Tue, 20 Jun 2023 08:45:05 +0200 Subject: [PATCH 17/59] Merge pull request #130 from ros-industrial/fix-placeholders-issue Remove using namespace std::placeholders --- .../node_canopen_402_driver_impl.hpp | 74 ++++++++++++------- 1 file changed, 49 insertions(+), 25 deletions(-) diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index d85cf48e..a96a07c7 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -7,8 +7,7 @@ #include using namespace ros2_canopen::node_interfaces; -using std::placeholders::_1; -using std::placeholders::_2; +using namespace std::placeholders; template NodeCanopen402Driver::NodeCanopen402Driver(NODETYPE * node) @@ -30,45 +29,64 @@ void NodeCanopen402Driver::init(bool called_from_base) this->node_->create_publisher("~/joint_states", 1); handle_init_service = this->node_->create_service( std::string(this->node_->get_name()).append("/init").c_str(), - std::bind(&NodeCanopen402Driver::handle_init, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_init, this, std::placeholders::_1, + std::placeholders::_2)); handle_halt_service = this->node_->create_service( std::string(this->node_->get_name()).append("/halt").c_str(), - std::bind(&NodeCanopen402Driver::handle_halt, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_halt, this, std::placeholders::_1, + std::placeholders::_2)); handle_recover_service = this->node_->create_service( std::string(this->node_->get_name()).append("/recover").c_str(), - std::bind(&NodeCanopen402Driver::handle_recover, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_recover, this, std::placeholders::_1, + std::placeholders::_2)); handle_set_mode_position_service = this->node_->create_service( std::string(this->node_->get_name()).append("/position_mode").c_str(), - std::bind(&NodeCanopen402Driver::handle_set_mode_position, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_set_mode_position, this, std::placeholders::_1, + std::placeholders::_2)); handle_set_mode_velocity_service = this->node_->create_service( std::string(this->node_->get_name()).append("/velocity_mode").c_str(), - std::bind(&NodeCanopen402Driver::handle_set_mode_velocity, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_set_mode_velocity, this, std::placeholders::_1, + std::placeholders::_2)); handle_set_mode_cyclic_velocity_service = this->node_->create_service( std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(), - std::bind(&NodeCanopen402Driver::handle_set_mode_cyclic_velocity, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_set_mode_cyclic_velocity, this, + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_cyclic_position_service = this->node_->create_service( std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(), - std::bind(&NodeCanopen402Driver::handle_set_mode_cyclic_position, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_set_mode_cyclic_position, this, + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_interpolated_position_service = this->node_->create_service( std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(), std::bind( - &NodeCanopen402Driver::handle_set_mode_interpolated_position, this, _1, _2)); + &NodeCanopen402Driver::handle_set_mode_interpolated_position, this, + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_torque_service = this->node_->create_service( std::string(this->node_->get_name()).append("/torque_mode").c_str(), - std::bind(&NodeCanopen402Driver::handle_set_mode_torque, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_set_mode_torque, this, std::placeholders::_1, + std::placeholders::_2)); handle_set_target_service = this->node_->create_service( std::string(this->node_->get_name()).append("/target").c_str(), - std::bind(&NodeCanopen402Driver::handle_set_target, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_set_target, this, std::placeholders::_1, + std::placeholders::_2)); } template <> @@ -79,58 +97,64 @@ void NodeCanopen402Driver::init(bool called_fro this->node_->create_publisher("~/joint_states", 10); handle_init_service = this->node_->create_service( std::string(this->node_->get_name()).append("/init").c_str(), - std::bind(&NodeCanopen402Driver::handle_init, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_init, this, + std::placeholders::_1, std::placeholders::_2)); handle_halt_service = this->node_->create_service( std::string(this->node_->get_name()).append("/halt").c_str(), - std::bind(&NodeCanopen402Driver::handle_halt, this, _1, _2)); + std::bind( + &NodeCanopen402Driver::handle_halt, this, + std::placeholders::_1, std::placeholders::_2)); handle_recover_service = this->node_->create_service( std::string(this->node_->get_name()).append("/recover").c_str(), std::bind( - &NodeCanopen402Driver::handle_recover, this, _1, _2)); + &NodeCanopen402Driver::handle_recover, this, + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_position_service = this->node_->create_service( std::string(this->node_->get_name()).append("/position_mode").c_str(), std::bind( - &NodeCanopen402Driver::handle_set_mode_position, this, _1, - _2)); + &NodeCanopen402Driver::handle_set_mode_position, this, + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_velocity_service = this->node_->create_service( std::string(this->node_->get_name()).append("/velocity_mode").c_str(), std::bind( - &NodeCanopen402Driver::handle_set_mode_velocity, this, _1, - _2)); + &NodeCanopen402Driver::handle_set_mode_velocity, this, + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_cyclic_velocity_service = this->node_->create_service( std::string(this->node_->get_name()).append("/cyclic_velocity_mode").c_str(), std::bind( &NodeCanopen402Driver::handle_set_mode_cyclic_velocity, this, - _1, _2)); + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_cyclic_position_service = this->node_->create_service( std::string(this->node_->get_name()).append("/cyclic_position_mode").c_str(), std::bind( &NodeCanopen402Driver::handle_set_mode_cyclic_position, this, - _1, _2)); + std::placeholders::_1, std::placeholders::_2)); handle_set_mode_interpolated_position_service = this->node_->create_service< std_srvs::srv::Trigger>( std::string(this->node_->get_name()).append("/interpolated_position_mode").c_str(), std::bind( &NodeCanopen402Driver::handle_set_mode_interpolated_position, - this, _1, _2)); + this, std::placeholders::_1, std::placeholders::_2)); handle_set_mode_torque_service = this->node_->create_service( std::string(this->node_->get_name()).append("/torque_mode").c_str(), std::bind( - &NodeCanopen402Driver::handle_set_mode_torque, this, _1, - _2)); + &NodeCanopen402Driver::handle_set_mode_torque, this, + std::placeholders::_1, std::placeholders::_2)); handle_set_target_service = this->node_->create_service( std::string(this->node_->get_name()).append("/target").c_str(), std::bind( - &NodeCanopen402Driver::handle_set_target, this, _1, _2)); + &NodeCanopen402Driver::handle_set_target, this, + std::placeholders::_1, std::placeholders::_2)); } template <> From cecc892596b6f6b27f405036bef44ba1e28ff457 Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu <32260301+ipa-vsp@users.noreply.github.com> Date: Wed, 21 Jun 2023 18:06:35 +0200 Subject: [PATCH 18/59] Sync humble branch with 0.2.1 release (#153) * Update rolling.yml * Merge pull request #152 from ros-industrial/fix-bdist-dep-in-lely-core Do not build dcf-tools bdist in lely_core_libraries * Merge pull request #142 from ros-industrial/iron-test-fixes-controllers Don't use ros2_control_test_assets since not needed anymore. Add load tests for all controllers. * Merge pull request #145 from ros-industrial/consistent-output-of-hex-values Use consistenlty (uppercase) HEX output for NodeID and Index. * Merge pull request #151 from ros-industrial/upgrade-launch-tests Fix proxy driver lifecyle and launch tests * Update Changelogs Signed-off-by: Christoph Hellmann Santos * 0.2.1 * Fix build warnings * Update status badge. --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Co-authored-by: Christoph Hellmann Santos --- .github/workflows/rolling.yml | 2 +- .pre-commit-config.yaml | 2 +- README.md | 4 +- canopen/CHANGELOG.rst | 3 + canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 5 + canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 6 + .../node_canopen_base_driver_impl.hpp | 5 +- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 7 + .../include/canopen_core/device_container.hpp | 8 + .../node_interfaces/node_canopen_driver.hpp | 9 +- .../node_interfaces/node_canopen_master.hpp | 10 + canopen_core/package.xml | 2 +- canopen_core/src/device_container.cpp | 8 + canopen_core/src/lifecycle_manager.cpp | 4 +- canopen_fake_slaves/CHANGELOG.rst | 5 + .../canopen_fake_slaves/basic_slave.hpp | 6 +- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 3 + canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 5 + .../node_canopen_basic_master.hpp | 1 + .../node_canopen_basic_master_impl.hpp | 1 + canopen_master_driver/package.xml | 2 +- .../test/test_node_canopen_basic_master.cpp | 2 +- canopen_proxy_driver/CHANGELOG.rst | 5 + .../node_canopen_proxy_driver_impl.hpp | 10 +- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 5 + canopen_ros2_control/package.xml | 2 +- canopen_ros2_control/src/canopen_system.cpp | 2 +- canopen_ros2_control/src/cia402_system.cpp | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 6 + canopen_ros2_controllers/CMakeLists.txt | 21 +- canopen_ros2_controllers/package.xml | 3 +- .../test_load_canopen_proxy_controller.cpp | 5 +- .../test_load_cia402_device_controller.cpp | 38 +++ .../test_load_cia402_robot_controller.cpp | 38 +++ canopen_tests/CHANGELOG.rst | 7 + .../launch_tests/test_cia402_driver.py | 108 +++++++ .../launch_tests/test_proxy_driver.py | 240 +++++++++++---- .../test_proxy_lifecycle_driver.py | 275 +++++++++++++----- .../launch_tests/test_robot_control.py | 92 ++++++ canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 5 + .../canopen_utils/launch_test_node.py | 125 ++++++++ canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 5 + lely_core_libraries/CMakeLists.txt | 3 +- lely_core_libraries/package.xml | 2 +- .../patches/0001-Don-t-build-bdist.patch | 30 ++ .../patches/0002-No-bdist.patch | 25 ++ 55 files changed, 1014 insertions(+), 158 deletions(-) create mode 100644 canopen_ros2_controllers/test/test_load_cia402_device_controller.cpp create mode 100644 canopen_ros2_controllers/test/test_load_cia402_robot_controller.cpp create mode 100644 canopen_tests/launch_tests/test_cia402_driver.py create mode 100644 canopen_tests/launch_tests/test_robot_control.py create mode 100644 canopen_utils/canopen_utils/launch_test_node.py create mode 100644 lely_core_libraries/patches/0001-Don-t-build-bdist.patch create mode 100644 lely_core_libraries/patches/0002-No-bdist.patch diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 62b5a37b..b72124d1 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -14,7 +14,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [rolling, iron] ROS_REPO: [testing] env: CCACHE_DIR: "${{ github.workspace }}/.ccache" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 40651881..99f8e46f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,7 +11,7 @@ # pre-commit autoupdate # # See https://github.com/pre-commit/pre-commit - +exclude: '.*\.patch' repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks diff --git a/README.md b/README.md index 063dad87..7ebb4364 100644 --- a/README.md +++ b/README.md @@ -2,8 +2,8 @@ ## Status -[![Build Status](https://github.com/ros-industrial/ros2_canopen/workflows/rolling/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) -[![Documentation Status](https://github.com/ros-industrial/ros2_canopen/workflows/Documentation/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) +[![humble](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble.yml/badge.svg)](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble.yml) +[![HUMBLE Documentation](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble_documentation.yml/badge.svg)](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble_documentation.yml) The stack is currently under development and not yet ready for production use. diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 997051b9..1add7c7e 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen/package.xml b/canopen/package.xml index aa172a86..9e795d92 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.2.0 + 0.2.1 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index fab1a3ee..46590b89 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Fix boost/std placeholders ambiguity in older boost versions +* Contributors: Christoph Hellmann Santos + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index be87a95a..52faa3c9 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.2.0 + 0.2.1 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 28fb3782..ba1cc883 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Fix base driver lifecyle +* Fix node polling mode in base driver +* Contributors: Błażej Sowa, Christoph Hellmann Santos + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index 1ea6b2b1..26ebddab 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -177,7 +177,8 @@ void NodeCanopenBaseDriver::deactivate(bool called_from_base) { nmt_state_publisher_thread_.join(); poll_timer_->cancel(); - this->lely_driver_->unset_sync_function(); + emcy_queue_.reset(); + rpdo_queue_.reset(); if (diagnostic_enabled_.load()) { diagnostic_updater_->removeByName("diagnostic updater"); @@ -187,6 +188,8 @@ void NodeCanopenBaseDriver::deactivate(bool called_from_base) template void NodeCanopenBaseDriver::cleanup(bool called_from_base) { + NodeCanopenDriver::cleanup(called_from_base); + // this->lely_driver_->unset_sync_function(); } template diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index 2b5c0ac2..ac9bc2c7 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.2.0 + 0.2.1 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 30783ca6..5dccc159 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Fix master and driver lifecycle +* Fix QoS build warning in canopen_core +* Use consistenlty HEX output for NodeID and Index. +* Contributors: Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_core/include/canopen_core/device_container.hpp b/canopen_core/include/canopen_core/device_container.hpp index da4042a8..8a81fd5d 100644 --- a/canopen_core/include/canopen_core/device_container.hpp +++ b/canopen_core/include/canopen_core/device_container.hpp @@ -215,6 +215,10 @@ class DeviceContainer : public rclcpp_components::ComponentManager std::vector devices; std::vector ids; uint32_t count = this->config_->get_all_devices(devices); + if (count == 0) + { + return ids; + } for (auto it = devices.begin(); it != devices.end(); it++) { @@ -244,6 +248,10 @@ class DeviceContainer : public rclcpp_components::ComponentManager { std::vector devices; uint32_t count = this->config_->get_all_devices(devices); + if (count == 0) + { + return ""; + } for (auto it = devices.begin(); it != devices.end(); it++) { auto node_id = config_->get_entry(*it, "node_id"); diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp index a76ee758..e1f4c73e 100644 --- a/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_driver.hpp @@ -316,6 +316,7 @@ class NodeCanopenDriver : public NodeCanopenDriverInterface { throw DriverException("Cleanup: driver is still activated"); } + this->cleanup(true); this->configured_.store(false); } @@ -326,7 +327,13 @@ class NodeCanopenDriver : public NodeCanopenDriverInterface * * @param called_from_base */ - virtual void cleanup(bool called_from_base) {} + virtual void cleanup(bool called_from_base) + { + RCLCPP_INFO(this->node_->get_logger(), "Cleanup"); + this->exec_.reset(); + this->master_.reset(); + this->master_set_.store(false); + } /** * @brief Shutdown the driver diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp index 373fe502..89794cc8 100644 --- a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp @@ -301,7 +301,17 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface throw MasterException("Cleanup: master is still active"); } this->cleanup(true); + io_guard_.reset(); + ctx_.reset(); + poll_.reset(); + loop_.reset(); + + exec_.reset(); + timer_.reset(); + ctrl_.reset(); + chan_.reset(); this->configured_.store(false); + this->master_set_.store(false); } virtual void cleanup(bool called_from_base) {} diff --git a/canopen_core/package.xml b/canopen_core/package.xml index c611b95e..b99e2655 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.2.0 + 0.2.1 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 diff --git a/canopen_core/src/device_container.cpp b/canopen_core/src/device_container.cpp index d4153a11..f8f1bde0 100644 --- a/canopen_core/src/device_container.cpp +++ b/canopen_core/src/device_container.cpp @@ -196,6 +196,10 @@ bool DeviceContainer::load_master() RCLCPP_INFO(this->get_logger(), "Loading Master Configuration."); std::vector devices; uint32_t count = this->config_->get_all_devices(devices); + if (count == 0) + { + return false; + } bool master_found = false; // Find master in configuration @@ -250,6 +254,10 @@ bool DeviceContainer::load_drivers() RCLCPP_INFO(this->get_logger(), "Loading Driver Configuration."); std::vector devices; uint32_t count = this->config_->get_all_devices(devices); + if (count == 0) + { + return false; + } for (auto it = devices.begin(); it != devices.end(); it++) { diff --git a/canopen_core/src/lifecycle_manager.cpp b/canopen_core/src/lifecycle_manager.cpp index 63bf8bf7..49bf58a4 100644 --- a/canopen_core/src/lifecycle_manager.cpp +++ b/canopen_core/src/lifecycle_manager.cpp @@ -124,7 +124,7 @@ unsigned int LifecycleManager::get_state(uint8_t node_id, std::chrono::seconds t if (future_status != std::future_status::ready) { RCLCPP_ERROR( - get_logger(), "Server time out while getting current state for node %hhu", node_id); + get_logger(), "Server time out while getting current state for node 0x%X", node_id); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } auto result = future_result.get()->current_state; @@ -153,7 +153,7 @@ bool LifecycleManager::change_state( if (future_status != std::future_status::ready) { RCLCPP_ERROR( - get_logger(), "Server time out while getting current state for node %hhu", node_id); + get_logger(), "Server time out while getting current state for node 0x%X", node_id); return false; } diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 720899cf..bb9afe52 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Fix fake slave for PDOs +* Contributors: Christoph Hellmann Santos + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp index 21b9a3ba..62a6dc97 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/basic_slave.hpp @@ -46,9 +46,9 @@ class SimpleSlave : public canopen::BasicSlave */ void OnWrite(uint16_t idx, uint8_t subidx) noexcept override { - // uint32_t val = (*this)[idx][subidx]; - //(*this)[0x4001][0] = val; - // this->TpdoEvent(0); + uint32_t val = (*this)[idx][subidx]; + (*this)[0x4001][0] = val; + this->TpdoEvent(0); } }; diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index 236b2aff..fc15bbb7 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.2.0 + 0.2.1 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index ffa87bbc..8ec11f7b 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 80b544ce..9f64d202 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.2.0 + 0.2.1 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index dab57468..08b0dcee 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Fix master lifecycle +* Contributors: Christoph Hellmann Santos + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp index 1bd58933..36c188b7 100644 --- a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp +++ b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp @@ -45,6 +45,7 @@ class NodeCanopenBasicMaster : public ros2_canopen::node_interfaces::NodeCanopen this->activated_.load(); RCLCPP_INFO(this->node_->get_logger(), "NodeCanopenBasicMaster"); } + virtual ~NodeCanopenBasicMaster() {} virtual void activate(bool called_from_base) override; virtual void deactivate(bool called_from_base) override; diff --git a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp index 776f267a..3fd638dc 100644 --- a/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp +++ b/canopen_master_driver/include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.hpp @@ -34,6 +34,7 @@ void NodeCanopenBasicMaster::activate(bool called_from_base) template void NodeCanopenBasicMaster::deactivate(bool called_from_base) { + master_bridge_.reset(); this->master_.reset(); } diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index 3799353e..7ab2a0d8 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.2.0 + 0.2.1 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/test/test_node_canopen_basic_master.cpp b/canopen_master_driver/test/test_node_canopen_basic_master.cpp index b0c67e19..6eda4624 100644 --- a/canopen_master_driver/test/test_node_canopen_basic_master.cpp +++ b/canopen_master_driver/test/test_node_canopen_basic_master.cpp @@ -24,7 +24,7 @@ class RclCppFixture node = new rclcpp::Node("Node"); interface = new ros2_canopen::node_interfaces::NodeCanopenBasicMaster(node); } - ~RclCppFixture() + virtual ~RclCppFixture() { rclcpp::shutdown(); delete (interface); diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 84f4e97a..637e093b 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Use consistenlty (uppercase) HEX output for NodeID and Index. +* Contributors: Christoph Hellmann Santos, Denis Štogl + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp index 15444a31..b5888291 100644 --- a/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp +++ b/canopen_proxy_driver/include/canopen_proxy_driver/node_interfaces/node_canopen_proxy_driver_impl.hpp @@ -160,7 +160,7 @@ void NodeCanopenProxyDriver::on_nmt(canopen::NmtState nmt_state) break; } RCLCPP_INFO( - this->node_->get_logger(), "Slave %hhu: Switched NMT state to %s", + this->node_->get_logger(), "Slave 0x%X: Switched NMT state to %s", this->lely_driver_->get_id(), message.data.c_str()); nmt_state_publisher->publish(message); @@ -183,7 +183,7 @@ bool NodeCanopenProxyDriver::tpdo_transmit(ros2_canopen::COData & data if (this->activated_.load()) { RCLCPP_INFO( - this->node_->get_logger(), "Node ID %hhu: Transmit PDO index %x, subindex %hhu, data %d", + this->node_->get_logger(), "Node ID 0x%X: Transmit PDO index %x, subindex %hhu, data %d", this->lely_driver_->get_id(), data.index_, data.subindex_, data.data_); // ToDo: Remove or make debug this->lely_driver_->tpdo_transmit(data); @@ -198,7 +198,7 @@ void NodeCanopenProxyDriver::on_rpdo(ros2_canopen::COData d) if (this->activated_.load()) { // RCLCPP_INFO( - // this->node_->get_logger(), "Node ID %hhu: Received PDO index %#04x, subindex %hhu, data + // this->node_->get_logger(), "Node ID 0x%X: Received PDO index %#04x, subindex %hhu, data // %x", this->lely_driver_->get_id(), d.index_, d.subindex_, d.data_); auto message = canopen_interfaces::msg::COData(); message.index = d.index_; @@ -266,7 +266,7 @@ bool NodeCanopenProxyDriver::sdo_read(ros2_canopen::COData & data) if (this->activated_.load()) { RCLCPP_INFO( - this->node_->get_logger(), "Slave %hhu: SDO Read Call index=0x%x subindex=%hhu", + this->node_->get_logger(), "Slave 0x%X: SDO Read Call index=0x%X subindex=%hhu", this->lely_driver_->get_id(), data.index_, data.subindex_); // Only allow one SDO request concurrently @@ -306,7 +306,7 @@ bool NodeCanopenProxyDriver::sdo_write(ros2_canopen::COData & data) if (this->activated_.load()) { RCLCPP_INFO( - this->node_->get_logger(), "Slave %hhu: SDO Write Call index=0x%x subindex=%hhu data=%u", + this->node_->get_logger(), "Slave 0x%X: SDO Write Call index=0x%X subindex=%hhu data=%u", this->lely_driver_->get_id(), data.index_, data.subindex_, data.data_); // Only allow one SDO request concurrently diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index c840fd23..d72542af 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.2.0 + 0.2.1 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 64735a92..210a280f 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Use consistenlty (uppercase) HEX output for NodeID and Index. +* Contributors: Christoph Hellmann Santos, Denis Štogl + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index 5b584997..4515e04e 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.2.0 + 0.2.1 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index d8d3ae24..99e9f153 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -146,7 +146,7 @@ void CanopenSystem::initDeviceContainer() proxy_driver->register_rpdo_cb(rpdo_cb); RCLCPP_INFO( - kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%x'", // + kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '0x%X'", // it->second->get_node_base_interface()->get_name(), it->first); } diff --git a/canopen_ros2_control/src/cia402_system.cpp b/canopen_ros2_control/src/cia402_system.cpp index 8c1cfbf4..48b4b99b 100644 --- a/canopen_ros2_control/src/cia402_system.cpp +++ b/canopen_ros2_control/src/cia402_system.cpp @@ -72,7 +72,7 @@ void Cia402System::initDeviceContainer() driver->register_rpdo_cb(rpdo_cb); RCLCPP_INFO( - kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '%x'", + kLogger, "\nRegistered driver:\n name: '%s'\n node_id: '0x%X'", it->second->get_node_base_interface()->get_name(), it->first); } diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index b89246ad..c0f8e9df 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Fix QoS build warning canopen_ros2_controllers +* Don't use ros2_control_test_assets since not needed anymore. Add load tests for all controllers. +* Contributors: Christoph Hellmann Santos, Denis Štogl, Vishnuprasad Prachandabhanu + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_ros2_controllers/CMakeLists.txt b/canopen_ros2_controllers/CMakeLists.txt index e5879ce1..13ce784e 100644 --- a/canopen_ros2_controllers/CMakeLists.txt +++ b/canopen_ros2_controllers/CMakeLists.txt @@ -13,7 +13,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools - ros2_control_test_assets std_msgs std_srvs ) @@ -56,13 +55,31 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + ament_add_gmock(test_load_canopen_proxy_controller test/test_load_canopen_proxy_controller.cpp) target_include_directories(test_load_canopen_proxy_controller PRIVATE include) ament_target_dependencies( test_load_canopen_proxy_controller controller_manager hardware_interface - ros2_control_test_assets + canopen_interfaces + ) + + ament_add_gmock(test_load_cia402_device_controller test/test_load_cia402_device_controller.cpp) + target_include_directories(test_load_cia402_device_controller PRIVATE include) + ament_target_dependencies( + test_load_cia402_device_controller + controller_manager + hardware_interface + canopen_interfaces + ) + + ament_add_gmock(test_load_cia402_robot_controller test/test_load_cia402_robot_controller.cpp) + target_include_directories(test_load_cia402_robot_controller PRIVATE include) + ament_target_dependencies( + test_load_cia402_robot_controller + controller_manager + hardware_interface canopen_interfaces ) diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index 6c7cb291..887ab274 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.2.0 + 0.2.1 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov @@ -21,7 +21,6 @@ rclcpp rclcpp_lifecycle realtime_tools - ros2_control_test_assets std_msgs std_srvs diff --git a/canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp b/canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp index fde6f722..89dd747c 100644 --- a/canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp +++ b/canopen_ros2_controllers/test/test_load_canopen_proxy_controller.cpp @@ -20,7 +20,6 @@ #include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadCanopenProxyController, load_controller) { @@ -30,9 +29,7 @@ TEST(TestLoadCanopenProxyController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + std::make_unique(), executor, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller( "test_canopen_ros2_controllers", "canopen_ros2_controllers/CanopenProxyController")); diff --git a/canopen_ros2_controllers/test/test_load_cia402_device_controller.cpp b/canopen_ros2_controllers/test/test_load_cia402_device_controller.cpp new file mode 100644 index 00000000..fb1ae42f --- /dev/null +++ b/canopen_ros2_controllers/test/test_load_cia402_device_controller.cpp @@ -0,0 +1,38 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" + +TEST(TestLoadCanopenProxyController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique(), executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_canopen_ros2_controllers", "canopen_ros2_controllers/Cia402DeviceController")); + + rclcpp::shutdown(); +} diff --git a/canopen_ros2_controllers/test/test_load_cia402_robot_controller.cpp b/canopen_ros2_controllers/test/test_load_cia402_robot_controller.cpp new file mode 100644 index 00000000..26dc700f --- /dev/null +++ b/canopen_ros2_controllers/test/test_load_cia402_robot_controller.cpp @@ -0,0 +1,38 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" + +TEST(TestLoadCanopenProxyController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique(), executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_canopen_ros2_controllers", "canopen_ros2_controllers/Cia402RobotController")); + + rclcpp::shutdown(); +} diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index cb80b1cf..6532b5e5 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Add trvivial integration test for robot_control +* Add trivial integration tests for cia402_driver +* Use the more tidy launch_test_node +* Contributors: Christoph Hellmann Santos + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_tests/launch_tests/test_cia402_driver.py b/canopen_tests/launch_tests/test_cia402_driver.py new file mode 100644 index 00000000..b5a9ae83 --- /dev/null +++ b/canopen_tests/launch_tests/test_cia402_driver.py @@ -0,0 +1,108 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from time import sleep +import time +import pytest +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_testing +import threading +import rclpy +from rclpy.node import Node +from canopen_utils.launch_test_node import LaunchTestNode +from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID, COTargetDouble +from canopen_interfaces.msg import COData +from std_srvs.srv import Trigger +import unittest + + +@pytest.mark.rostest +def generate_test_description(): + + launch_desc = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_tests"), "launch"), + "/cia402_setup.launch.py", + ] + ) + ) + + ready_to_test = launch.actions.TimerAction( + period=5.0, + actions=[launch_testing.actions.ReadyToTest()], + ) + + return (LaunchDescription([launch_desc, ready_to_test]), {}) + + +class TestSDO(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) + + @classmethod + def setUp(cls): + cls.ok = True + rclpy.init() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() + + @classmethod + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() + rclpy.shutdown() + + def test_sdo_read(self): + req = CORead.Request() + req.index = 0x1000 + req.subindex = 0 + + res = CORead.Response() + res.success = True + res.data = 0xFFFF0192 + + self.node.call_service("cia402_device_1/sdo_read", CORead, req, res) + + def test_init(self): + req = Trigger.Request() + + res = Trigger.Response() + res.success = True + + self.node.call_service("cia402_device_1/init", Trigger, req, res) + + def test_position_mode(self): + req = Trigger.Request() + + res = Trigger.Response() + res.success = True + + self.node.call_service("cia402_device_1/init", Trigger, req, res) + self.node.call_service("cia402_device_1/position_mode", Trigger, req, res) + + target_req = COTargetDouble.Request() + target_req.target = 1.0 + target_res = COTargetDouble.Response() + target_res.success = True + + self.node.call_service("cia402_device_1/target", COTargetDouble, target_req, target_res) diff --git a/canopen_tests/launch_tests/test_proxy_driver.py b/canopen_tests/launch_tests/test_proxy_driver.py index 8b3f5165..c90f7ed0 100644 --- a/canopen_tests/launch_tests/test_proxy_driver.py +++ b/canopen_tests/launch_tests/test_proxy_driver.py @@ -14,6 +14,7 @@ import os from time import sleep +import time import pytest from ament_index_python import get_package_share_directory from launch import LaunchDescription @@ -23,11 +24,10 @@ import launch_testing import threading import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node -from canopen_utils.test_node import TestNode -from lifecycle_msgs.msg import State, Transition +from canopen_utils.launch_test_node import LaunchTestNode +from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID +from canopen_interfaces.msg import COData from std_srvs.srv import Trigger import unittest @@ -52,58 +52,200 @@ def generate_test_description(): return (LaunchDescription([launch_desc, ready_to_test]), {}) -class TestLifecycle(unittest.TestCase): +class TestNMT(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) + @classmethod def setUpClass(cls): - print("SetupClass") + cls.ok = True + rclpy.init() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() @classmethod def tearDownClass(cls): - print("TearDownClass") + cls.ok = False + cls.thread.join() + cls.node.destroy_node() + rclpy.shutdown() + + def test_reset_nmt(self): + request = Trigger.Request() + response = Trigger.Response() + response.success = True + self.node.call_service("proxy_device_2/nmt_reset_node", Trigger, request, response) + time.sleep(1) + + +class TestSDO(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod - def setUp(self): - print("Setup") + def setUp(cls): + cls.ok = True rclpy.init() - self.node = TestNode() - self.x = threading.Thread(target=rclpy.spin, args=[self.node]) - self.x.start() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() - def tearDown(self): - print("TearDown") + @classmethod + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() rclpy.shutdown() - self.node.destroy_node() - self.x.join() - - def test_02_nmt(self): - assert self.node.checkTrigger("proxy_device_1", "nmt_reset_node") - assert self.node.checkTrigger("proxy_device_2", "nmt_reset_node") - sleep(1.0) - - def test_03_sdo_read(self): - assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, data=0) - assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, data=0) - - def test_04_sdo_write(self): - assert self.node.checkSDOWrite("proxy_device_1", index=0x4000, subindex=0, data=100) - assert self.node.checkSDOWrite("proxy_device_2", index=0x4000, subindex=0, data=100) - assert self.node.checkSDORead("proxy_device_1", index=0x4000, subindex=0, data=100) - assert self.node.checkSDORead("proxy_device_2", index=0x4000, subindex=0, data=100) - - def test_05_sdo_read_id(self): - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=100) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=100) - - def test_06_sdo_write_id(self): - assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) - assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) - - # def test_07_rpdo_tpdo(self): - # assert self.node.checkRpdoTpdo( - # "proxy_device_1", index=0x4000, subindex=0, data=101 - # ) - # assert self.node.checkRpdoTpdo( - # "proxy_device_2", index=0x4000, subindex=0, data=202 - # ) + + def test_sdo_read(self): + req = CORead.Request() + req.index = 0x4000 + req.subindex = 0 + + res = CORead.Response() + res.success = True + res.data = 0 + + self.node.call_service("proxy_device_1/sdo_read", CORead, req, res) + self.node.call_service("proxy_device_2/sdo_read", CORead, req, res) + + def test_sdo_write(self): + index = 0x4000 + subindex = 0 + data = 100 + + req = COWrite.Request() + req.index = index + req.subindex = subindex + req.data = data + + res = COWrite.Response() + res.success = True + + rreq = CORead.Request() + rreq.index = index + rreq.subindex = subindex + + rres = CORead.Response() + rres.success = True + rres.data = data + + self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) + self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) + time.sleep(0.01) + self.node.call_service("proxy_device_1/sdo_read", CORead, rreq, rres) + self.node.call_service("proxy_device_2/sdo_read", CORead, rreq, rres) + req.data = 0 + time.sleep(0.01) + self.node.call_service("proxy_device_1/sdo_write", COWrite, req, res) + self.node.call_service("proxy_device_2/sdo_write", COWrite, req, res) + time.sleep(0.01) + + +class TestSDOMaster(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) + + @classmethod + def setUp(cls): + cls.ok = True + rclpy.init() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() + + @classmethod + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() + rclpy.shutdown() + + def test_sdo_read(self): + req = COReadID.Request() + req.index = 0x4000 + req.subindex = 0 + req.nodeid = 2 + req.canopen_datatype = 0x7 + + res = COReadID.Response() + res.success = True + res.data = 0 + + self.node.call_service("/master/sdo_read", COReadID, req, res) + req.nodeid = 3 + self.node.call_service("/master/sdo_read", COReadID, req, res) + + def test_sdo_write(self): + index = 0x4000 + subindex = 0 + data = 100 + req = COWriteID.Request() + req.index = index + req.subindex = subindex + req.data = data + req.canopen_datatype = 0x7 + req.nodeid = 2 + res = COWriteID.Response() + res.success = True + rreq = COReadID.Request() + rreq.index = index + rreq.subindex = subindex + rreq.nodeid = 2 + rres = COReadID.Response() + rres.success = True + rres.data = data + rreq.canopen_datatype = 0x7 + self.node.call_service("master/sdo_write", COWriteID, req, res) + self.node.call_service("master/sdo_write", COWriteID, req, res) + self.node.call_service("master/sdo_read", COReadID, rreq, rres) + self.node.call_service("master/sdo_read", COReadID, rreq, rres) + req.data = 0 + self.node.call_service("master/sdo_write", COWriteID, req, res) + self.node.call_service("master/sdo_write", COWriteID, req, res) + + +class TestPDO(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) + + @classmethod + def setUp(cls): + cls.ok = True + rclpy.init() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() + + @classmethod + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() + rclpy.shutdown() + + def test_pdo(self): + msg = COData() + msg.index = 0x4000 + msg.subindex = 0 + msg.data = 200 + + pub_msg = COData() + pub_msg.index = 0x4001 + pub_msg.subindex = 0 + pub_msg.data = 200 + thread = threading.Thread( + target=self.node.subscribe_and_wait_for_message, + args=["proxy_device_1/rpdo", COData, pub_msg], + ) + thread.start() + self.node.publish_message("proxy_device_1/tpdo", COData, msg) + time.sleep(0.1) + msg.data = 0 + self.node.publish_message("proxy_device_1/tpdo", COData, msg) + thread.join() diff --git a/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py index 9fb4f3e9..1c81ee9c 100644 --- a/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py +++ b/canopen_tests/launch_tests/test_proxy_lifecycle_driver.py @@ -14,6 +14,7 @@ import os from time import sleep +import time import pytest from ament_index_python import get_package_share_directory from launch import LaunchDescription @@ -23,13 +24,12 @@ import launch_testing import threading import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from canopen_utils.test_node import TestNode -from lifecycle_msgs.msg import State, Transition -from std_srvs.srv import Trigger +from canopen_utils.launch_test_node import LaunchTestNode +from lifecycle_msgs.msg import Transition +from lifecycle_msgs.srv import ChangeState import unittest +from canopen_interfaces.srv import CORead, COWrite +from canopen_interfaces.msg import COData @pytest.mark.rostest @@ -53,73 +53,212 @@ def generate_test_description(): class TestLifecycle(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) + + @classmethod + def setUp(cls): + cls.ok = True + rclpy.init() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() + + @classmethod + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() + rclpy.shutdown() + + def test_configure_unconfigure(self): + req = ChangeState.Request() + req.transition.id = Transition.TRANSITION_CONFIGURE + + res = ChangeState.Response() + res.success = True + + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + + req.transition.id = Transition.TRANSITION_CLEANUP + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + + def test_full_cycle(self): + req = ChangeState.Request() + + res = ChangeState.Response() + res.success = True + + req.transition.id = Transition.TRANSITION_CONFIGURE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CONFIGURE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_ACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************ACTIVATE SUCCESSFUL*************************") + + req.transition.id = Transition.TRANSITION_DEACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************DEACTIVATE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_CLEANUP + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CLEANUP SUCCESSFUL*************************") + + req.transition.id = Transition.TRANSITION_CONFIGURE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CONFIGURE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_ACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************ACTIVATE SUCCESSFUL*************************") + + req.transition.id = Transition.TRANSITION_DEACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************DEACTIVATE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_CLEANUP + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CLEANUP SUCCESSFUL*************************") + + +class TestSDO(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) + @classmethod - def setUpClass(cls): - print("SetupClass") + def setUp(cls): + cls.ok = True + rclpy.init() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() @classmethod - def tearDownClass(cls): - print("TearDownClass") + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() + rclpy.shutdown() + + def test_full_cycle_sdo(self): + req = ChangeState.Request() + + res = ChangeState.Response() + res.success = True + + req.transition.id = Transition.TRANSITION_CONFIGURE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CONFIGURE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_ACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************ACTIVATE SUCCESSFUL*************************") + + data = 100 + + readreq = CORead.Request() + readreq.index = 0x4000 + readreq.subindex = 0x00 + + readres = CORead.Response() + readres.success = True + readres.data = data + + writereq = COWrite.Request() + writereq.index = 0x4000 + writereq.subindex = 0x00 + writereq.data = data + + writeres = COWrite.Response() + writeres.success = True + + self.node.call_service("proxy_device_1/sdo_write", COWrite, writereq, writeres) + self.node.call_service("proxy_device_2/sdo_write", COWrite, writereq, writeres) + self.node.call_service("proxy_device_1/sdo_read", CORead, readreq, readres) + self.node.call_service("proxy_device_2/sdo_read", CORead, readreq, readres) + data = 0 + self.node.call_service("proxy_device_1/sdo_write", COWrite, writereq, writeres) + self.node.call_service("proxy_device_2/sdo_write", COWrite, writereq, writeres) + + req.transition.id = Transition.TRANSITION_DEACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************DEACTIVATE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_CLEANUP + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CLEANUP SUCCESSFUL*************************") + + req.transition.id = Transition.TRANSITION_CONFIGURE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CONFIGURE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_ACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************ACTIVATE SUCCESSFUL*************************") + + self.node.call_service("proxy_device_1/sdo_read", CORead, readreq, readres) + self.node.call_service("proxy_device_2/sdo_read", CORead, readreq, readres) + + req.transition.id = Transition.TRANSITION_DEACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************DEACTIVATE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_CLEANUP + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CLEANUP SUCCESSFUL*************************") + + +class TestPDO(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) @classmethod - def setUp(self): - print("Setup") + def setUp(cls): + cls.ok = True rclpy.init() - self.node = TestNode() - self.x = threading.Thread(target=rclpy.spin, args=[self.node]) - self.x.start() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() - def tearDown(self): - print("TearDown") + @classmethod + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() rclpy.shutdown() - self.node.destroy_node() - self.x.join() - - def test_01_to_active(self): - assert self.node.checkTransition( - "lifecycle_manager", State.PRIMARY_STATE_UNCONFIGURED, Transition.TRANSITION_CONFIGURE - ), "Could not configure device manager" - assert self.node.checkTransition( - "lifecycle_manager", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_ACTIVATE - ), "Could not configure device manager" - - def test_02_nmt(self): - assert self.node.checkTrigger("proxy_device_1", "nmt_reset_node") - assert self.node.checkTrigger("proxy_device_2", "nmt_reset_node") - sleep(1.0) - - def test_03_sdo_read(self): - assert self.node.checkSDORead("proxy_device_1", index=0x1000, subindex=0, data=0) - assert self.node.checkSDORead("proxy_device_2", index=0x1000, subindex=0, data=0) - - def test_04_sdo_write(self): - assert self.node.checkSDOWrite("proxy_device_1", index=0x4000, subindex=0, data=100) - assert self.node.checkSDOWrite("proxy_device_2", index=0x4000, subindex=0, data=100) - assert self.node.checkSDORead("proxy_device_1", index=0x4000, subindex=0, data=100) - assert self.node.checkSDORead("proxy_device_2", index=0x4000, subindex=0, data=100) - - def test_05_sdo_read_id(self): - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=100) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=100) - - def test_06_sdo_write_id(self): - assert self.node.checkSDOWriteID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) - assert self.node.checkSDOWriteID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) - assert self.node.checkSDOReadID(node_id=2, index=0x4000, subindex=0, type=0x7, data=999) - assert self.node.checkSDOReadID(node_id=3, index=0x4000, subindex=0, type=0x7, data=999) - - # def test_07_rpdo_tpdo(self): - # assert self.node.checkRpdoTpdo( - # "proxy_device_1", index=0x4000, subindex=0, data=101 - # ) - # assert self.node.checkRpdoTpdo( - # "proxy_device_2", index=0x4000, subindex=0, data=202 - # ) - - # def test_08_to_unconfigured(self): - # assert self.node.checkTransition( - # "lifecycle_manager", State.PRIMARY_STATE_ACTIVE, Transition.TRANSITION_DEACTIVATE - # ), "Could not configure device manager" - # assert self.node.checkTransition( - # "lifecycle_manager", State.PRIMARY_STATE_INACTIVE, Transition.TRANSITION_CLEANUP - # ), "Could not configure device manager" + + def test_full_cycle_sdo(self): + req = ChangeState.Request() + + res = ChangeState.Response() + res.success = True + + req.transition.id = Transition.TRANSITION_CONFIGURE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CONFIGURE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_ACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************ACTIVATE SUCCESSFUL*************************") + + msg = COData() + msg.index = 0x4000 + msg.subindex = 0 + msg.data = 200 + + pub_msg = COData() + pub_msg.index = 0x4001 + pub_msg.subindex = 0 + pub_msg.data = 200 + thread = threading.Thread( + target=self.node.subscribe_and_wait_for_message, + args=["proxy_device_1/rpdo", COData, pub_msg], + ) + thread.start() + self.node.publish_message("proxy_device_1/tpdo", COData, msg) + time.sleep(0.1) + msg.data = 0 + self.node.publish_message("proxy_device_1/tpdo", COData, msg) + thread.join() + + req.transition.id = Transition.TRANSITION_DEACTIVATE + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************DEACTIVATE SUCCESSFUL*************************") + req.transition.id = Transition.TRANSITION_CLEANUP + self.node.call_service("/lifecycle_manager/change_state", ChangeState, req, res) + print("*************************CLEANUP SUCCESSFUL*************************") diff --git a/canopen_tests/launch_tests/test_robot_control.py b/canopen_tests/launch_tests/test_robot_control.py new file mode 100644 index 00000000..91aa448b --- /dev/null +++ b/canopen_tests/launch_tests/test_robot_control.py @@ -0,0 +1,92 @@ +# Copyright 2022 Christoph Hellmann Santos +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from time import sleep +import time +import pytest +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_testing +import threading +import rclpy +from rclpy.node import Node +from canopen_utils.launch_test_node import LaunchTestNode +from canopen_interfaces.srv import CORead, COWrite, COReadID, COWriteID, COTargetDouble +from canopen_interfaces.msg import COData +from std_srvs.srv import Trigger +from std_msgs.msg import Float64MultiArray +import unittest + + +@pytest.mark.rostest +def generate_test_description(): + + launch_desc = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join(get_package_share_directory("canopen_tests"), "launch"), + "/robot_control_setup.launch.py", + ] + ) + ) + + ready_to_test = launch.actions.TimerAction( + period=5.0, + actions=[launch_testing.actions.ReadyToTest()], + ) + + return (LaunchDescription([launch_desc, ready_to_test]), {}) + + +class TestSDO(unittest.TestCase): + def run_node(self): + while self.ok: + rclpy.spin_once(self.node, timeout_sec=0.1) + + @classmethod + def setUp(cls): + cls.ok = True + rclpy.init() + cls.node = LaunchTestNode() + cls.thread = threading.Thread(target=cls.run_node, args=[cls]) + cls.thread.start() + + @classmethod + def tearDown(cls): + cls.ok = False + cls.thread.join() + cls.node.destroy_node() + rclpy.shutdown() + + def test_sdo_read(self): + req = CORead.Request() + req.index = 0x1000 + req.subindex = 0 + + res = CORead.Response() + res.success = True + res.data = 0x60192 + + self.node.call_service("joint_1/sdo_read", CORead, req, res) + + def test_forward_control(self): + message = Float64MultiArray() + message.data = [1.0, 0.0] + + self.node.publish_message("forward_position_controller", Float64MultiArray, message) + time.sleep(1.0) diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 9fe51c70..fb89228d 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.2.0 + 0.2.1 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 90216616..966f93e2 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Add more tidy launch_test_node.py +* Contributors: Christoph Hellmann Santos + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/canopen_utils/canopen_utils/launch_test_node.py b/canopen_utils/canopen_utils/launch_test_node.py new file mode 100644 index 00000000..d0b1ef5e --- /dev/null +++ b/canopen_utils/canopen_utils/launch_test_node.py @@ -0,0 +1,125 @@ +import time +import re + +import launch_testing +import launch_testing.asserts +import launch_testing.actions +import launch_testing.util +import launch_testing.tools +from launch_testing.asserts import assertSequentialStdout + +from rclpy.node import Node +from rclpy.publisher import Publisher +from rclpy.subscription import Subscription +from rclpy.client import Client +from rclpy.qos import QoSProfile +from threading import Condition + + +class Ros2ActiveIoHandler: + def __init__(self, proc_output: launch_testing.ActiveIoHandler): + self.proc_output = proc_output + + def checkInRos2Stream(self, process, expected_output: str): + resolved_procs = launch_testing.util.resolveProcesses( + info_obj=self.proc_output, process=process, cmd_args=None, strict_proc_matching=True + ) + + pat = re.compile(r".*\]: " + expected_output) + + for proc in resolved_procs: + for output in self.proc_output[proc]: + text = output.text.decode() + print("recceived: " + text) + res = pat.search(text) + if res is not None: + return True + return False + + def waitFor(self, process, expected_output: str, timeout: float = 1.0): + success = False + + with self.proc_output._sync_lock: + # TODO(pete.baughman): Searching through all of the IO can be time consuming/wasteful. + # We can optimize this by making a note of where we left off searching and only + # searching new messages when we return from the wait. + success = self.proc_output._sync_lock.wait_for( + lambda: self.checkInRos2Stream(process=process, expected_output=expected_output), + timeout=timeout, + ) + + return success + + def assertWaitFor(self, process, expected_output: str, timeout: float = 1.0): + success = self.waitFor(process=process, expected_output=expected_output, timeout=timeout) + assert success, f"Waiting for output {expected_output} timed out" + + +class LaunchTestNode(Node): + def __init__(self): + super().__init__("launch_test_node") + self.get_logger().info("launch_test_node initialized") + + def publish_delayed(self, publisher: Publisher, msg, delay: float): + publisher.publish(msg) + time.sleep(delay) + + def publish_and_check_output( + self, + publisher: Publisher, + msg, + expected_output: str, + proc_output: launch_testing.ActiveIoHandler, + process=None, + ): + + self.publish_delayed(publisher, msg, 0.1) + Ros2ActiveIoHandler(proc_output).assertWaitFor( + process=process, expected_output=expected_output + ) + + def subscribe_and_wait_for_message(self, topic: str, msg_type, msg, timeout: float = 1.0): + condition = Condition() + self.received = False + + def callback(rec_msg): + with condition: + if not self.received: + self.received = msg == rec_msg + if self.received: + condition.notify() + + subscription: Subscription = self.create_subscription(msg_type, topic, callback, 1) + with condition: + condition.wait(timeout=timeout) + self.destroy_subscription(subscription) + assert self.received, "Did not receive the expected message." + + def call_service( + self, service_name: str, srv_type, service_request, expected_response, timeout: float = 2.0 + ): + condition = Condition() + + def callback(arg): + with condition: + condition.notify() + + client: Client = self.create_client( + srv_type, service_name, qos_profile=QoSProfile(depth=1) + ) + if not client.wait_for_service(timeout_sec=timeout): + assert False, "Service server not available." + future = client.call_async(service_request) + future.add_done_callback(callback=callback) + with condition: + if not condition.wait(timeout=timeout): + assert False, "Service call timed out." + + res = future.result() + assert res == expected_response, "Did not receive the expected response." + self.destroy_client(client) + + def publish_message(self, topic_name: str, topic_type, msg): + publisher = self.create_publisher(topic_type, topic_name, 10) + publisher.publish(msg) + self.destroy_publisher(publisher) diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index df4a62be..655d6627 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.2.0 + 0.2.1 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index 41e8aabb..36a60b80 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.2.0", + version="0.2.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 6f8a9bd0..8a0a8a0d 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2023-06-21) +------------------ +* Do not build dcf-tools bdist in lely_core_libraries anymore, fixes buildfarm issue +* Contributors: Christoph Hellmann Santos + 0.2.0 (2023-06-14) ------------------ * Created package diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index 980568cb..194d4518 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -10,7 +10,8 @@ ExternalProject_Add(upstr_lely_core_libraries # Name for custom target GIT_TAG 7824cbb2ac08d091c4fa2fb397669b938de9e3f5 TIMEOUT 60 #--Update/Patch step---------- - # UPDATE_COMMAND mkdir -p /include /lib + UPDATE_COMMAND git apply ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Don-t-build-bdist.patch + COMMAND git apply ${CMAKE_CURRENT_SOURCE_DIR}/patches/0002-No-bdist.patch #--Configure step------------- CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag #--Build step----------------- diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index 9eddf087..bb731491 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.2.0 + 0.2.1 ROS wrapper for lely-core-libraries diff --git a/lely_core_libraries/patches/0001-Don-t-build-bdist.patch b/lely_core_libraries/patches/0001-Don-t-build-bdist.patch new file mode 100644 index 00000000..e893e8a3 --- /dev/null +++ b/lely_core_libraries/patches/0001-Don-t-build-bdist.patch @@ -0,0 +1,30 @@ +From 10217b41326c2a0a878068558612c774854eb496 Mon Sep 17 00:00:00 2001 +From: Christoph Hellmann Santos +Date: Wed, 21 Jun 2023 08:19:04 +0200 +Subject: [PATCH] Don't build bdist + +--- + python/dcf-tools/Makefile.am | 7 ------- + 1 file changed, 7 deletions(-) + +diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am +index 9852c84a..58edc597 100644 +--- a/python/dcf-tools/Makefile.am ++++ b/python/dcf-tools/Makefile.am +@@ -30,13 +30,6 @@ clean-local: + + install-exec-local: python-install + +-.PHONY: python-bdist_wheel +-python-bdist_wheel: +-if HAVE_PYTHON3 +- @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ +- bdist_wheel --dist-dir $(dist_dir) +-endif +- + .PHONY: python-install + python-install: + if HAVE_PYTHON3 +-- +2.34.1 + diff --git a/lely_core_libraries/patches/0002-No-bdist.patch b/lely_core_libraries/patches/0002-No-bdist.patch new file mode 100644 index 00000000..ffe6f9f4 --- /dev/null +++ b/lely_core_libraries/patches/0002-No-bdist.patch @@ -0,0 +1,25 @@ +From e122577c29f833fd5d2d9066c1a1740008d66776 Mon Sep 17 00:00:00 2001 +From: Christoph Hellmann Santos +Date: Wed, 21 Jun 2023 08:25:33 +0200 +Subject: [PATCH] No bdist + +--- + python/dcf-tools/Makefile.am | 2 +- + 1 file changed, 1 insertion(+), 1 deletion(-) + +diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am +index 58edc597..8a5b2c24 100644 +--- a/python/dcf-tools/Makefile.am ++++ b/python/dcf-tools/Makefile.am +@@ -23,7 +23,7 @@ EXTRA_DIST += setup.py + build_base = $(realpath $(builddir))/build + dist_dir = $(realpath $(builddir))/dist + +-all-local: python-sdist python-bdist_wheel ++all-local: python-sdist + + clean-local: + rm -rf $(build_base) $(dist_dir) $(srcdir)/*.egg-info $(builddir)/*.egg-info +-- +2.34.1 + From 150f40d92f247eab18cf5e9128377a2856f85999 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 21 Jun 2023 21:14:22 +0200 Subject: [PATCH 19/59] Force patch application --- lely_core_libraries/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index 194d4518..f13df9b0 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -10,8 +10,8 @@ ExternalProject_Add(upstr_lely_core_libraries # Name for custom target GIT_TAG 7824cbb2ac08d091c4fa2fb397669b938de9e3f5 TIMEOUT 60 #--Update/Patch step---------- - UPDATE_COMMAND git apply ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Don-t-build-bdist.patch - COMMAND git apply ${CMAKE_CURRENT_SOURCE_DIR}/patches/0002-No-bdist.patch + UPDATE_COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Don-t-build-bdist.patch + COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0002-No-bdist.patch #--Configure step------------- CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag #--Build step----------------- From 8d63e51ebefbc655f35429345df652a431c6ea78 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 21 Jun 2023 21:41:15 +0200 Subject: [PATCH 20/59] Update rolling.yml --- .github/workflows/rolling.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index b72124d1..f2f72ce1 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -16,6 +16,7 @@ jobs: matrix: ROS_DISTRO: [rolling, iron] ROS_REPO: [testing] + PRERELEASE: [true, false] env: CCACHE_DIR: "${{ github.workspace }}/.ccache" steps: @@ -32,3 +33,4 @@ jobs: AFTER_BUILD_TARGET_WORKSPACE: 'set +u && COLCON_TRACE=0 AMENT_TRACE_SETUP_FILES=0 source /root/target_ws/install/setup.bash && set -u' ROS_DISTRO: ${{ matrix.ROS_DISTRO }} ROS_REPO: ${{ matrix.ROS_REPO }} + PRERELEASE: ${{ matrix.PRERELEASE }} From 9f68a47f06cbece8522ffd54dba874e137d471f0 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 21 Jun 2023 21:42:43 +0200 Subject: [PATCH 21/59] Update rolling.yml --- .github/workflows/rolling.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index f2f72ce1..bd1b0851 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -33,4 +33,4 @@ jobs: AFTER_BUILD_TARGET_WORKSPACE: 'set +u && COLCON_TRACE=0 AMENT_TRACE_SETUP_FILES=0 source /root/target_ws/install/setup.bash && set -u' ROS_DISTRO: ${{ matrix.ROS_DISTRO }} ROS_REPO: ${{ matrix.ROS_REPO }} - PRERELEASE: ${{ matrix.PRERELEASE }} + PRERELEASE: true From 3f25a601c504dfde31b495bd7c3677119e5b43de Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 21 Jun 2023 22:24:02 +0200 Subject: [PATCH 22/59] Update rolling.yml --- .github/workflows/rolling.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index bd1b0851..1b252c74 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -16,7 +16,6 @@ jobs: matrix: ROS_DISTRO: [rolling, iron] ROS_REPO: [testing] - PRERELEASE: [true, false] env: CCACHE_DIR: "${{ github.workspace }}/.ccache" steps: @@ -33,4 +32,4 @@ jobs: AFTER_BUILD_TARGET_WORKSPACE: 'set +u && COLCON_TRACE=0 AMENT_TRACE_SETUP_FILES=0 source /root/target_ws/install/setup.bash && set -u' ROS_DISTRO: ${{ matrix.ROS_DISTRO }} ROS_REPO: ${{ matrix.ROS_REPO }} - PRERELEASE: true + PRERELEASE: false From 0823f84a4e8387ef405fc8b85ffc654ae7f04bb4 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 21 Jun 2023 22:33:04 +0200 Subject: [PATCH 23/59] Reset hard before patch applicaiton Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index f13df9b0..09a70516 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -10,7 +10,9 @@ ExternalProject_Add(upstr_lely_core_libraries # Name for custom target GIT_TAG 7824cbb2ac08d091c4fa2fb397669b938de9e3f5 TIMEOUT 60 #--Update/Patch step---------- - UPDATE_COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Don-t-build-bdist.patch + UPDATE_COMMAND + COMMAND git reset --hard + COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Don-t-build-bdist.patch COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0002-No-bdist.patch #--Configure step------------- CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag From ceb5e9a58743268e28757565e1581f049c669e99 Mon Sep 17 00:00:00 2001 From: Vishnuprasad Prachandabhanu Date: Wed, 12 Jul 2023 11:10:47 +0200 Subject: [PATCH 24/59] Add buildfarm dev job status --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 7ebb4364..18736c4a 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,11 @@ ## Status +| Build Process | Status | +|---------------|--------| +| Industrial CI Build | [![CI Build Status](https://github.com/ros-industrial/ros2_canopen/workflows/rolling/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) | +| Documentation Build | [![CI Documentation Status](https://github.com/ros-industrial/ros2_canopen/workflows/Documentation/badge.svg?branch=master)](https://github.com/ros-industrial/ros2_canopen/actions) | +| Buildfarm Build | [![Buildfarm Status](https://build.ros2.org/job/Rdev__ros2_canopen__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Rdev__ros2_canopen__ubuntu_jammy_amd64/) | [![humble](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble.yml/badge.svg)](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble.yml) [![HUMBLE Documentation](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble_documentation.yml/badge.svg)](https://github.com/ros-industrial/ros2_canopen/actions/workflows/humble_documentation.yml) From 6611aa069eaf1e216e537047a9d7f57ff6bb845d Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 21 Jun 2023 22:59:42 +0200 Subject: [PATCH 25/59] Update Changelogs Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 3 +++ canopen_402_driver/CHANGELOG.rst | 3 +++ canopen_base_driver/CHANGELOG.rst | 3 +++ canopen_core/CHANGELOG.rst | 3 +++ canopen_fake_slaves/CHANGELOG.rst | 3 +++ canopen_interfaces/CHANGELOG.rst | 3 +++ canopen_master_driver/CHANGELOG.rst | 3 +++ canopen_proxy_driver/CHANGELOG.rst | 3 +++ canopen_ros2_control/CHANGELOG.rst | 3 +++ canopen_ros2_controllers/CHANGELOG.rst | 3 +++ canopen_tests/CHANGELOG.rst | 3 +++ canopen_utils/CHANGELOG.rst | 3 +++ lely_core_libraries/CHANGELOG.rst | 5 +++++ 13 files changed, 41 insertions(+) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 1add7c7e..7670b14e 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index 46590b89..e5a2dc5f 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Fix boost/std placeholders ambiguity in older boost versions diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index ba1cc883..256f3cf1 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Fix base driver lifecyle diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 5dccc159..f447c553 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Fix master and driver lifecycle diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index bb9afe52..6abc5d34 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Fix fake slave for PDOs diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 8ec11f7b..7fd1a358 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 08b0dcee..9489c32d 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Fix master lifecycle diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 637e093b..bb440a67 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Use consistenlty (uppercase) HEX output for NodeID and Index. diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 210a280f..3c6f4d8e 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Use consistenlty (uppercase) HEX output for NodeID and Index. diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index c0f8e9df..fa94167f 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Fix QoS build warning canopen_ros2_controllers diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 6532b5e5..c09b80ff 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Add trvivial integration test for robot_control diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 966f93e2..37618ecd 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.1 (2023-06-21) ------------------ * Add more tidy launch_test_node.py diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 8a0a8a0d..a6e447e3 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Reset hard before patch applicaiton +* Contributors: Christoph Hellmann Santos + 0.2.1 (2023-06-21) ------------------ * Do not build dcf-tools bdist in lely_core_libraries anymore, fixes buildfarm issue From 200ba25f0e50996a55501e04b864f31f84f8cc90 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Wed, 21 Jun 2023 23:00:03 +0200 Subject: [PATCH 26/59] 0.2.2 --- canopen/CHANGELOG.rst | 4 ++-- canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 4 ++-- canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 4 ++-- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 4 ++-- canopen_core/package.xml | 2 +- canopen_fake_slaves/CHANGELOG.rst | 4 ++-- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 4 ++-- canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 4 ++-- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/CHANGELOG.rst | 4 ++-- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 4 ++-- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 4 ++-- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/CHANGELOG.rst | 4 ++-- canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 4 ++-- canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- lely_core_libraries/package.xml | 2 +- 27 files changed, 40 insertions(+), 40 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 7670b14e..dc0b1c8c 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen/package.xml b/canopen/package.xml index 9e795d92..e34c70cb 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.2.1 + 0.2.2 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index e5a2dc5f..75b53018 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index 52faa3c9..2d24cf29 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.2.1 + 0.2.2 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 256f3cf1..463a0af7 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index ac9bc2c7..d9b36cb9 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.2.1 + 0.2.2 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index f447c553..42486701 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_core/package.xml b/canopen_core/package.xml index b99e2655..553a3778 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.2.1 + 0.2.2 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 6abc5d34..fef18c22 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index fc15bbb7..d5929011 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.2.1 + 0.2.2 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 7fd1a358..1469aef3 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 9f64d202..9b5599b0 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.2.1 + 0.2.2 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 9489c32d..c2d6893b 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index 7ab2a0d8..6a35603e 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.2.1 + 0.2.2 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index bb440a67..336491e3 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index d72542af..c414f266 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.2.1 + 0.2.2 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 3c6f4d8e..1211dcb0 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index 4515e04e..c4e7a15b 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.2.1 + 0.2.2 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index fa94167f..4be682dd 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index 887ab274..47a0b11f 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.2.1 + 0.2.2 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index c09b80ff..e243cfd0 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index fb89228d..dbcc770c 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.2.1 + 0.2.2 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 37618ecd..d3d79878 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ 0.2.1 (2023-06-21) ------------------ diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index 655d6627..4e7defff 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.2.1 + 0.2.2 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index 36a60b80..4769c459 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.2.1", + version="0.2.2", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index a6e447e3..6f95abdf 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.2 (2023-06-21) +------------------ * Reset hard before patch applicaiton * Contributors: Christoph Hellmann Santos diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index bb731491..77010227 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.2.1 + 0.2.2 ROS wrapper for lely-core-libraries From e50a2603f4836a57b3a6ccc805edc47f311cba6c Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Thu, 22 Jun 2023 09:35:27 +0200 Subject: [PATCH 27/59] Solve buildfarm issues (#155) * Move exec deps from ros2_control to tests Signed-off-by: Christoph Hellmann Santos * Remove mkdir in install dir from cogen and dcfgen This causes a permission denied error on buildfarm. The install command creates it anyways Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos --- canopen_ros2_control/package.xml | 12 ------------ canopen_tests/package.xml | 12 ++++++++++++ .../cmake/lely_core_libraries-extras.cmake | 4 ++-- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index c4e7a15b..84337fc0 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -20,18 +20,6 @@ rclcpp_components rclcpp_lifecycle - xacro - controller_manager - robot_state_publisher - - joint_state_broadcaster - joint_trajectory_controller - forward_command_controller - - canopen_fake_slaves - canopen_tests - canopen_ros2_controllers - ament_cmake_gmock ros2_control_test_assets diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index dbcc770c..4a35e46a 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -15,6 +15,18 @@ canopen_core canopen_proxy_driver + xacro + controller_manager + robot_state_publisher + + joint_state_broadcaster + joint_trajectory_controller + forward_command_controller + + canopen_fake_slaves + canopen_tests + canopen_ros2_controllers + ament_lint_auto diff --git a/lely_core_libraries/cmake/lely_core_libraries-extras.cmake b/lely_core_libraries/cmake/lely_core_libraries-extras.cmake index 6aab6f02..35a81c36 100644 --- a/lely_core_libraries/cmake/lely_core_libraries-extras.cmake +++ b/lely_core_libraries/cmake/lely_core_libraries-extras.cmake @@ -10,7 +10,7 @@ macro( COMMAND rm -rf ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/* COMMAND rm -rf ${CMAKE_BINARY_DIR}/config/${TARGET}/* COMMAND mkdir -p ${CMAKE_BINARY_DIR}/config/${TARGET} - COMMAND mkdir -p ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/ + #COMMAND mkdir -p ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/ ) add_custom_target( @@ -47,7 +47,7 @@ macro( COMMAND rm -rf ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/* COMMAND rm -rf ${CMAKE_BINARY_DIR}/config/${TARGET}/* COMMAND mkdir -p ${CMAKE_BINARY_DIR}/config/${TARGET} - COMMAND mkdir -p ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/ + #COMMAND mkdir -p ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config/${TARGET}/ ) add_custom_target( From 3a7934cbef8042478b27a0c6e46c91fe56303abe Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Thu, 22 Jun 2023 09:41:28 +0200 Subject: [PATCH 28/59] Create prerelease-rolling.yml --- .github/workflows/prerelease-rolling.yml | 31 ++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 .github/workflows/prerelease-rolling.yml diff --git a/.github/workflows/prerelease-rolling.yml b/.github/workflows/prerelease-rolling.yml new file mode 100644 index 00000000..7fb183b5 --- /dev/null +++ b/.github/workflows/prerelease-rolling.yml @@ -0,0 +1,31 @@ +name: Prerelease-Test Rolling + +on: + workflow_dispatch: + +jobs: + industrial_ci: + name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + ROS_REPO: [testing] + env: + CCACHE_DIR: "${{ github.workspace }}/.ccache" + steps: + - uses: actions/checkout@v2 + - uses: actions/cache@v2 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} + restore-keys: | + ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- + - uses: 'ros-industrial/industrial_ci@master' + env: + BEFORE_INSTALL_TARGET_DEPENDENCIES: 'sudo apt-get update' + AFTER_BUILD_TARGET_WORKSPACE: 'set +u && COLCON_TRACE=0 AMENT_TRACE_SETUP_FILES=0 source /root/target_ws/install/setup.bash && set -u' + ROS_DISTRO: ${{ matrix.ROS_DISTRO }} + ROS_REPO: ${{ matrix.ROS_REPO }} + PRERELEASE: true From afdc6038b14d20716f75dc3976cc9832a4940dd6 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Thu, 22 Jun 2023 09:44:30 +0200 Subject: [PATCH 29/59] Update package.xml --- canopen_tests/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 4a35e46a..2fe52856 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -24,7 +24,6 @@ forward_command_controller canopen_fake_slaves - canopen_tests canopen_ros2_controllers ament_lint_auto From cf02ba2ee34d9a0215e84981ebb847182e9263c6 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 09:48:09 +0200 Subject: [PATCH 30/59] Readable CI naming Signed-off-by: Christoph Hellmann Santos --- .github/workflows/ci-format.yml | 2 +- .github/workflows/humble.yml | 2 +- .github/workflows/humble_documentation.yml | 2 +- .github/workflows/iron.yml | 2 +- .github/workflows/iron_documentation.yml | 2 +- .github/workflows/rolling.yml | 2 +- .github/workflows/rolling_documentation.yml | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 36ea2778..c18e42c9 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -1,7 +1,7 @@ # This is a format job. Pre-commit has a first-party GitHub action, so we use # that: https://github.com/pre-commit/action -name: Format +name: Pre-commit Format on: workflow_dispatch: diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index db46e326..2d410538 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -1,4 +1,4 @@ -name: humble +name: Industrial CI Humble on: push: diff --git a/.github/workflows/humble_documentation.yml b/.github/workflows/humble_documentation.yml index fb04666b..06200578 100644 --- a/.github/workflows/humble_documentation.yml +++ b/.github/workflows/humble_documentation.yml @@ -1,4 +1,4 @@ -name: HUMBLE Documentation +name: Documentation Humble # Controls when the workflow will run on: diff --git a/.github/workflows/iron.yml b/.github/workflows/iron.yml index 5b99389a..f21224ce 100644 --- a/.github/workflows/iron.yml +++ b/.github/workflows/iron.yml @@ -1,4 +1,4 @@ -name: iron +name: Industiral CI Iron on: push: diff --git a/.github/workflows/iron_documentation.yml b/.github/workflows/iron_documentation.yml index a8bcef2b..59f03908 100644 --- a/.github/workflows/iron_documentation.yml +++ b/.github/workflows/iron_documentation.yml @@ -1,4 +1,4 @@ -name: IRON Documentation +name: Documentation Iron # Controls when the workflow will run on: diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 1b252c74..94065799 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -1,4 +1,4 @@ -name: rolling +name: Industrial CI Rolling on: push: diff --git a/.github/workflows/rolling_documentation.yml b/.github/workflows/rolling_documentation.yml index 70428392..ee335cba 100644 --- a/.github/workflows/rolling_documentation.yml +++ b/.github/workflows/rolling_documentation.yml @@ -1,4 +1,4 @@ -name: ROLLING Documentation +name: Documentation Rolling # Controls when the workflow will run on: From 8ed8413f9e2252b0f68b9ce7cdbe923a46111f8b Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 09:55:20 +0200 Subject: [PATCH 31/59] Remove ccache for prerelease testing Signed-off-by: Christoph Hellmann Santos --- .github/workflows/prerelease-rolling.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/prerelease-rolling.yml b/.github/workflows/prerelease-rolling.yml index 7fb183b5..d39fcd06 100644 --- a/.github/workflows/prerelease-rolling.yml +++ b/.github/workflows/prerelease-rolling.yml @@ -12,8 +12,6 @@ jobs: matrix: ROS_DISTRO: [rolling] ROS_REPO: [testing] - env: - CCACHE_DIR: "${{ github.workspace }}/.ccache" steps: - uses: actions/checkout@v2 - uses: actions/cache@v2 From 52ee896d229b6998030646f73d679008a221c9c4 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 09:56:45 +0200 Subject: [PATCH 32/59] Remove cchache action Signed-off-by: Christoph Hellmann Santos --- .github/workflows/prerelease-rolling.yml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/.github/workflows/prerelease-rolling.yml b/.github/workflows/prerelease-rolling.yml index d39fcd06..bac8bac8 100644 --- a/.github/workflows/prerelease-rolling.yml +++ b/.github/workflows/prerelease-rolling.yml @@ -14,12 +14,6 @@ jobs: ROS_REPO: [testing] steps: - uses: actions/checkout@v2 - - uses: actions/cache@v2 - with: - path: ${{ env.CCACHE_DIR }} - key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} - restore-keys: | - ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- - uses: 'ros-industrial/industrial_ci@master' env: BEFORE_INSTALL_TARGET_DEPENDENCIES: 'sudo apt-get update' From 02aa86c1ba15031412c3a7a75dad2571bd1b3462 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Thu, 22 Jun 2023 17:13:48 +0200 Subject: [PATCH 33/59] Fix buildfarm issue with lely_core_libraries (#158) Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/CMakeLists.txt | 22 +++++++--- .../patches/0001-Don-t-build-bdist.patch | 30 ------------- .../patches/0001-Fix-dcf-tools.patch | 44 +++++++++++++++++++ .../patches/0002-No-bdist.patch | 25 ----------- 4 files changed, 59 insertions(+), 62 deletions(-) delete mode 100644 lely_core_libraries/patches/0001-Don-t-build-bdist.patch create mode 100644 lely_core_libraries/patches/0001-Fix-dcf-tools.patch delete mode 100644 lely_core_libraries/patches/0002-No-bdist.patch diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index 09a70516..eb8510df 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -2,33 +2,41 @@ cmake_minimum_required(VERSION 3.8) project(lely_core_libraries) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) +set(python_version "python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}") include(ExternalProject) ExternalProject_Add(upstr_lely_core_libraries # Name for custom target #--Download step-------------- + SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/upstream GIT_REPOSITORY https://gitlab.com/lely_industries/lely-core.git GIT_TAG 7824cbb2ac08d091c4fa2fb397669b938de9e3f5 TIMEOUT 60 #--Update/Patch step---------- UPDATE_COMMAND COMMAND git reset --hard - COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Don-t-build-bdist.patch - COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0002-No-bdist.patch + COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Fix-dcf-tools.patch #--Configure step------------- CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag #--Build step----------------- BUILD_IN_SOURCE 1 # Use source dir for build dir #--Install step--------------- INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}" # Installation prefix - BUILD_COMMAND cd COMMAND $(MAKE) --silent + BUILD_COMMAND cd COMMAND $(MAKE) ) - +message(STATUS "PREFIX_PATH: ${CMAKE_PREFIX_PATH}") +message(STATUS "BUILD_DIR: ${CMAKE_CURRENT_BINARY_DIR}") +message(STATUS "INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}") install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib DESTINATION ${CMAKE_INSTALL_PREFIX}) +install(DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}/dcfgen_lib/ + DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/${python_version}/site-packages + PATTERN "dcfgen_lib/*" + ) install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/include DESTINATION ${CMAKE_INSTALL_PREFIX}) -install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin/ DESTINATION bin +install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin/ DESTINATION ${CMAKE_INSTALL_PREFIX}/bin PATTERN "bin/*" PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ - GROUP_EXECUTE GROUP_READ) + GROUP_EXECUTE GROUP_READ WORLD_READ WORLD_EXECUTE) set(lely_core_cmake_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include("cmake/lely_core_libraries-extras.cmake" NO_POLICY_SCOPE) @@ -44,7 +52,7 @@ ament_python_install_package(cogen SCRIPTS_DESTINATION lib/cogen) # install entry-point script(s) in bin as well install( DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_python/cogen/scripts/ - DESTINATION bin/ + DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ USE_SOURCE_PERMISSIONS) ament_export_include_directories(include) diff --git a/lely_core_libraries/patches/0001-Don-t-build-bdist.patch b/lely_core_libraries/patches/0001-Don-t-build-bdist.patch deleted file mode 100644 index e893e8a3..00000000 --- a/lely_core_libraries/patches/0001-Don-t-build-bdist.patch +++ /dev/null @@ -1,30 +0,0 @@ -From 10217b41326c2a0a878068558612c774854eb496 Mon Sep 17 00:00:00 2001 -From: Christoph Hellmann Santos -Date: Wed, 21 Jun 2023 08:19:04 +0200 -Subject: [PATCH] Don't build bdist - ---- - python/dcf-tools/Makefile.am | 7 ------- - 1 file changed, 7 deletions(-) - -diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am -index 9852c84a..58edc597 100644 ---- a/python/dcf-tools/Makefile.am -+++ b/python/dcf-tools/Makefile.am -@@ -30,13 +30,6 @@ clean-local: - - install-exec-local: python-install - --.PHONY: python-bdist_wheel --python-bdist_wheel: --if HAVE_PYTHON3 -- @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ -- bdist_wheel --dist-dir $(dist_dir) --endif -- - .PHONY: python-install - python-install: - if HAVE_PYTHON3 --- -2.34.1 - diff --git a/lely_core_libraries/patches/0001-Fix-dcf-tools.patch b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch new file mode 100644 index 00000000..53d85cb1 --- /dev/null +++ b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch @@ -0,0 +1,44 @@ +From 80b4aacd880ea66151eb8cf94510e8ae7daa8371 Mon Sep 17 00:00:00 2001 +From: Christoph Hellmann Santos +Date: Thu, 22 Jun 2023 13:52:11 +0200 +Subject: [PATCH] Fix dcf-tools + +--- + python/dcf-tools/Makefile.am | 11 ++--------- + 1 file changed, 2 insertions(+), 9 deletions(-) + +diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am +index 9852c84a..793c2804 100644 +--- a/python/dcf-tools/Makefile.am ++++ b/python/dcf-tools/Makefile.am +@@ -23,25 +23,18 @@ EXTRA_DIST += setup.py + build_base = $(realpath $(builddir))/build + dist_dir = $(realpath $(builddir))/dist + +-all-local: python-sdist python-bdist_wheel ++all-local: python-sdist + + clean-local: + rm -rf $(build_base) $(dist_dir) $(srcdir)/*.egg-info $(builddir)/*.egg-info + + install-exec-local: python-install + +-.PHONY: python-bdist_wheel +-python-bdist_wheel: +-if HAVE_PYTHON3 +- @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ +- bdist_wheel --dist-dir $(dist_dir) +-endif +- + .PHONY: python-install + python-install: + if HAVE_PYTHON3 + @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ +- install --prefix $(DESTDIR)$(prefix) --root / ++ egg_info build install --record install.log --install-scripts $(DESTDIR)$(prefix)/bin/ --install-lib $(DESTDIR)$(prefix)/dcfgen_lib/ --single-version-externally-managed + endif + + .PHONY: python-sdist +-- +2.34.1 + diff --git a/lely_core_libraries/patches/0002-No-bdist.patch b/lely_core_libraries/patches/0002-No-bdist.patch deleted file mode 100644 index ffe6f9f4..00000000 --- a/lely_core_libraries/patches/0002-No-bdist.patch +++ /dev/null @@ -1,25 +0,0 @@ -From e122577c29f833fd5d2d9066c1a1740008d66776 Mon Sep 17 00:00:00 2001 -From: Christoph Hellmann Santos -Date: Wed, 21 Jun 2023 08:25:33 +0200 -Subject: [PATCH] No bdist - ---- - python/dcf-tools/Makefile.am | 2 +- - 1 file changed, 1 insertion(+), 1 deletion(-) - -diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am -index 58edc597..8a5b2c24 100644 ---- a/python/dcf-tools/Makefile.am -+++ b/python/dcf-tools/Makefile.am -@@ -23,7 +23,7 @@ EXTRA_DIST += setup.py - build_base = $(realpath $(builddir))/build - dist_dir = $(realpath $(builddir))/dist - --all-local: python-sdist python-bdist_wheel -+all-local: python-sdist - - clean-local: - rm -rf $(build_base) $(dist_dir) $(srcdir)/*.egg-info $(builddir)/*.egg-info --- -2.34.1 - From a70cc72c440181bc77b5fb0c66e8c985fee716a0 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 17:17:37 +0200 Subject: [PATCH 34/59] Update Changelogs Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 3 +++ canopen_402_driver/CHANGELOG.rst | 3 +++ canopen_base_driver/CHANGELOG.rst | 3 +++ canopen_core/CHANGELOG.rst | 3 +++ canopen_fake_slaves/CHANGELOG.rst | 3 +++ canopen_interfaces/CHANGELOG.rst | 3 +++ canopen_master_driver/CHANGELOG.rst | 3 +++ canopen_proxy_driver/CHANGELOG.rst | 3 +++ canopen_ros2_control/CHANGELOG.rst | 6 ++++++ canopen_ros2_controllers/CHANGELOG.rst | 3 +++ canopen_tests/CHANGELOG.rst | 11 +++++++++++ canopen_utils/CHANGELOG.rst | 3 +++ lely_core_libraries/CHANGELOG.rst | 8 ++++++++ 13 files changed, 55 insertions(+) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index dc0b1c8c..d5c6c9be 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index 75b53018..deccd02f 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 463a0af7..ab980b2d 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 42486701..a63e6bcf 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index fef18c22..40ee9605 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 1469aef3..fc8ffd85 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index c2d6893b..1983ddf8 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 336491e3..59fbd025 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 1211dcb0..ae6c581f 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Solve buildfarm issues (`#155 `_) + --------- +* Contributors: Christoph Hellmann Santos + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index 4be682dd..8e6446b8 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index e243cfd0..845b1774 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update package.xml +* Solve buildfarm issues (`#155 `_) + * Move exec deps from ros2_control to tests + * Remove mkdir in install dir from cogen and dcfgen + This causes a permission denied error on buildfarm. + The install command creates it anyways + --------- +* Contributors: Christoph Hellmann Santos + 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index d3d79878..9ce7b182 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.2 (2023-06-21) ------------------ diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 6f95abdf..103ba5e1 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix manual dcfgen python installation on buildfarm (`#158 `_) +* Solve buildfarm dependency issues (`#155 `_) + + --------- +* Contributors: Christoph Hellmann Santos + 0.2.2 (2023-06-21) ------------------ * Reset hard before patch applicaiton From 2c6900b102070f2a4407edba980b5b802a51a10f Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 17:20:17 +0200 Subject: [PATCH 35/59] Update Changelogs Signed-off-by: Christoph Hellmann Santos --- canopen_ros2_control/CHANGELOG.rst | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index ae6c581f..d4daa8ba 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -4,7 +4,7 @@ Changelog for package canopen_ros2_control Forthcoming ----------- -* Solve buildfarm issues (`#155 `_) +* Solve buildfarm issues --------- * Contributors: Christoph Hellmann Santos diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 103ba5e1..3f7630af 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -4,8 +4,8 @@ Changelog for package lely_core_libraries Forthcoming ----------- -* Fix manual dcfgen python installation on buildfarm (`#158 `_) -* Solve buildfarm dependency issues (`#155 `_) +* Fix manual dcfgen python installation on buildfarm +* Solve buildfarm dependency issues --------- * Contributors: Christoph Hellmann Santos From 1e4c2ed0086d1926db6e4e0289219fa4ef669732 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 17:20:56 +0200 Subject: [PATCH 36/59] Update Changelogs Signed-off-by: Christoph Hellmann Santos --- canopen_ros2_control/CHANGELOG.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index d4daa8ba..955f804c 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -5,7 +5,6 @@ Changelog for package canopen_ros2_control Forthcoming ----------- * Solve buildfarm issues - --------- * Contributors: Christoph Hellmann Santos 0.2.2 (2023-06-21) From 7384531380828b35714bbd30d7b3a70448ccd490 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 17:21:38 +0200 Subject: [PATCH 37/59] Update Changelogs Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/CHANGELOG.rst | 2 -- 1 file changed, 2 deletions(-) diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 3f7630af..57667d27 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -6,8 +6,6 @@ Forthcoming ----------- * Fix manual dcfgen python installation on buildfarm * Solve buildfarm dependency issues - - --------- * Contributors: Christoph Hellmann Santos 0.2.2 (2023-06-21) From 60afd8359a7c67f71b318970e98b78ddb5c613fa Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 17:21:48 +0200 Subject: [PATCH 38/59] 0.2.3 --- canopen/CHANGELOG.rst | 4 ++-- canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 4 ++-- canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 4 ++-- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 4 ++-- canopen_core/package.xml | 2 +- canopen_fake_slaves/CHANGELOG.rst | 4 ++-- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 4 ++-- canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 4 ++-- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/CHANGELOG.rst | 4 ++-- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 4 ++-- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 4 ++-- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/CHANGELOG.rst | 4 ++-- canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 4 ++-- canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- lely_core_libraries/package.xml | 2 +- 27 files changed, 40 insertions(+), 40 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index d5c6c9be..86b291c6 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen/package.xml b/canopen/package.xml index e34c70cb..658666d1 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.2.2 + 0.2.3 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index deccd02f..f387f14d 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index 2d24cf29..04c31fde 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.2.2 + 0.2.3 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index ab980b2d..308217d7 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index d9b36cb9..5e856113 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.2.2 + 0.2.3 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index a63e6bcf..c974768f 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_core/package.xml b/canopen_core/package.xml index 553a3778..8fb365fc 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.2.2 + 0.2.3 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 40ee9605..983afb6d 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index d5929011..48dcffd5 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.2.2 + 0.2.3 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index fc8ffd85..96a118dc 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 9b5599b0..3b4fba03 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.2.2 + 0.2.3 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 1983ddf8..7e84a96a 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index 6a35603e..c4c58925 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.2.2 + 0.2.3 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 59fbd025..fba871e0 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index c414f266..84e4a7d7 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.2.2 + 0.2.3 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 955f804c..3b99e77d 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ * Solve buildfarm issues * Contributors: Christoph Hellmann Santos diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index 84337fc0..e349f14e 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.2.2 + 0.2.3 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index 8e6446b8..0d9b78a3 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index 47a0b11f..dc806181 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.2.2 + 0.2.3 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 845b1774..65ef0fe1 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ * Update package.xml * Solve buildfarm issues (`#155 `_) * Move exec deps from ros2_control to tests diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 2fe52856..8376bd26 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.2.2 + 0.2.3 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 9ce7b182..3824df3a 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ 0.2.2 (2023-06-21) ------------------ diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index 4e7defff..c4b5440f 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.2.2 + 0.2.3 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index 4769c459..44fb7253 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.2.2", + version="0.2.3", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 57667d27..e7b21f3c 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.3 (2023-06-22) +------------------ * Fix manual dcfgen python installation on buildfarm * Solve buildfarm dependency issues * Contributors: Christoph Hellmann Santos diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index 77010227..e89e8023 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.2.2 + 0.2.3 ROS wrapper for lely-core-libraries From f3a669d53b8b3d15d13c6e69db54c0df315c484c Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 22:02:42 +0200 Subject: [PATCH 39/59] Clean up lely build folder after install for rpm build Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index eb8510df..78872e3d 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -38,6 +38,8 @@ install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin/ DESTINATION ${CMAKE_INSTALL_P PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_READ WORLD_EXECUTE) +install(CODE "file(REMOVE_RECURSE ${CMAKE_CURRENT_BINARY_DIR}/lib ${CMAKE_CURRENT_BINARY_DIR}/dcfgen_lib ${CMAKE_CURRENT_BINARY_DIR}/include ${CMAKE_CURRENT_BINARY_DIR}/bin)") + set(lely_core_cmake_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include("cmake/lely_core_libraries-extras.cmake" NO_POLICY_SCOPE) From 3c2345d86c57d6e83620336ebae0c94ddeab5427 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 22:03:09 +0200 Subject: [PATCH 40/59] Add empy dependency for dcfgen Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index e89e8023..efd338bb 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -14,6 +14,7 @@ autoconf automake libtool + python3-empy ament_cmake From 1aa6f02f52096be6225351146a7b851e51aa0637 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 22:23:35 +0200 Subject: [PATCH 41/59] Update changelog Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 3 +++ canopen_402_driver/CHANGELOG.rst | 3 +++ canopen_base_driver/CHANGELOG.rst | 3 +++ canopen_core/CHANGELOG.rst | 3 +++ canopen_fake_slaves/CHANGELOG.rst | 3 +++ canopen_interfaces/CHANGELOG.rst | 3 +++ canopen_master_driver/CHANGELOG.rst | 3 +++ canopen_proxy_driver/CHANGELOG.rst | 3 +++ canopen_ros2_control/CHANGELOG.rst | 3 +++ canopen_ros2_controllers/CHANGELOG.rst | 3 +++ canopen_tests/CHANGELOG.rst | 3 +++ canopen_utils/CHANGELOG.rst | 3 +++ lely_core_libraries/CHANGELOG.rst | 6 ++++++ 13 files changed, 42 insertions(+) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 86b291c6..d773a567 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index f387f14d..fd74c0d5 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 308217d7..db98546e 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index c974768f..802f2f33 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 983afb6d..92c39067 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 96a118dc..41e3fe59 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 7e84a96a..0eb49254 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index fba871e0..df03fbc8 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 3b99e77d..bf92af96 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ * Solve buildfarm issues diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index 0d9b78a3..ff5227eb 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 65ef0fe1..4bc457cf 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ * Update package.xml diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 3824df3a..8c133c31 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.3 (2023-06-22) ------------------ diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index e7b21f3c..f45e751a 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add empy dependency for dcfgen +* Clean up lely build folder after install for rpm build +* Contributors: Christoph Hellmann Santos + 0.2.3 (2023-06-22) ------------------ * Fix manual dcfgen python installation on buildfarm From 97c98c8282c1d06b6358c886cad0efc42e77cf68 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Thu, 22 Jun 2023 22:23:53 +0200 Subject: [PATCH 42/59] 0.2.4 --- canopen/CHANGELOG.rst | 4 ++-- canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 4 ++-- canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 4 ++-- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 4 ++-- canopen_core/package.xml | 2 +- canopen_fake_slaves/CHANGELOG.rst | 4 ++-- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 4 ++-- canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 4 ++-- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/CHANGELOG.rst | 4 ++-- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 4 ++-- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 4 ++-- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/CHANGELOG.rst | 4 ++-- canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 4 ++-- canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- lely_core_libraries/package.xml | 2 +- 27 files changed, 40 insertions(+), 40 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index d773a567..7586dd26 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen/package.xml b/canopen/package.xml index 658666d1..6493164a 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.2.3 + 0.2.4 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index fd74c0d5..16f03d81 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index 04c31fde..b98094be 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.2.3 + 0.2.4 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index db98546e..0773dc4b 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index 5e856113..8907464d 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.2.3 + 0.2.4 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 802f2f33..42ba2ac3 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_core/package.xml b/canopen_core/package.xml index 8fb365fc..db7d3470 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.2.3 + 0.2.4 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 92c39067..fc6755b6 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index 48dcffd5..7120483f 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.2.3 + 0.2.4 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 41e3fe59..54cf6acd 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 3b4fba03..9fa3a645 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.2.3 + 0.2.4 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 0eb49254..14a951af 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index c4c58925..3beb7e47 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.2.3 + 0.2.4 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index df03fbc8..11bdb969 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index 84e4a7d7..e4503583 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.2.3 + 0.2.4 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index bf92af96..564acd3b 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index e349f14e..dbaabf42 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.2.3 + 0.2.4 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index ff5227eb..bef1ab05 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index dc806181..d9b27cff 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.2.3 + 0.2.4 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 4bc457cf..9e3c6f19 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 8376bd26..6eb92811 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.2.3 + 0.2.4 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 8c133c31..cb6d9b17 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ 0.2.3 (2023-06-22) ------------------ diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index c4b5440f..a7029355 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.2.3 + 0.2.4 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index 44fb7253..00ccba2f 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.2.3", + version="0.2.4", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index f45e751a..0776c146 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.4 (2023-06-22) +------------------ * Add empy dependency for dcfgen * Clean up lely build folder after install for rpm build * Contributors: Christoph Hellmann Santos diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index efd338bb..bbbea46e 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.2.3 + 0.2.4 ROS wrapper for lely-core-libraries From d5d0060829c8db466003aa730d38772b54d59825 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Fri, 23 Jun 2023 08:13:02 +0200 Subject: [PATCH 43/59] Directly install lely (#160) Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/CMakeLists.txt | 32 +++++------ .../patches/0001-Fix-dcf-tools.patch | 57 +++++++++++++++---- 2 files changed, 61 insertions(+), 28 deletions(-) diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index 78872e3d..88d9f000 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -8,6 +8,7 @@ include(ExternalProject) ExternalProject_Add(upstr_lely_core_libraries # Name for custom target #--Download step-------------- SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/upstream + INSTALL_DIR "${CMAKE_INSTALL_PREFIX}" # Installation prefix GIT_REPOSITORY https://gitlab.com/lely_industries/lely-core.git GIT_TAG 7824cbb2ac08d091c4fa2fb397669b938de9e3f5 TIMEOUT 60 @@ -20,25 +21,22 @@ ExternalProject_Add(upstr_lely_core_libraries # Name for custom target #--Build step----------------- BUILD_IN_SOURCE 1 # Use source dir for build dir #--Install step--------------- - INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}" # Installation prefix BUILD_COMMAND cd COMMAND $(MAKE) ) -message(STATUS "PREFIX_PATH: ${CMAKE_PREFIX_PATH}") -message(STATUS "BUILD_DIR: ${CMAKE_CURRENT_BINARY_DIR}") -message(STATUS "INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}") -install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib DESTINATION ${CMAKE_INSTALL_PREFIX}) -install(DIRECTORY - ${CMAKE_CURRENT_BINARY_DIR}/dcfgen_lib/ - DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/${python_version}/site-packages - PATTERN "dcfgen_lib/*" - ) -install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/include DESTINATION ${CMAKE_INSTALL_PREFIX}) -install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin/ DESTINATION ${CMAKE_INSTALL_PREFIX}/bin - PATTERN "bin/*" - PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ - GROUP_EXECUTE GROUP_READ WORLD_READ WORLD_EXECUTE) - -install(CODE "file(REMOVE_RECURSE ${CMAKE_CURRENT_BINARY_DIR}/lib ${CMAKE_CURRENT_BINARY_DIR}/dcfgen_lib ${CMAKE_CURRENT_BINARY_DIR}/include ${CMAKE_CURRENT_BINARY_DIR}/bin)") +# message(STATUS "PREFIX_PATH: ${CMAKE_PREFIX_PATH}") +# message(STATUS "BUILD_DIR: ${CMAKE_CURRENT_BINARY_DIR}") +# message(STATUS "INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}") +# install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib DESTINATION ${CMAKE_INSTALL_PREFIX}) +# install(DIRECTORY +# ${CMAKE_CURRENT_BINARY_DIR}/dcfgen_lib/ +# DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/${python_version}/site-packages +# PATTERN "dcfgen_lib/*" +# ) +# install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/include DESTINATION ${CMAKE_INSTALL_PREFIX}) +# install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin/ DESTINATION ${CMAKE_INSTALL_PREFIX}/bin +# PATTERN "bin/*" +# PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ +# GROUP_EXECUTE GROUP_READ WORLD_READ WORLD_EXECUTE) set(lely_core_cmake_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include("cmake/lely_core_libraries-extras.cmake" NO_POLICY_SCOPE) diff --git a/lely_core_libraries/patches/0001-Fix-dcf-tools.patch b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch index 53d85cb1..2fa2072b 100644 --- a/lely_core_libraries/patches/0001-Fix-dcf-tools.patch +++ b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch @@ -1,23 +1,50 @@ -From 80b4aacd880ea66151eb8cf94510e8ae7daa8371 Mon Sep 17 00:00:00 2001 +From 0d1dd06a25e6de2fa1396ca299757a45eff30846 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos -Date: Thu, 22 Jun 2023 13:52:11 +0200 +Date: Fri, 23 Jun 2023 07:14:18 +0200 Subject: [PATCH] Fix dcf-tools --- - python/dcf-tools/Makefile.am | 11 ++--------- - 1 file changed, 2 insertions(+), 9 deletions(-) + configure.ac | 10 +++------- + python/dcf-tools/Makefile.am | 21 ++++----------------- + 2 files changed, 7 insertions(+), 24 deletions(-) +diff --git a/configure.ac b/configure.ac +index 1dd9410d..c274636b 100644 +--- a/configure.ac ++++ b/configure.ac +@@ -569,19 +569,15 @@ AC_ARG_ENABLE([python], + AS_HELP_STRING([--disable-python], [disable Python tools and bindings])) + + AM_CONDITIONAL([HAVE_PYTHON2], [false]) +-AC_ARG_ENABLE([python2], +- AS_HELP_STRING([--disable-python2], [disable Python 2 tools and bindings])) +-AS_IF([test "$enable_python" == "no"], [enable_python2=no]) +-AS_IF([test "$enable_python2" != "no"], [ +- AX_CHECK_PYTHON([2], [AM_CONDITIONAL([HAVE_PYTHON2], [true])]) +-]) + + AM_CONDITIONAL([HAVE_PYTHON3], [false]) + AC_ARG_ENABLE([python3], + AS_HELP_STRING([--disable-python3], [disable Python 3 tools and bindings])) + AS_IF([test "$enable_python" == "no"], [enable_python3=no]) + AS_IF([test "$enable_python3" != "no"], [ +- AX_CHECK_PYTHON([3], [AM_CONDITIONAL([HAVE_PYTHON3], [true])]) ++ AM_PATH_PYTHON(,, [:]) ++ AC_SUBST(PYTHON3, $PYTHON) ++ AM_CONDITIONAL([HAVE_PYTHON3], [test "$PYTHON" != :]) + ]) + + AM_CONDITIONAL([NO_CYTHON], [false]) diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am -index 9852c84a..793c2804 100644 +index 9852c84a..c41aa5bd 100644 --- a/python/dcf-tools/Makefile.am +++ b/python/dcf-tools/Makefile.am -@@ -23,25 +23,18 @@ EXTRA_DIST += setup.py +@@ -23,30 +23,17 @@ EXTRA_DIST += setup.py build_base = $(realpath $(builddir))/build dist_dir = $(realpath $(builddir))/dist -all-local: python-sdist python-bdist_wheel -+all-local: python-sdist - +- clean-local: rm -rf $(build_base) $(dist_dir) $(srcdir)/*.egg-info $(builddir)/*.egg-info @@ -35,10 +62,18 @@ index 9852c84a..793c2804 100644 if HAVE_PYTHON3 @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ - install --prefix $(DESTDIR)$(prefix) --root / -+ egg_info build install --record install.log --install-scripts $(DESTDIR)$(prefix)/bin/ --install-lib $(DESTDIR)$(prefix)/dcfgen_lib/ --single-version-externally-managed +-endif +- +-.PHONY: python-sdist +-python-sdist: +-if HAVE_PYTHON3 +- @cd $(srcdir); $(PYTHON3_ENV) $(PYTHON3) setup.py \ +- sdist --dist-dir $(dist_dir) ++ egg_info build install --record install.log \ ++ --install-scripts $(DESTDIR)$(prefix)/bin/ \ ++ --install-lib $(DESTDIR)$(prefix)/lib/python$(PYTHON_VERSION)/site-packages/ \ ++ --single-version-externally-managed endif - - .PHONY: python-sdist -- 2.34.1 From 70879895959462358a4bd7dd55c6a67242d1c8e3 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Fri, 23 Jun 2023 08:14:26 +0200 Subject: [PATCH 44/59] Update changelogs Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 3 +++ canopen_402_driver/CHANGELOG.rst | 3 +++ canopen_base_driver/CHANGELOG.rst | 3 +++ canopen_core/CHANGELOG.rst | 3 +++ canopen_fake_slaves/CHANGELOG.rst | 3 +++ canopen_interfaces/CHANGELOG.rst | 3 +++ canopen_master_driver/CHANGELOG.rst | 3 +++ canopen_proxy_driver/CHANGELOG.rst | 3 +++ canopen_ros2_control/CHANGELOG.rst | 3 +++ canopen_ros2_controllers/CHANGELOG.rst | 3 +++ canopen_tests/CHANGELOG.rst | 3 +++ canopen_utils/CHANGELOG.rst | 3 +++ lely_core_libraries/CHANGELOG.rst | 5 +++++ 13 files changed, 41 insertions(+) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 7586dd26..965689d1 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index 16f03d81..595d0326 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 0773dc4b..0d8df4aa 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 42ba2ac3..97455cda 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index fc6755b6..cf651b74 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 54cf6acd..a76463e6 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 14a951af..663e2ca2 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 11bdb969..510f4484 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 564acd3b..c74c19e7 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index bef1ab05..c365756a 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 9e3c6f19..81297a55 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index cb6d9b17..c60f690f 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.4 (2023-06-22) ------------------ diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 0776c146..cbbe1c85 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Directly install lely +* Contributors: Christoph Hellmann Santos + 0.2.4 (2023-06-22) ------------------ * Add empy dependency for dcfgen From 7524a0110f1b487fe751c9963a820c6ddfbd0b3f Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Fri, 23 Jun 2023 08:14:39 +0200 Subject: [PATCH 45/59] 0.2.5 --- canopen/CHANGELOG.rst | 4 ++-- canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 4 ++-- canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 4 ++-- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 4 ++-- canopen_core/package.xml | 2 +- canopen_fake_slaves/CHANGELOG.rst | 4 ++-- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 4 ++-- canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 4 ++-- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/CHANGELOG.rst | 4 ++-- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 4 ++-- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 4 ++-- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/CHANGELOG.rst | 4 ++-- canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 4 ++-- canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- lely_core_libraries/package.xml | 2 +- 27 files changed, 40 insertions(+), 40 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 965689d1..a509ef50 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen/package.xml b/canopen/package.xml index 6493164a..b2a8ce14 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.2.4 + 0.2.5 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index 595d0326..5f6dc775 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index b98094be..406cb7d6 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.2.4 + 0.2.5 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 0d8df4aa..e17f71fb 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index 8907464d..32b0b56c 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.2.4 + 0.2.5 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 97455cda..9ba5fc99 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_core/package.xml b/canopen_core/package.xml index db7d3470..ea6a368c 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.2.4 + 0.2.5 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index cf651b74..fe578a39 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index 7120483f..2437f76f 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.2.4 + 0.2.5 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index a76463e6..c6deacf4 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index 9fa3a645..e9de0b9e 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.2.4 + 0.2.5 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 663e2ca2..5ab30682 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index 3beb7e47..bc1fa00e 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.2.4 + 0.2.5 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 510f4484..50b3ae20 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index e4503583..9fe4c81a 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.2.4 + 0.2.5 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index c74c19e7..06848a31 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index dbaabf42..dcc9d8ff 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.2.4 + 0.2.5 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index c365756a..e235040e 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index d9b27cff..2568c137 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.2.4 + 0.2.5 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 81297a55..1dd98bf3 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 6eb92811..42ccb2d2 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.2.4 + 0.2.5 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index c60f690f..afe34224 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ 0.2.4 (2023-06-22) ------------------ diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index a7029355..d2f0765e 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.2.4 + 0.2.5 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index 00ccba2f..6bc78302 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.2.4", + version="0.2.5", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index cbbe1c85..fe75ffea 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.5 (2023-06-23) +------------------ * Directly install lely * Contributors: Christoph Hellmann Santos diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index bbbea46e..a2ae6ee8 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.2.4 + 0.2.5 ROS wrapper for lely-core-libraries From 4c691ca5ddadfcebcd8888e8b79e7b6af9fc7a61 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Fri, 23 Jun 2023 08:25:24 +0200 Subject: [PATCH 46/59] Update README.md (#159) * Update README.md Signed-off-by: Christoph Hellmann Santos --- README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/README.md b/README.md index 18736c4a..509eab84 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,20 @@ The stack is currently under development and not yet ready for production use. +| Binary Package (rolling) | Jammy | Rhel9 | +|---------|-------|-------| +| canopen_interfaces | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_interfaces__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_interfaces__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_interfaces__rhel_9_x86_64__binary/) | +| lely_core_libraries | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__lely_core_libraries__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__lely_core_libraries__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__lely_core_libraries__rhel_9_x86_64__binary/) | +| canopen_core | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_core__rhel_9_x86_64__binary/) | +| canopen_master_driver | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_master_driver__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_master_driver__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_master_driver__rhel_9_x86_64__binary/) | +| canopen_base_driver | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_base_driver__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_base_driver__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_base_driver__rhel_9_x86_64__binary/) | +| canopen_proxy_driver | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_proxy_driver__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_proxy_driver__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_proxy_driver__rhel_9_x86_64__binary/) | +| canopen_402_driver | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_402_driver__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_402_driver__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_402_driver__rhel_9_x86_64__binary/) | +| canopen_ros2_control | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_ros2_control__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_ros2_control__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_control__rhel_9_x86_64__binary/) | +| canopen_ros2_control | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_ros2_controllers__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_ros2_controllers__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_ros2_controllers__rhel_9_x86_64__binary/) | +| canopen_tests | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_tests__rhel_9_x86_64__binary/) | +| canopen_utils | [![Build Status](https://build.ros2.org/job/Rbin_uJ64__canopen_utils__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uJ64__canopen_utils__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/badge/icon)](https://build.ros2.org/job/Rbin_rhel_el964__canopen_utils__rhel_9_x86_64__binary/) | + ## Documentation The documentation consists of two parts: a manual and an api reference. The documentation is built for rolling (master), iron and humble and hosted on github pages. From b6e7bbc46a2eb6137296362579978a3fb7114d4a Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sat, 24 Jun 2023 13:56:51 +0200 Subject: [PATCH 47/59] Move install from external project to cmake main Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/CMakeLists.txt | 33 +++++++------------ .../patches/0001-Fix-dcf-tools.patch | 13 ++++---- 2 files changed, 19 insertions(+), 27 deletions(-) diff --git a/lely_core_libraries/CMakeLists.txt b/lely_core_libraries/CMakeLists.txt index 88d9f000..9489bde8 100644 --- a/lely_core_libraries/CMakeLists.txt +++ b/lely_core_libraries/CMakeLists.txt @@ -9,34 +9,25 @@ ExternalProject_Add(upstr_lely_core_libraries # Name for custom target #--Download step-------------- SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/upstream INSTALL_DIR "${CMAKE_INSTALL_PREFIX}" # Installation prefix + BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/build GIT_REPOSITORY https://gitlab.com/lely_industries/lely-core.git GIT_TAG 7824cbb2ac08d091c4fa2fb397669b938de9e3f5 TIMEOUT 60 - #--Update/Patch step---------- + #UPDATE step apply patch to fix dcf-tools install UPDATE_COMMAND COMMAND git reset --hard COMMAND git apply --whitespace=fix --reject ${CMAKE_CURRENT_SOURCE_DIR}/patches/0001-Fix-dcf-tools.patch - #--Configure step------------- - CONFIGURE_COMMAND autoreconf -i COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag - #--Build step----------------- - BUILD_IN_SOURCE 1 # Use source dir for build dir - #--Install step--------------- - BUILD_COMMAND cd COMMAND $(MAKE) + #CONFIGURE step execute autoreconf and configure + CONFIGURE_COMMAND autoreconf -i + COMMAND /configure --prefix= --disable-cython --disable-doc --disable-tests --disable-static --disable-diag + #BUILD STEP execute make + BUILD_COMMAND $(MAKE) -C ${CMAKE_CURRENT_BINARY_DIR}/build + #INSTALL STEP do nothing as we install in main + INSTALL_COMMAND "" ) -# message(STATUS "PREFIX_PATH: ${CMAKE_PREFIX_PATH}") -# message(STATUS "BUILD_DIR: ${CMAKE_CURRENT_BINARY_DIR}") -# message(STATUS "INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}") -# install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib DESTINATION ${CMAKE_INSTALL_PREFIX}) -# install(DIRECTORY -# ${CMAKE_CURRENT_BINARY_DIR}/dcfgen_lib/ -# DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/${python_version}/site-packages -# PATTERN "dcfgen_lib/*" -# ) -# install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/include DESTINATION ${CMAKE_INSTALL_PREFIX}) -# install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin/ DESTINATION ${CMAKE_INSTALL_PREFIX}/bin -# PATTERN "bin/*" -# PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ -# GROUP_EXECUTE GROUP_READ WORLD_READ WORLD_EXECUTE) + +#INSTALL lely_core_libraries - execute make install +install(CODE "execute_process(COMMAND ${CMAKE_MAKE_PROGRAM} install VERBOSE=1 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/build)") set(lely_core_cmake_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include("cmake/lely_core_libraries-extras.cmake" NO_POLICY_SCOPE) diff --git a/lely_core_libraries/patches/0001-Fix-dcf-tools.patch b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch index 2fa2072b..78e83f1e 100644 --- a/lely_core_libraries/patches/0001-Fix-dcf-tools.patch +++ b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch @@ -1,12 +1,12 @@ -From 0d1dd06a25e6de2fa1396ca299757a45eff30846 Mon Sep 17 00:00:00 2001 +From 62b4b4fcd1f1491f524249471b5830cd760e11ac Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos -Date: Fri, 23 Jun 2023 07:14:18 +0200 +Date: Sat, 24 Jun 2023 13:42:45 +0200 Subject: [PATCH] Fix dcf-tools --- configure.ac | 10 +++------- - python/dcf-tools/Makefile.am | 21 ++++----------------- - 2 files changed, 7 insertions(+), 24 deletions(-) + python/dcf-tools/Makefile.am | 23 +++++------------------ + 2 files changed, 8 insertions(+), 25 deletions(-) diff --git a/configure.ac b/configure.ac index 1dd9410d..c274636b 100644 @@ -36,7 +36,7 @@ index 1dd9410d..c274636b 100644 AM_CONDITIONAL([NO_CYTHON], [false]) diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am -index 9852c84a..c41aa5bd 100644 +index 9852c84a..6db4d232 100644 --- a/python/dcf-tools/Makefile.am +++ b/python/dcf-tools/Makefile.am @@ -23,30 +23,17 @@ EXTRA_DIST += setup.py @@ -60,7 +60,7 @@ index 9852c84a..c41aa5bd 100644 .PHONY: python-install python-install: if HAVE_PYTHON3 - @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ +- @$(PYTHON3_ENV) $(PYTHON3) $(srcdir)/setup.py \ - install --prefix $(DESTDIR)$(prefix) --root / -endif - @@ -69,6 +69,7 @@ index 9852c84a..c41aa5bd 100644 -if HAVE_PYTHON3 - @cd $(srcdir); $(PYTHON3_ENV) $(PYTHON3) setup.py \ - sdist --dist-dir $(dist_dir) ++ @cd $(srcdir) && $(PYTHON3_ENV) $(PYTHON3) setup.py \ + egg_info build install --record install.log \ + --install-scripts $(DESTDIR)$(prefix)/bin/ \ + --install-lib $(DESTDIR)$(prefix)/lib/python$(PYTHON_VERSION)/site-packages/ \ From 98db4b0120c9a31863d985051e1a6b398d73efc4 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sat, 24 Jun 2023 13:58:59 +0200 Subject: [PATCH 48/59] Run prerelease on tag push Signed-off-by: Christoph Hellmann Santos --- .github/workflows/prerelease-rolling.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/prerelease-rolling.yml b/.github/workflows/prerelease-rolling.yml index bac8bac8..27b286ea 100644 --- a/.github/workflows/prerelease-rolling.yml +++ b/.github/workflows/prerelease-rolling.yml @@ -2,6 +2,9 @@ name: Prerelease-Test Rolling on: workflow_dispatch: + push: + tags: + - "*" jobs: industrial_ci: From 121109a10950bd5c9d2e8a5a10fc3473e68864a2 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sat, 24 Jun 2023 14:13:45 +0200 Subject: [PATCH 49/59] Update changelogs Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 3 +++ canopen_402_driver/CHANGELOG.rst | 3 +++ canopen_base_driver/CHANGELOG.rst | 3 +++ canopen_core/CHANGELOG.rst | 3 +++ canopen_fake_slaves/CHANGELOG.rst | 3 +++ canopen_interfaces/CHANGELOG.rst | 3 +++ canopen_master_driver/CHANGELOG.rst | 3 +++ canopen_proxy_driver/CHANGELOG.rst | 3 +++ canopen_ros2_control/CHANGELOG.rst | 3 +++ canopen_ros2_controllers/CHANGELOG.rst | 3 +++ canopen_tests/CHANGELOG.rst | 3 +++ canopen_utils/CHANGELOG.rst | 3 +++ lely_core_libraries/CHANGELOG.rst | 5 +++++ 13 files changed, 41 insertions(+) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index a509ef50..7273488e 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index 5f6dc775..e2b80585 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index e17f71fb..018234c2 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 9ba5fc99..9b6fbef7 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index fe578a39..bb5488dc 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index c6deacf4..ea3d756a 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 5ab30682..b31ab396 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 50b3ae20..9f45a6b2 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 06848a31..ce509ac0 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index e235040e..bc8af44b 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 1dd98bf3..13486d38 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index afe34224..73e4aff2 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.5 (2023-06-23) ------------------ diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index fe75ffea..8f32d5d2 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Move install from external project to cmake main +* Contributors: Christoph Hellmann Santos + 0.2.5 (2023-06-23) ------------------ * Directly install lely From 84c732bd0036ed4b4a4b5751a4a802ca865cc8ef Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Sat, 24 Jun 2023 14:13:59 +0200 Subject: [PATCH 50/59] 0.2.6 --- canopen/CHANGELOG.rst | 4 ++-- canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 4 ++-- canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 4 ++-- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 4 ++-- canopen_core/package.xml | 2 +- canopen_fake_slaves/CHANGELOG.rst | 4 ++-- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 4 ++-- canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 4 ++-- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/CHANGELOG.rst | 4 ++-- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 4 ++-- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 4 ++-- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/CHANGELOG.rst | 4 ++-- canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 4 ++-- canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- lely_core_libraries/package.xml | 2 +- 27 files changed, 40 insertions(+), 40 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 7273488e..121b55ce 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen/package.xml b/canopen/package.xml index b2a8ce14..abc77f80 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.2.5 + 0.2.6 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index e2b80585..8f11b78d 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index 406cb7d6..88138732 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.2.5 + 0.2.6 Driiver for devices implementing CIA402 profile christoph LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 018234c2..a0c3f546 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index 32b0b56c..fc3c80f7 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.2.5 + 0.2.6 Library containing abstract CANopen driver class for ros2_canopen christoph Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 9b6fbef7..80a9b4c4 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_core/package.xml b/canopen_core/package.xml index ea6a368c..ba4f4ace 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.2.5 + 0.2.6 Core ros2_canopen functionalities such as DeviceContainer and master christoph Apache-2.0 diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index bb5488dc..8fa56c74 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index 2437f76f..0588e7c4 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.2.5 + 0.2.6 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index ea3d756a..72a0f679 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index e9de0b9e..de3221d6 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.2.5 + 0.2.6 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index b31ab396..5aec5b3d 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index bc1fa00e..e68214db 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.2.5 + 0.2.6 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 9f45a6b2..d7953f3b 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index 9fe4c81a..7e84d678 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.2.5 + 0.2.6 Simple proxy driver for the ros2_canopen stack christoph Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index ce509ac0..37c8fbde 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index dcc9d8ff..18d8038f 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.2.5 + 0.2.6 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index bc8af44b..ce7448fc 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index 2568c137..c010a79f 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.2.5 + 0.2.6 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 13486d38..14f93883 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 42ccb2d2..71ead850 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.2.5 + 0.2.6 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 73e4aff2..b1b6e49c 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ 0.2.5 (2023-06-23) ------------------ diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index d2f0765e..776fa019 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.2.5 + 0.2.6 Utils for working with ros2_canopen. christoph Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index 6bc78302..060b809d 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.2.5", + version="0.2.6", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 8f32d5d2..567b8c2f 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.6 (2023-06-24) +------------------ * Move install from external project to cmake main * Contributors: Christoph Hellmann Santos diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index a2ae6ee8..f52f4b94 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.2.5 + 0.2.6 ROS wrapper for lely-core-libraries From 4ef76cb7af79c0c37736de3adec7405ad06ccdbb Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Mon, 26 Jun 2023 10:21:43 +0200 Subject: [PATCH 51/59] Merge branch 'yos627-patch-1' --- canopen_base_driver/src/lely_driver_bridge.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/canopen_base_driver/src/lely_driver_bridge.cpp b/canopen_base_driver/src/lely_driver_bridge.cpp index 2da1b2aa..3e292303 100644 --- a/canopen_base_driver/src/lely_driver_bridge.cpp +++ b/canopen_base_driver/src/lely_driver_bridge.cpp @@ -379,7 +379,7 @@ void LelyDriverBridge::tpdo_transmit(COData data) tpdo_mapped[data.index_][data.subindex_].WriteEvent(); std::cout << "async_pdo_write: id=" << (unsigned int)get_id() << " index=0x" << std::hex << (unsigned int)data.index_ << " subindex=" << (unsigned int)data.subindex_ - << (uint32_t)data.data_ << std::endl; + << " data:" << (uint32_t)data.data_ << std::endl; } catch (lely::canopen::SdoError & e) { From 6f994189abf38c70e4027c9ec36fd718a3f76fa1 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Tue, 27 Jun 2023 10:30:54 +0200 Subject: [PATCH 52/59] Fix python versions (#164) Signed-off-by: Christoph Hellmann Santos --- lely_core_libraries/patches/0001-Fix-dcf-tools.patch | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/lely_core_libraries/patches/0001-Fix-dcf-tools.patch b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch index 78e83f1e..0a5d72ca 100644 --- a/lely_core_libraries/patches/0001-Fix-dcf-tools.patch +++ b/lely_core_libraries/patches/0001-Fix-dcf-tools.patch @@ -1,6 +1,6 @@ -From 62b4b4fcd1f1491f524249471b5830cd760e11ac Mon Sep 17 00:00:00 2001 +From 1fcc088cdf64bee181f1141077af01267cc224ad Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos -Date: Sat, 24 Jun 2023 13:42:45 +0200 +Date: Tue, 27 Jun 2023 10:15:06 +0200 Subject: [PATCH] Fix dcf-tools --- @@ -29,14 +29,14 @@ index 1dd9410d..c274636b 100644 AS_IF([test "$enable_python" == "no"], [enable_python3=no]) AS_IF([test "$enable_python3" != "no"], [ - AX_CHECK_PYTHON([3], [AM_CONDITIONAL([HAVE_PYTHON3], [true])]) -+ AM_PATH_PYTHON(,, [:]) -+ AC_SUBST(PYTHON3, $PYTHON) ++ AM_PATH_PYTHON([3.3],, [:]) ++ AC_SUBST(PYTHON3, "$PYTHON") + AM_CONDITIONAL([HAVE_PYTHON3], [test "$PYTHON" != :]) ]) AM_CONDITIONAL([NO_CYTHON], [false]) diff --git a/python/dcf-tools/Makefile.am b/python/dcf-tools/Makefile.am -index 9852c84a..6db4d232 100644 +index 9852c84a..dd5cdfa0 100644 --- a/python/dcf-tools/Makefile.am +++ b/python/dcf-tools/Makefile.am @@ -23,30 +23,17 @@ EXTRA_DIST += setup.py From ef394e2b17c4e09123a93bd42d4db4c889dee990 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Tue, 27 Jun 2023 10:31:07 +0200 Subject: [PATCH 53/59] Fix maintainer naming (#163) Signed-off-by: Christoph Hellmann Santos --- canopen_402_driver/package.xml | 2 +- canopen_base_driver/package.xml | 2 +- canopen_core/package.xml | 2 +- canopen_proxy_driver/package.xml | 2 +- canopen_utils/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index 88138732..3a9253f2 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -4,7 +4,7 @@ canopen_402_driver 0.2.6 Driiver for devices implementing CIA402 profile - christoph + Christoph Hellmann Santos LGPL-v3 ament_cmake_ros diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index fc3c80f7..ebf88aca 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -4,7 +4,7 @@ canopen_base_driver 0.2.6 Library containing abstract CANopen driver class for ros2_canopen - christoph + Christoph Hellmann Santos Apache-2.0 ament_cmake_ros diff --git a/canopen_core/package.xml b/canopen_core/package.xml index ba4f4ace..d04b198d 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -4,7 +4,7 @@ canopen_core 0.2.6 Core ros2_canopen functionalities such as DeviceContainer and master - christoph + Christoph Hellmann Santos Apache-2.0 ament_cmake diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index 7e84d678..e2504971 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -4,7 +4,7 @@ canopen_proxy_driver 0.2.6 Simple proxy driver for the ros2_canopen stack - christoph + Christoph Hellmann Santos Apache-2.0 ament_cmake_ros diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index 776fa019..5f1d1640 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -4,7 +4,7 @@ canopen_utils 0.2.6 Utils for working with ros2_canopen. - christoph + Christoph Hellmann Santos Apache-2.0 rclpy canopen_interfaces From 543cc2ab5909b5ab4a6591b4958a349f800b9f52 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Tue, 27 Jun 2023 13:14:27 +0200 Subject: [PATCH 54/59] Add missing license headers and activate ament_copyright (#165) * Add missing license headers Signed-off-by: Christoph Hellmann Santos * Add fix ament_lint_cmake Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos --- .pre-commit-config.yaml | 61 ++++--------------- canopen/sphinx/conf.py | 14 +++++ canopen_402_driver/CMakeLists.txt | 4 +- .../include/canopen_402_driver/base.hpp | 16 +++++ .../canopen_402_driver/cia402_driver.hpp | 16 +++++ .../include/canopen_402_driver/command.hpp | 17 ++++++ .../default_homing_mode.hpp | 17 ++++++ .../canopen_402_driver/homing_mode.hpp | 17 ++++++ .../lifecycle_cia402_driver.hpp | 16 +++++ .../include/canopen_402_driver/mode.hpp | 17 ++++++ .../mode_forward_helper.hpp | 17 ++++++ .../canopen_402_driver/mode_target_helper.hpp | 17 ++++++ .../include/canopen_402_driver/motor.hpp | 17 ++++++ .../node_canopen_402_driver.hpp | 18 ++++++ .../node_canopen_402_driver_impl.hpp | 18 ++++++ .../profiled_position_mode.hpp | 17 ++++++ .../include/canopen_402_driver/state.hpp | 17 ++++++ .../canopen_402_driver/visibility_control.h | 15 +++++ .../canopen_402_driver/word_accessor.hpp | 17 ++++++ canopen_402_driver/src/cia402_driver.cpp | 16 +++++ canopen_402_driver/src/command.cpp | 16 +++++ .../src/default_homing_mode.cpp | 16 +++++ .../src/lifecycle_cia402_driver.cpp | 16 +++++ canopen_402_driver/src/motor.cpp | 18 ++++++ .../node_canopen_402_driver.cpp | 16 +++++ canopen_402_driver/src/state.cpp | 16 +++++ canopen_402_driver/test/CMakeLists.txt | 2 +- .../test/test_driver_component.cpp | 15 +++++ canopen_base_driver/CMakeLists.txt | 4 +- .../diagnostic_collector.hpp | 14 +++++ canopen_base_driver/test/CMakeLists.txt | 8 +-- canopen_core/CMakeLists.txt | 10 +-- canopen_core/ConfigExtras.cmake | 14 +++++ canopen_core/test/CMakeLists.txt | 20 +++--- .../canopen_fake_slaves/motion_generator.hpp | 14 +++++ canopen_master_driver/CMakeLists.txt | 4 +- .../visibility_control.h | 14 +++++ canopen_master_driver/test/CMakeLists.txt | 8 +-- canopen_proxy_driver/CMakeLists.txt | 4 +- canopen_proxy_driver/test/CMakeLists.txt | 6 +- .../launch/cia402_diagnostics_setup.launch.py | 14 +++++ .../launch/proxy_diagnostics_setup.launch.py | 14 +++++ .../canopen_utils/launch_test_node.py | 14 +++++ canopen_utils/setup.py | 14 +++++ .../cmake/lely_core_libraries-extras.cmake | 19 ++++-- 45 files changed, 585 insertions(+), 89 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 99f8e46f..28a165cd 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -45,19 +45,6 @@ repos: - id: black args: ["--line-length=99"] - # PEP 257 - # - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - # rev: v0.3.3 - # hooks: - # - id: pep257 - # args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - - # - repo: https://github.com/pycqa/flake8 - # rev: 5.0.4 - # hooks: - # - id: flake8 - # args: ["--ignore=E501"] - # CPP hooks - repo: local hooks: @@ -68,36 +55,8 @@ repos: language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] - # The same options as in ament_cppcheck are used, but its not working... - #- repo: https://github.com/pocc/pre-commit-hooks - #rev: v1.1.1 - #hooks: - #- id: cppcheck - #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] - # - repo: local - # hooks: - # - id: ament_cppcheck - # name: ament_cppcheck - # description: Static code analysis of C/C++ files. - # stages: [commit] - # entry: ament_cppcheck - # language: system - # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - - # # Maybe use https://github.com/cpplint/cpplint instead - # - repo: local - # hooks: - # - id: ament_cpplint - # name: ament_cpplint - # description: Static code analysis of C/C++ files. - # stages: [commit] - # entry: ament_cpplint - # language: system - # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # args: ["--linelength=100", "--filter=-whitespace/newline"] - - # # Cmake hooks + # Cmake hooks # - repo: local # hooks: # - id: ament_lint_cmake @@ -107,16 +66,18 @@ repos: # entry: ament_lint_cmake # language: system # files: CMakeLists\.txt$ + # exclude: lely_core_libraries\/CMakeLists\.txt$ # # Copyright - # - repo: local - # hooks: - # - id: ament_copyright - # name: ament_copyright - # description: Check if copyright notice is available in all files. - # stages: [commit] - # entry: ament_copyright - # language: system + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + args: ["--add-missing", "ROS-Industrial", "apache2"] + language: system # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 diff --git a/canopen/sphinx/conf.py b/canopen/sphinx/conf.py index a0137f98..a0f31d18 100644 --- a/canopen/sphinx/conf.py +++ b/canopen/sphinx/conf.py @@ -1,3 +1,17 @@ +# Copyright 2023 ROS-Industrial +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + # Configuration file for the Sphinx documentation builder. # # This file only contains a selection of the most common options. For a full diff --git a/canopen_402_driver/CMakeLists.txt b/canopen_402_driver/CMakeLists.txt index 901dc436..3ec6e208 100644 --- a/canopen_402_driver/CMakeLists.txt +++ b/canopen_402_driver/CMakeLists.txt @@ -67,7 +67,7 @@ ament_target_dependencies( add_library(lifecycle_cia402_driver src/lifecycle_cia402_driver.cpp - ) +) target_compile_features(lifecycle_cia402_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_cia402_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_cia402_driver PUBLIC @@ -94,7 +94,7 @@ set(node_plugins "${node_plugins}ros2_canopen::LifecycleCia402Driver;$. +// #ifndef CANOPEN_402_BASE_H #define CANOPEN_402_BASE_H #include diff --git a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp index 5ad3e992..d54eecc8 100644 --- a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef CANOPEN_402_DRIVER__402_DRIVER_HPP_ #define CANOPEN_402_DRIVER__402_DRIVER_HPP_ #include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" diff --git a/canopen_402_driver/include/canopen_402_driver/command.hpp b/canopen_402_driver/include/canopen_402_driver/command.hpp index 855399ea..45a26599 100644 --- a/canopen_402_driver/include/canopen_402_driver/command.hpp +++ b/canopen_402_driver/include/canopen_402_driver/command.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef CANOPEN_402_DRIVER_COMMAND_HPP #define CANOPEN_402_DRIVER_COMMAND_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp b/canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp index 079d50af..6af8afe3 100644 --- a/canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp +++ b/canopen_402_driver/include/canopen_402_driver/default_homing_mode.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef DEFAULT_HOMING_MODE_HPP #define DEFAULT_HOMING_MODE_HPP #include diff --git a/canopen_402_driver/include/canopen_402_driver/homing_mode.hpp b/canopen_402_driver/include/canopen_402_driver/homing_mode.hpp index 31103426..4fc10439 100644 --- a/canopen_402_driver/include/canopen_402_driver/homing_mode.hpp +++ b/canopen_402_driver/include/canopen_402_driver/homing_mode.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef HOMING_MODE_HPP #define HOMING_MODE_HPP #include "base.hpp" diff --git a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp index e9d7f27d..2ccef5ca 100644 --- a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef CANOPEN_402_DRIVER__CANOPEN_LIFECYCLE_402_DRIVER_HPP_ #define CANOPEN_402_DRIVER__CANOPEN_LIFECYCLE_402_DRIVER_HPP_ diff --git a/canopen_402_driver/include/canopen_402_driver/mode.hpp b/canopen_402_driver/include/canopen_402_driver/mode.hpp index e7ccc032..1bf9e168 100644 --- a/canopen_402_driver/include/canopen_402_driver/mode.hpp +++ b/canopen_402_driver/include/canopen_402_driver/mode.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef CANOPEN_402_DRIVER_MODE_HPP #define CANOPEN_402_DRIVER_MODE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp b/canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp index 216b091b..d272afd9 100644 --- a/canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp +++ b/canopen_402_driver/include/canopen_402_driver/mode_forward_helper.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef MODE_FORWARD_HELPER_HPP #define MODE_FORWARD_HELPER_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp b/canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp index 1e08057f..424881bd 100644 --- a/canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp +++ b/canopen_402_driver/include/canopen_402_driver/mode_target_helper.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef MODE_TARGET_HELPER_HPP #define MODE_TARGET_HELPER_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/motor.hpp b/canopen_402_driver/include/canopen_402_driver/motor.hpp index a3d8537c..cf040f97 100644 --- a/canopen_402_driver/include/canopen_402_driver/motor.hpp +++ b/canopen_402_driver/include/canopen_402_driver/motor.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef MOTOR_HPP #define MOTOR_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp index 9142f825..1e38588e 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp @@ -1,3 +1,21 @@ +// Copyright 2023 Christoph Hellmann Santos +// Vishnuprasad Prachandabhanu +// Lovro Ivanov +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef NODE_CANOPEN_402_DRIVER #define NODE_CANOPEN_402_DRIVER diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index a96a07c7..48cc5e1b 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -1,3 +1,21 @@ +// Copyright 2023 Christoph Hellmann Santos +// Vishnuprasad Prachandabhanu +// Lovro Ivanov +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef NODE_CANOPEN_402_DRIVER_IMPL_HPP_ #define NODE_CANOPEN_402_DRIVER_IMPL_HPP_ diff --git a/canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp b/canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp index ca56eaa8..a664cb89 100644 --- a/canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp +++ b/canopen_402_driver/include/canopen_402_driver/profiled_position_mode.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef PROFILED_POSITION_MODE_HPP #define PROFILED_POSITION_MODE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/state.hpp b/canopen_402_driver/include/canopen_402_driver/state.hpp index 4a146118..afe59f14 100644 --- a/canopen_402_driver/include/canopen_402_driver/state.hpp +++ b/canopen_402_driver/include/canopen_402_driver/state.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef CANOPEN_402_DRIVER_STATE_HPP #define CANOPEN_402_DRIVER_STATE_HPP diff --git a/canopen_402_driver/include/canopen_402_driver/visibility_control.h b/canopen_402_driver/include/canopen_402_driver/visibility_control.h index 231c9eea..97ff1470 100644 --- a/canopen_402_driver/include/canopen_402_driver/visibility_control.h +++ b/canopen_402_driver/include/canopen_402_driver/visibility_control.h @@ -1,3 +1,18 @@ +// Copyright 2023 Christoph Hellmann Santos +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// #ifndef CANOPEN_402_DRIVER__VISIBILITY_CONTROL_H_ #define CANOPEN_402_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/canopen_402_driver/include/canopen_402_driver/word_accessor.hpp b/canopen_402_driver/include/canopen_402_driver/word_accessor.hpp index 34e54914..aff300df 100644 --- a/canopen_402_driver/include/canopen_402_driver/word_accessor.hpp +++ b/canopen_402_driver/include/canopen_402_driver/word_accessor.hpp @@ -1,3 +1,20 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #ifndef WORD_ACCESSOR_HPP #define WORD_ACCESSOR_HPP diff --git a/canopen_402_driver/src/cia402_driver.cpp b/canopen_402_driver/src/cia402_driver.cpp index 2d07f9ea..42eabfa1 100644 --- a/canopen_402_driver/src/cia402_driver.cpp +++ b/canopen_402_driver/src/cia402_driver.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #include "canopen_402_driver/cia402_driver.hpp" using namespace ros2_canopen; diff --git a/canopen_402_driver/src/command.cpp b/canopen_402_driver/src/command.cpp index 7bb13169..380eba2f 100644 --- a/canopen_402_driver/src/command.cpp +++ b/canopen_402_driver/src/command.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// #include "canopen_402_driver/command.hpp" using namespace ros2_canopen; diff --git a/canopen_402_driver/src/default_homing_mode.cpp b/canopen_402_driver/src/default_homing_mode.cpp index 4f8de6f8..ffbf5ed9 100644 --- a/canopen_402_driver/src/default_homing_mode.cpp +++ b/canopen_402_driver/src/default_homing_mode.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// #include "canopen_402_driver/default_homing_mode.hpp" using namespace ros2_canopen; diff --git a/canopen_402_driver/src/lifecycle_cia402_driver.cpp b/canopen_402_driver/src/lifecycle_cia402_driver.cpp index 56b2c02e..25be462e 100644 --- a/canopen_402_driver/src/lifecycle_cia402_driver.cpp +++ b/canopen_402_driver/src/lifecycle_cia402_driver.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #include "canopen_402_driver/lifecycle_cia402_driver.hpp" using namespace ros2_canopen; diff --git a/canopen_402_driver/src/motor.cpp b/canopen_402_driver/src/motor.cpp index ecddda93..28adfa02 100644 --- a/canopen_402_driver/src/motor.cpp +++ b/canopen_402_driver/src/motor.cpp @@ -1,3 +1,21 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2023 Vishnuprasad Prachandabhanu +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #include "canopen_402_driver/motor.hpp" using namespace ros2_canopen; diff --git a/canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp b/canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp index c1ccc77a..a36eade1 100644 --- a/canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp +++ b/canopen_402_driver/src/node_interfaces/node_canopen_402_driver.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + #include "canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp" #include "canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp" #include "canopen_core/driver_error.hpp" diff --git a/canopen_402_driver/src/state.cpp b/canopen_402_driver/src/state.cpp index ce3f6d60..80da38fc 100644 --- a/canopen_402_driver/src/state.cpp +++ b/canopen_402_driver/src/state.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 Christoph Hellmann Santos +// Copyright 2014-2022 Authors of ros_canopen +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// #include "canopen_402_driver/state.hpp" #include using namespace ros2_canopen; diff --git a/canopen_402_driver/test/CMakeLists.txt b/canopen_402_driver/test/CMakeLists.txt index 046f3ffc..69932041 100644 --- a/canopen_402_driver/test/CMakeLists.txt +++ b/canopen_402_driver/test/CMakeLists.txt @@ -6,4 +6,4 @@ ament_target_dependencies(test_driver_component ) target_include_directories(test_driver_component PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) diff --git a/canopen_402_driver/test/test_driver_component.cpp b/canopen_402_driver/test/test_driver_component.cpp index dc2ebb73..5f2226a6 100644 --- a/canopen_402_driver/test/test_driver_component.cpp +++ b/canopen_402_driver/test/test_driver_component.cpp @@ -1,3 +1,18 @@ +// Copyright 2023 Christoph Hellmann Santos +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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 General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// #include #include #include diff --git a/canopen_base_driver/CMakeLists.txt b/canopen_base_driver/CMakeLists.txt index 76f5fc49..47c26e03 100644 --- a/canopen_base_driver/CMakeLists.txt +++ b/canopen_base_driver/CMakeLists.txt @@ -67,7 +67,7 @@ message(STATUS "node_canopen_base_driver: ${lely_core_libraries_LIBRARIES}") add_library(lifecycle_base_driver src/lifecycle_base_driver.cpp - ) +) target_compile_features(lifecycle_base_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_base_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_base_driver PUBLIC @@ -94,7 +94,7 @@ set(node_plugins "${node_plugins}ros2_canopen::LifecycleBaseDriver;$ $ - ) +) ament_target_dependencies(node_canopen_driver rclcpp rclcpp_lifecycle @@ -72,7 +72,7 @@ target_compile_options(node_canopen_master PUBLIC -fPIC -Wl,--no-undefined) target_include_directories(node_canopen_master PUBLIC $ $ - ) +) ament_target_dependencies(node_canopen_master rclcpp rclcpp_lifecycle @@ -95,7 +95,7 @@ target_compile_options(device_container PUBLIC -fPIC -Wl,--no-undefined) target_include_directories(device_container PUBLIC $ $ - ) +) ament_target_dependencies(device_container rclcpp rclcpp_lifecycle @@ -111,7 +111,7 @@ target_link_libraries(device_container add_executable(device_container_node src/device_container_node.cpp - ) +) target_link_libraries(device_container_node device_container node_canopen_master diff --git a/canopen_core/ConfigExtras.cmake b/canopen_core/ConfigExtras.cmake index 01e6b9a1..8f7786db 100644 --- a/canopen_core/ConfigExtras.cmake +++ b/canopen_core/ConfigExtras.cmake @@ -1 +1,15 @@ +# Copyright (c) 2023 Vishnuprasad Prachandabhanu +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# find_package(Boost REQUIRED system thread) diff --git a/canopen_core/test/CMakeLists.txt b/canopen_core/test/CMakeLists.txt index 65b35a4b..8ddc2a6d 100644 --- a/canopen_core/test/CMakeLists.txt +++ b/canopen_core/test/CMakeLists.txt @@ -10,7 +10,7 @@ ament_target_dependencies(test_node_canopen_driver ) target_include_directories(test_node_canopen_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_node_canopen_driver node_canopen_driver ) @@ -27,7 +27,7 @@ ament_target_dependencies(test_node_canopen_master ) target_include_directories(test_node_canopen_master PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_node_canopen_master node_canopen_master ) @@ -42,14 +42,14 @@ ament_target_dependencies(test_device_container ) target_include_directories(test_device_container PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_device_container node_canopen_master node_canopen_driver device_container ) endif() -FILE(COPY bus_configs DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) +file(COPY bus_configs DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) ament_add_gmock(test_canopen_driver test_canopen_driver.cpp @@ -59,7 +59,7 @@ ament_target_dependencies(test_canopen_driver ) target_include_directories(test_canopen_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_canopen_driver node_canopen_driver ) @@ -72,7 +72,7 @@ ament_target_dependencies(test_lifecycle_canopen_driver ) target_include_directories(test_lifecycle_canopen_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_lifecycle_canopen_driver node_canopen_driver ) @@ -85,7 +85,7 @@ ament_target_dependencies(test_canopen_master ) target_include_directories(test_canopen_master PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_canopen_master node_canopen_master ) @@ -98,7 +98,7 @@ ament_target_dependencies(test_lifecycle_canopen_master ) target_include_directories(test_lifecycle_canopen_master PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_lifecycle_canopen_master node_canopen_master ) @@ -111,7 +111,7 @@ ament_target_dependencies(test_lifecycle_manager ) target_include_directories(test_lifecycle_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_lifecycle_manager device_container ) @@ -124,7 +124,7 @@ ament_target_dependencies(test_errors ) target_include_directories(test_errors PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/ - ) +) target_link_libraries(test_errors device_container node_canopen_master diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp index ac000bb7..e610d3bb 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/motion_generator.hpp @@ -1,3 +1,17 @@ +// Copyright 2023 ROS-Industrial +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef __MOTIONGENERATOR_H__ #define __MOTIONGENERATOR_H__ diff --git a/canopen_master_driver/CMakeLists.txt b/canopen_master_driver/CMakeLists.txt index 9beffec6..4da167c4 100644 --- a/canopen_master_driver/CMakeLists.txt +++ b/canopen_master_driver/CMakeLists.txt @@ -62,7 +62,7 @@ ament_target_dependencies( add_library(lifecycle_master_driver src/lifecycle_master_driver.cpp - ) +) target_compile_features(lifecycle_master_driver PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_options(lifecycle_master_driver PUBLIC -Wl,--no-undefined) target_include_directories(lifecycle_master_driver PUBLIC @@ -90,7 +90,7 @@ set(node_plugins "${node_plugins}ros2_canopen::LifecycleMasterDriver;$ Date: Thu, 29 Jun 2023 18:24:56 +0200 Subject: [PATCH 55/59] [ros2controllers] Correct Proxy controller after changes and update tests. (#148) * Correct Proxy controller after changes and update docs * Update codespellignore Signed-off-by: Christoph Hellmann Santos * Fix tests Signed-off-by: Christoph Hellmann Santos * Fix owns/ons error in tests Signed-off-by: Christoph Hellmann Santos * Fixup tests. --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Christoph Hellmann Santos --- .codespellignore | 1 + .pre-commit-config.yaml | 2 +- .../ros2_control_system-test.rst | 13 +++++---- canopen_ros2_control/src/canopen_system.cpp | 2 +- .../src/canopen_proxy_controller.cpp | 27 +++++++------------ .../test/test_canopen_proxy_controller.cpp | 8 +++--- .../test/test_canopen_proxy_controller.hpp | 8 +++--- 7 files changed, 27 insertions(+), 34 deletions(-) create mode 100644 .codespellignore diff --git a/.codespellignore b/.codespellignore new file mode 100644 index 00000000..2fd0dac6 --- /dev/null +++ b/.codespellignore @@ -0,0 +1 @@ +ons diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 28a165cd..2e301602 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -101,5 +101,5 @@ repos: rev: v2.2.1 hooks: - id: codespell - args: ['--write-changes'] + args: ['--write-changes', '-I', '.codespellignore'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio|dcf|eds)$ diff --git a/canopen/sphinx/software-tests/ros2_control_system-test.rst b/canopen/sphinx/software-tests/ros2_control_system-test.rst index 1809dd1b..c9e2735e 100644 --- a/canopen/sphinx/software-tests/ros2_control_system-test.rst +++ b/canopen/sphinx/software-tests/ros2_control_system-test.rst @@ -8,7 +8,7 @@ Test details :header: "Detail", "Information" :delim: ; - Package; canopen_ros2_control + Package; canopen_tests Test file; launch/canopen_system.launch.py Description; Create an exemplary ros2_control SystemInterface with CAN master and communicates to a slave node. Prerequisites; vcan0 must be available @@ -17,7 +17,7 @@ Test details Explanation of the test ------------------------ -The test is starting generic system interface and generic controller for CanOpen devices. +The test starts generic system interface and generic proxy controller for CanOpen devices. Generic system interface enables integration of values from the CAN Bus into ros2_control framework and the controller enables you to send and receive data from the CAN bus through ros2_control to ROS2. The next few lines show you some command to have exemplary usage of the ros2_control integration: @@ -40,10 +40,9 @@ The next few lines show you some command to have exemplary usage of the ros2_con .. code-block:: bash - ros2 topic pub -r 100 node_1_controller/rpdo canopen_interfaces/msg/COData " - index: 25 - subindex: 35 - data: 238 - type: 8" + ros2 topic pub --once /node_1_controller/tpdo canopen_interfaces/msg/COData " + index: 25 + subindex: 35 + data: 238" Now watch how data in the first two opened terminals are changing. diff --git a/canopen_ros2_control/src/canopen_system.cpp b/canopen_ros2_control/src/canopen_system.cpp index 99e9f153..268d709c 100644 --- a/canopen_ros2_control/src/canopen_system.cpp +++ b/canopen_ros2_control/src/canopen_system.cpp @@ -207,7 +207,7 @@ std::vector CanopenSystem::export_command_ info_.joints[i].name, "tpdo/data", &canopen_data_[node_id].tpdo_data.data)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, "tpdo/owns", &canopen_data_[node_id].tpdo_data.one_shot)); + info_.joints[i].name, "tpdo/ons", &canopen_data_[node_id].tpdo_data.one_shot)); command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, "nmt/reset", &canopen_data_[node_id].nmt_state.reset_ons)); diff --git a/canopen_ros2_controllers/src/canopen_proxy_controller.cpp b/canopen_ros2_controllers/src/canopen_proxy_controller.cpp index 2ac95c2a..56bdc0a2 100644 --- a/canopen_ros2_controllers/src/canopen_proxy_controller.cpp +++ b/canopen_ros2_controllers/src/canopen_proxy_controller.cpp @@ -32,13 +32,6 @@ namespace using ControllerCommandMsg = canopen_ros2_controllers::CanopenProxyController::ControllerCommandMsg; // called from RT control loop -void reset_controller_command_msg( - std::shared_ptr & msg, const std::string & joint_name) -{ - msg->index = 0u; - msg->subindex = 0u; - msg->data = 0u; -} bool propagate_controller_command_msg(std::shared_ptr & msg) { // TODO (livanov93): add better logic to decide if a message @@ -106,10 +99,7 @@ controller_interface::CallbackReturn CanopenProxyController::on_configure( tpdo_subscriber_ = get_node()->create_subscription( "~/tpdo", rclcpp::SystemDefaultsQoS(), callback_cmd); - std::shared_ptr msg = std::make_shared(); - reset_controller_command_msg(msg, joint_name_); - - input_cmd_.writeFromNonRT(msg); + input_cmd_.writeFromNonRT(nullptr); try { @@ -219,12 +209,11 @@ CanopenProxyController::command_interface_configuration() const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(9); + command_interfaces_config.names.reserve(8); command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/index"); command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/subindex"); - command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/type"); command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/data"); - command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/owns"); + command_interfaces_config.names.push_back(joint_name_ + "/" + "tpdo/ons"); command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/reset"); command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/reset_fbk"); command_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/start"); @@ -239,10 +228,9 @@ controller_interface::InterfaceConfiguration CanopenProxyController::state_inter controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(5); + state_interfaces_config.names.reserve(4); state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/index"); state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/subindex"); - state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/type"); state_interfaces_config.names.push_back(joint_name_ + "/" + "rpdo/data"); state_interfaces_config.names.push_back(joint_name_ + "/" + "nmt/state"); @@ -253,7 +241,10 @@ controller_interface::CallbackReturn CanopenProxyController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_command_msg(*(input_cmd_.readFromRT)(), joint_name_); + if (input_cmd_.readFromRT()) + { + *(input_cmd_.readFromRT()) = nullptr; + } return controller_interface::CallbackReturn::SUCCESS; } @@ -342,6 +333,8 @@ controller_interface::return_type CanopenProxyController::update( command_interfaces_[CommandInterfaces::TPDO_DATA].set_value((*current_cmd)->data); // tpdo data one shot mechanism command_interfaces_[CommandInterfaces::TPDO_ONS].set_value(kCommandValue); + + *(input_cmd_.readFromRT()) = nullptr; } return controller_interface::return_type::OK; diff --git a/canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp b/canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp index b30ee640..6f1732d7 100644 --- a/canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp +++ b/canopen_ros2_controllers/test/test_canopen_proxy_controller.cpp @@ -58,6 +58,8 @@ TEST_F(CanopenProxyControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(controller_->joint_name_.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + // check that the message is reset + auto msg = controller_->input_cmd_.readFromNonRT(); ASSERT_THAT(controller_->joint_name_, joint_name_); } @@ -91,10 +93,8 @@ TEST_F(CanopenProxyControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_cmd_.readFromNonRT(); - EXPECT_EQ((*msg)->index, 0u); - EXPECT_EQ((*msg)->subindex, 0u); - EXPECT_EQ((*msg)->data, 0u); + auto msg = *(controller_->input_cmd_.readFromNonRT()); + EXPECT_EQ(msg, nullptr); } TEST_F(CanopenProxyControllerTest, update_success) diff --git a/canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp b/canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp index b2fe921a..2e7788a5 100644 --- a/canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp +++ b/canopen_ros2_controllers/test/test_canopen_proxy_controller.hpp @@ -232,16 +232,16 @@ class CanopenProxyControllerFixture : public ::testing::Test // Controller-related parameters std::string joint_name_ = {"joint1"}; std::vector command_interface_names_ = { - "tpdo/index", "tpdo/subindex", "tpdo/type", "tpdo/data", "tpdo/owns", + "tpdo/index", "tpdo/subindex", "tpdo/data", "tpdo/ons", "nmt/reset", "nmt/reset_fbk", "nmt/start", "nmt/start_fbk"}; std::vector state_interface_names_ = { - "rpdo/index", "rpdo/subindex", "rpdo/type", "rpdo/data", "nmt/state"}; + "rpdo/index", "rpdo/subindex", "rpdo/data", "nmt/state"}; // see StateInterfaces in canopen_proxy_controller.hpp for correct order; - std::array joint_state_values_ = {0, 0, 0, 0, 0}; + std::array joint_state_values_ = {0, 0, 0, 0}; // see CommandInterfaces in canopen_proxy_controller.hpp for correct order; - std::array joint_command_values_ = {101.101, 101.101, 101.101, 101.101, 101.101, + std::array joint_command_values_ = {101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101}; std::vector state_itfs_; From 5efe74b4bde3a6c7a21c85262515a3be31c8348a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 30 Jun 2023 16:09:22 +0200 Subject: [PATCH 56/59] Update ros2_control docs. (#149) * Correct Proxy controller after changes and update docs * Update codespellignore Signed-off-by: Christoph Hellmann Santos * Fix tests Signed-off-by: Christoph Hellmann Santos * Fix owns/ons error in tests Signed-off-by: Christoph Hellmann Santos * Fixup tests. * Small docs updates. * Change tpdo data. * Add nmt to the system test. * Update doc to prove working tpdo rpdo * Fix typo * Fix formatting. * Optimize docs. --------- Signed-off-by: Christoph Hellmann Santos Co-authored-by: Christoph Hellmann Santos Co-authored-by: Xi Huang --- canopen/sphinx/quickstart/examples.rst | 21 ++++++++++- canopen/sphinx/quickstart/operation.rst | 1 + .../ros2_control_system-test.rst | 37 ++++++++++++++++--- 3 files changed, 53 insertions(+), 6 deletions(-) diff --git a/canopen/sphinx/quickstart/examples.rst b/canopen/sphinx/quickstart/examples.rst index 20780343..2e3505ca 100644 --- a/canopen/sphinx/quickstart/examples.rst +++ b/canopen/sphinx/quickstart/examples.rst @@ -2,7 +2,7 @@ Running Examples ================ In order to tryout the library a few examples are provided in the ``canopen_tests`` directory. -You can run them if you have started the vcan0 interface. +You can run them if you have :ref:`started the vcan0 interface `. Service Interface --------------------- @@ -22,6 +22,25 @@ Managed Service Interface ROS2 Control ------------ +Proxy Setup +,,,,,,,,,,, + +.. code-block:: bash + + ros2 launch canopen_tests canopen_system.launch.py + + +CiA402 Setup +,,,,,,,,,,,, + +.. code-block:: bash + + ros2 launch canopen_tests cia402_system.launch.py + + +Robot Setup +,,,,,,,,,,,, + .. code-block:: bash ros2 launch canopen_tests robot_control_setup.launch.py diff --git a/canopen/sphinx/quickstart/operation.rst b/canopen/sphinx/quickstart/operation.rst index d600dee2..9229fbf9 100644 --- a/canopen/sphinx/quickstart/operation.rst +++ b/canopen/sphinx/quickstart/operation.rst @@ -1,5 +1,6 @@ Setup CAN Controller ==================== +.. _quick-start-setup-can-controller: **Option 1**: Virtual CANController diff --git a/canopen/sphinx/software-tests/ros2_control_system-test.rst b/canopen/sphinx/software-tests/ros2_control_system-test.rst index c9e2735e..b377b405 100644 --- a/canopen/sphinx/software-tests/ros2_control_system-test.rst +++ b/canopen/sphinx/software-tests/ros2_control_system-test.rst @@ -13,6 +13,15 @@ Test details Description; Create an exemplary ros2_control SystemInterface with CAN master and communicates to a slave node. Prerequisites; vcan0 must be available +To bring up vcan0: + + .. code-block:: bash + + sudo modprobe vcan + sudo ip link add dev vcan0 type vcan + sudo ip link set vcan0 txqueuelen 1000 + sudo ip link set up vcan0 + Explanation of the test ------------------------ @@ -36,13 +45,31 @@ The next few lines show you some command to have exemplary usage of the ros2_con ros2 topic echo /node_1_controller/rpdo -3. In a new terminal publish some data to the controller to write them to the CAN bus: +3. Open a new terminal reset the state of nmt. + + .. code-block:: bash + + ros2 service call /node_1_controller/nmt_reset_node std_srvs/srv/Trigger {} + + You will expect changes in the ``/dynamic_joint_states`` topic. + + +4. In a new terminal publish some data to the controller to write them to the CAN bus: .. code-block:: bash ros2 topic pub --once /node_1_controller/tpdo canopen_interfaces/msg/COData " - index: 25 - subindex: 35 - data: 238" + index: 0x4000 + subindex: 0 + data: 0x1122" + + Now watch how data in the topic ``/node_1_controller/rpdo`` are changing. There is a mirror of the data on 0x4001. + That is, the slave node will mirror the data on 0x4001 via its tpdo and the proxy device will get the data via its rpdo. + You should see this + + .. code-block:: bash - Now watch how data in the first two opened terminals are changing. + index: 16385 # This is 0x4001 + subindex: 0 + data: 4386 + --- From 4b5da6159ee30974b80c69bc9d7e3f9cf3609b8a Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Fri, 30 Jun 2023 16:24:22 +0200 Subject: [PATCH 57/59] Update changelog Signed-off-by: Christoph Hellmann Santos --- canopen/CHANGELOG.rst | 5 +++++ canopen_402_driver/CHANGELOG.rst | 6 ++++++ canopen_base_driver/CHANGELOG.rst | 7 +++++++ canopen_core/CHANGELOG.rst | 6 ++++++ canopen_fake_slaves/CHANGELOG.rst | 5 +++++ canopen_interfaces/CHANGELOG.rst | 3 +++ canopen_master_driver/CHANGELOG.rst | 5 +++++ canopen_proxy_driver/CHANGELOG.rst | 6 ++++++ canopen_ros2_control/CHANGELOG.rst | 5 +++++ canopen_ros2_controllers/CHANGELOG.rst | 5 +++++ canopen_tests/CHANGELOG.rst | 5 +++++ canopen_utils/CHANGELOG.rst | 6 ++++++ lely_core_libraries/CHANGELOG.rst | 6 ++++++ 13 files changed, 70 insertions(+) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 121b55ce..4659cbd0 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update ros2_control docs. +* Contributors: Christoph Hellmann Santos, Dr. Denis, Xi Huang + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index 8f11b78d..ae4c58fa 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Fix maintainer naming +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index a0c3f546..20f3daf3 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Fix maintainer naming +* Update printed output in lely_driver_bridge.cpp +* Contributors: Christoph Hellmann Santos, yos627 + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index 80a9b4c4..e32c48bf 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Fix maintainer naming +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 8fa56c74..850de7a1 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 72a0f679..0f078b9b 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 5aec5b3d..18128df2 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index d7953f3b..29669154 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Fix maintainer naming +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 37c8fbde..72fed366 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Correct Proxy controller after changes and update tests. +* Contributors: Dr. Denis, Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index ce7448fc..eea537d3 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Correct Proxy controller after changes and update tests. +* Contributors: Dr. Denis, Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index 14f93883..e029f03c 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index b1b6e49c..269f2656 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Fix maintainer naming +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 567b8c2f..675b1dc7 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing license headers and activate ament_copyright +* Fix python versions +* Contributors: Christoph Hellmann Santos + 0.2.6 (2023-06-24) ------------------ * Move install from external project to cmake main From e0b5706810b699629f7222ed3c37a8ea5e6ee3cb Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos Date: Fri, 30 Jun 2023 16:35:36 +0200 Subject: [PATCH 58/59] 0.2.7 --- canopen/CHANGELOG.rst | 4 ++-- canopen/package.xml | 2 +- canopen_402_driver/CHANGELOG.rst | 4 ++-- canopen_402_driver/package.xml | 2 +- canopen_base_driver/CHANGELOG.rst | 4 ++-- canopen_base_driver/package.xml | 2 +- canopen_core/CHANGELOG.rst | 4 ++-- canopen_core/package.xml | 2 +- canopen_fake_slaves/CHANGELOG.rst | 4 ++-- canopen_fake_slaves/package.xml | 2 +- canopen_interfaces/CHANGELOG.rst | 4 ++-- canopen_interfaces/package.xml | 2 +- canopen_master_driver/CHANGELOG.rst | 4 ++-- canopen_master_driver/package.xml | 2 +- canopen_proxy_driver/CHANGELOG.rst | 4 ++-- canopen_proxy_driver/package.xml | 2 +- canopen_ros2_control/CHANGELOG.rst | 4 ++-- canopen_ros2_control/package.xml | 2 +- canopen_ros2_controllers/CHANGELOG.rst | 4 ++-- canopen_ros2_controllers/package.xml | 2 +- canopen_tests/CHANGELOG.rst | 4 ++-- canopen_tests/package.xml | 2 +- canopen_utils/CHANGELOG.rst | 4 ++-- canopen_utils/package.xml | 2 +- canopen_utils/setup.py | 2 +- lely_core_libraries/CHANGELOG.rst | 4 ++-- lely_core_libraries/package.xml | 2 +- 27 files changed, 40 insertions(+), 40 deletions(-) diff --git a/canopen/CHANGELOG.rst b/canopen/CHANGELOG.rst index 4659cbd0..b5089d8c 100644 --- a/canopen/CHANGELOG.rst +++ b/canopen/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Update ros2_control docs. * Contributors: Christoph Hellmann Santos, Dr. Denis, Xi Huang diff --git a/canopen/package.xml b/canopen/package.xml index abc77f80..1cc8f507 100644 --- a/canopen/package.xml +++ b/canopen/package.xml @@ -2,7 +2,7 @@ canopen - 0.2.6 + 0.2.7 Meta-package aggregating the ros2_canopen packages and documentation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_402_driver/CHANGELOG.rst b/canopen_402_driver/CHANGELOG.rst index ae4c58fa..433c27e4 100644 --- a/canopen_402_driver/CHANGELOG.rst +++ b/canopen_402_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_402_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos diff --git a/canopen_402_driver/package.xml b/canopen_402_driver/package.xml index 3a9253f2..ecc05a48 100644 --- a/canopen_402_driver/package.xml +++ b/canopen_402_driver/package.xml @@ -2,7 +2,7 @@ canopen_402_driver - 0.2.6 + 0.2.7 Driiver for devices implementing CIA402 profile Christoph Hellmann Santos LGPL-v3 diff --git a/canopen_base_driver/CHANGELOG.rst b/canopen_base_driver/CHANGELOG.rst index 20f3daf3..7ff5717d 100644 --- a/canopen_base_driver/CHANGELOG.rst +++ b/canopen_base_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_base_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Update printed output in lely_driver_bridge.cpp diff --git a/canopen_base_driver/package.xml b/canopen_base_driver/package.xml index ebf88aca..b000b6a5 100644 --- a/canopen_base_driver/package.xml +++ b/canopen_base_driver/package.xml @@ -2,7 +2,7 @@ canopen_base_driver - 0.2.6 + 0.2.7 Library containing abstract CANopen driver class for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_core/CHANGELOG.rst b/canopen_core/CHANGELOG.rst index e32c48bf..55fe1b93 100644 --- a/canopen_core/CHANGELOG.rst +++ b/canopen_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos diff --git a/canopen_core/package.xml b/canopen_core/package.xml index d04b198d..b6f94a63 100644 --- a/canopen_core/package.xml +++ b/canopen_core/package.xml @@ -2,7 +2,7 @@ canopen_core - 0.2.6 + 0.2.7 Core ros2_canopen functionalities such as DeviceContainer and master Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_fake_slaves/CHANGELOG.rst b/canopen_fake_slaves/CHANGELOG.rst index 850de7a1..e7a047c1 100644 --- a/canopen_fake_slaves/CHANGELOG.rst +++ b/canopen_fake_slaves/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_fake_slaves ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Contributors: Christoph Hellmann Santos diff --git a/canopen_fake_slaves/package.xml b/canopen_fake_slaves/package.xml index 0588e7c4..ba763ec3 100644 --- a/canopen_fake_slaves/package.xml +++ b/canopen_fake_slaves/package.xml @@ -2,7 +2,7 @@ canopen_fake_slaves - 0.2.6 + 0.2.7 Package with mock canopen slave Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_interfaces/CHANGELOG.rst b/canopen_interfaces/CHANGELOG.rst index 0f078b9b..df74dc39 100644 --- a/canopen_interfaces/CHANGELOG.rst +++ b/canopen_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ 0.2.6 (2023-06-24) ------------------ diff --git a/canopen_interfaces/package.xml b/canopen_interfaces/package.xml index de3221d6..91d23b71 100644 --- a/canopen_interfaces/package.xml +++ b/canopen_interfaces/package.xml @@ -2,7 +2,7 @@ canopen_interfaces - 0.2.6 + 0.2.7 Services and Messages for ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_master_driver/CHANGELOG.rst b/canopen_master_driver/CHANGELOG.rst index 18128df2..5200470f 100644 --- a/canopen_master_driver/CHANGELOG.rst +++ b/canopen_master_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_master_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Contributors: Christoph Hellmann Santos diff --git a/canopen_master_driver/package.xml b/canopen_master_driver/package.xml index e68214db..732e6737 100644 --- a/canopen_master_driver/package.xml +++ b/canopen_master_driver/package.xml @@ -2,7 +2,7 @@ canopen_master_driver - 0.2.6 + 0.2.7 Basic canopen master implementation Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_proxy_driver/CHANGELOG.rst b/canopen_proxy_driver/CHANGELOG.rst index 29669154..2abb6d83 100644 --- a/canopen_proxy_driver/CHANGELOG.rst +++ b/canopen_proxy_driver/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_proxy_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos diff --git a/canopen_proxy_driver/package.xml b/canopen_proxy_driver/package.xml index e2504971..5b61f3f1 100644 --- a/canopen_proxy_driver/package.xml +++ b/canopen_proxy_driver/package.xml @@ -2,7 +2,7 @@ canopen_proxy_driver - 0.2.6 + 0.2.7 Simple proxy driver for the ros2_canopen stack Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_ros2_control/CHANGELOG.rst b/canopen_ros2_control/CHANGELOG.rst index 72fed366..fe59c359 100644 --- a/canopen_ros2_control/CHANGELOG.rst +++ b/canopen_ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Correct Proxy controller after changes and update tests. * Contributors: Dr. Denis, Christoph Hellmann Santos diff --git a/canopen_ros2_control/package.xml b/canopen_ros2_control/package.xml index 18d8038f..c0c32962 100644 --- a/canopen_ros2_control/package.xml +++ b/canopen_ros2_control/package.xml @@ -2,7 +2,7 @@ canopen_ros2_control - 0.2.6 + 0.2.7 ros2_control wrapper for ros2_canopen functionalities Lovro Ivanov Denis Stogl diff --git a/canopen_ros2_controllers/CHANGELOG.rst b/canopen_ros2_controllers/CHANGELOG.rst index eea537d3..ca2e935b 100644 --- a/canopen_ros2_controllers/CHANGELOG.rst +++ b/canopen_ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Correct Proxy controller after changes and update tests. * Contributors: Dr. Denis, Christoph Hellmann Santos diff --git a/canopen_ros2_controllers/package.xml b/canopen_ros2_controllers/package.xml index c010a79f..f94e5bcd 100644 --- a/canopen_ros2_controllers/package.xml +++ b/canopen_ros2_controllers/package.xml @@ -2,7 +2,7 @@ canopen_ros2_controllers - 0.2.6 + 0.2.7 ros2_control controllers for ros2_canopen functionalities Denis Stogl Lovro Ivanov diff --git a/canopen_tests/CHANGELOG.rst b/canopen_tests/CHANGELOG.rst index e029f03c..7dcbb16f 100644 --- a/canopen_tests/CHANGELOG.rst +++ b/canopen_tests/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Contributors: Christoph Hellmann Santos diff --git a/canopen_tests/package.xml b/canopen_tests/package.xml index 71ead850..a367512d 100644 --- a/canopen_tests/package.xml +++ b/canopen_tests/package.xml @@ -2,7 +2,7 @@ canopen_tests - 0.2.6 + 0.2.7 Package with tests for ros2_canopen Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/CHANGELOG.rst b/canopen_utils/CHANGELOG.rst index 269f2656..c2db1a9c 100644 --- a/canopen_utils/CHANGELOG.rst +++ b/canopen_utils/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package canopen_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Fix maintainer naming * Contributors: Christoph Hellmann Santos diff --git a/canopen_utils/package.xml b/canopen_utils/package.xml index 5f1d1640..38d206b2 100644 --- a/canopen_utils/package.xml +++ b/canopen_utils/package.xml @@ -2,7 +2,7 @@ canopen_utils - 0.2.6 + 0.2.7 Utils for working with ros2_canopen. Christoph Hellmann Santos Apache-2.0 diff --git a/canopen_utils/setup.py b/canopen_utils/setup.py index f8474385..0b617dca 100644 --- a/canopen_utils/setup.py +++ b/canopen_utils/setup.py @@ -18,7 +18,7 @@ setup( name=package_name, - version="0.2.6", + version="0.2.7", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lely_core_libraries/CHANGELOG.rst b/lely_core_libraries/CHANGELOG.rst index 675b1dc7..b90a7104 100644 --- a/lely_core_libraries/CHANGELOG.rst +++ b/lely_core_libraries/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package lely_core_libraries ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.2.7 (2023-06-30) +------------------ * Add missing license headers and activate ament_copyright * Fix python versions * Contributors: Christoph Hellmann Santos diff --git a/lely_core_libraries/package.xml b/lely_core_libraries/package.xml index f52f4b94..cf67ee00 100644 --- a/lely_core_libraries/package.xml +++ b/lely_core_libraries/package.xml @@ -2,7 +2,7 @@ lely_core_libraries - 0.2.6 + 0.2.7 ROS wrapper for lely-core-libraries From 8930215927633b4d31bc8cc6dca42520f52a874d Mon Sep 17 00:00:00 2001 From: waterlinux Date: Fri, 11 Aug 2023 18:23:58 +0200 Subject: [PATCH 59/59] docs: fixed launch file path in instructions --- canopen/sphinx/user-guide/how-to-create-a-robot-system.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst b/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst index 0395bc81..8dba5f54 100644 --- a/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst +++ b/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst @@ -130,6 +130,7 @@ leveragin ros2_control. [ FindPackageShare("canopen_tests"), "urdf", + "robot_controller", "robot_controller.urdf.xacro", ] ),