From d7bad552c60242df17f24cc0e0079d9f4633ca0b Mon Sep 17 00:00:00 2001 From: Lewe Christiansen <51883655+LeweC@users.noreply.github.com> Date: Wed, 22 Nov 2023 16:36:44 +0100 Subject: [PATCH 1/2] Documentation: Fix launch file spelling (#208) --- canopen/sphinx/quickstart/examples.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/canopen/sphinx/quickstart/examples.rst b/canopen/sphinx/quickstart/examples.rst index 2e3505ca..09843b28 100644 --- a/canopen/sphinx/quickstart/examples.rst +++ b/canopen/sphinx/quickstart/examples.rst @@ -9,7 +9,7 @@ Service Interface .. code-block:: bash - ros2 launch canopen_tests ci402_setup.launch.py + ros2 launch canopen_tests cia402_setup.launch.py Managed Service Interface @@ -17,7 +17,7 @@ Managed Service Interface .. code-block:: bash - ros2 launch canopen_tests ci402_lifecycle_setup.launch.py + ros2 launch canopen_tests cia402_lifecycle_setup.launch.py ROS2 Control ------------ From 70fb66f9e575d798264745e00458f9df7b3d3f7f Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 22 Nov 2023 16:49:11 +0100 Subject: [PATCH 2/2] Add simple sequence homing emulation (#229) Signed-off-by: Christoph Hellmann Santos --- .../canopen_fake_slaves/cia402_slave.hpp | 54 ++++++++++++++++++- 1 file changed, 53 insertions(+), 1 deletion(-) 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 4ae7bb26..609152cc 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -69,6 +69,11 @@ class CIA402MockSlave : public canopen::BasicSlave RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); interpolated_position_mode.join(); } + if (homing_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + homing_mode.join(); + } } protected: @@ -156,6 +161,7 @@ class CIA402MockSlave : public canopen::BasicSlave std::thread cyclic_position_mode; std::thread cyclic_velocity_mode; std::thread interpolated_position_mode; + std::thread homing_mode; double cycle_time; @@ -328,7 +334,42 @@ class CIA402MockSlave : public canopen::BasicSlave } } - void run_position_mode() {} + void run_homing_mode() + { + bool homed = false; + while ((state.load() == InternalState::Operation_Enable) && (operation_mode.load() == Homing) && + (rclcpp::ok())) + { + bool start_homing = control_word & CW_Operation_mode_specific0; + + if (start_homing && !homed) + { + set_status_bit(SW_Manufacturer_specific1); // Motor active + } + else + { + homed = false; + continue; + } + { + std::lock_guard lock(w_mutex); + (*this)[0x6041][0] = status_word; + this->TpdoEvent(1); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + if (start_homing) + { + clear_status_bit(SW_Manufacturer_specific1); // Motor inactive + set_status_bit(SW_Target_reached); // Homing complete + set_status_bit(SW_Operation_mode_specific0); // Homing attained + (*this)[0x6041][0] = status_word; + (*this)[0x6064][0] = static_cast(0); + (*this)[0x606C][0] = static_cast(0); + homed = true; + } + } + } void set_new_status_word_and_state() { @@ -518,6 +559,12 @@ class CIA402MockSlave : public canopen::BasicSlave rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); interpolated_position_mode.join(); } + if (homing_mode.joinable()) + { + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + homing_mode.join(); + } old_operation_mode.store(operation_mode.load()); switch (operation_mode.load()) { @@ -553,6 +600,11 @@ class CIA402MockSlave : public canopen::BasicSlave std::thread(std::bind(&CIA402MockSlave::run_interpolated_position_mode, this)); } + void start_homing_mode() + { + homing_mode = std::thread(std::bind(&CIA402MockSlave::run_homing_mode, this)); + } + void on_quickstop_active() { if (is_enable_operation())