From 7687f3276107d81f52e1f961566942508eb07d85 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+ipa-cmh@users.noreply.github.com> Date: Wed, 22 Nov 2023 16:58:29 +0100 Subject: [PATCH] Add fake profile velocity (#230) * Add simple sequence homing emulation Signed-off-by: Christoph Hellmann Santos * Add fake velocity mode Signed-off-by: Christoph Hellmann Santos * Formatting Signed-off-by: Christoph Hellmann Santos --------- Signed-off-by: Christoph Hellmann Santos --- .../canopen_fake_slaves/cia402_slave.hpp | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) 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 609152cc..8c7f13c3 100644 --- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp +++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp @@ -74,6 +74,11 @@ class CIA402MockSlave : public canopen::BasicSlave RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); homing_mode.join(); } + if (profiled_velocity_mode.joinable()) + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + profiled_velocity_mode.join(); + } } protected: @@ -334,6 +339,32 @@ class CIA402MockSlave : public canopen::BasicSlave } } + void run_profile_velocity_mode() + { + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_profile_velocity_mode"); + double actual_position = static_cast(((int32_t)(*this)[0x6064][0])) / 1000.0; + double target_velocity = static_cast(((int32_t)(*this)[0x60FF][0])) / 1000.0; + double old_target = target_velocity; + double control_cycle_period_d = 0.01; + while ((state.load() == InternalState::Operation_Enable) && + (operation_mode.load() == Profiled_Velocity) && (rclcpp::ok())) + { + actual_position = static_cast(((int32_t)(*this)[0x6064][0])) / 1000.0; + target_velocity = static_cast(((int32_t)(*this)[0x60FF][0])) / 1000.0; + if (old_target != target_velocity) + { + old_target = target_velocity; + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "New target velocity: %f", target_velocity); + } + (*this)[0x606C][0] = (int32_t)(target_velocity * 1000); + (*this)[0x6064][0] = + (int32_t)((actual_position + target_velocity * control_cycle_period_d) * 1000.0); + std::this_thread::sleep_for( + std::chrono::milliseconds(((int32_t)control_cycle_period_d * 1000))); + } + RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Leaving run_profile_velocity_mode"); + } + void run_homing_mode() { bool homed = false; @@ -565,6 +596,12 @@ class CIA402MockSlave : public canopen::BasicSlave rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); homing_mode.join(); } + if (profiled_velocity_mode.joinable()) + { + RCLCPP_INFO( + rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread."); + profiled_velocity_mode.join(); + } old_operation_mode.store(operation_mode.load()); switch (operation_mode.load()) { @@ -577,6 +614,12 @@ class CIA402MockSlave : public canopen::BasicSlave case Interpolated_Position: start_interpolated_pos_mode(); break; + case Homing: + start_homing_mode(); + break; + case Profiled_Velocity: + start_profile_velocity_mode(); + break; default: break; } @@ -605,6 +648,12 @@ class CIA402MockSlave : public canopen::BasicSlave homing_mode = std::thread(std::bind(&CIA402MockSlave::run_homing_mode, this)); } + void start_profile_velocity_mode() + { + profiled_velocity_mode = + std::thread(std::bind(&CIA402MockSlave::run_profile_velocity_mode, this)); + } + void on_quickstop_active() { if (is_enable_operation())