Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Run interpolated mode in fake slave #126

Merged
merged 1 commit into from
Jun 7, 2023
Merged

Run interpolated mode in fake slave #126

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,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
Expand Down
81 changes: 81 additions & 0 deletions canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,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:
Expand Down Expand Up @@ -136,6 +141,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;

Expand Down Expand Up @@ -248,6 +254,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<double>((uint8_t)(*this)[0x60C2][1]);
double target_position = static_cast<double>((int32_t)(*this)[0x60C1][1]);

// int32_t offset = (*this)[0x60B0][0];

// Convert parameters to SI units
interpolation_period *= std::pow(10.0, static_cast<double>((int8_t)(*this)[0x60C2][2]));
target_position /= 1000.0;
double actual_position = static_cast<double>((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<int>(interpolation_period * 1000)));

target_position = static_cast<double>((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<std::mutex> 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<int32_t>(actual_position * 1000);
(*this)[0x606C][0] = static_cast<int32_t>(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<std::mutex> lock(w_mutex);
(*this)[0x6041][0] = status_word;
this->TpdoEvent(1);
}
}
}
}

void run_position_mode() {}

void set_new_status_word_and_state()
Expand Down Expand Up @@ -432,6 +498,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())
{
Expand All @@ -441,6 +513,9 @@ class CIA402MockSlave : public canopen::BasicSlave
case Profiled_Position:
start_profile_pos_mode();
break;
case Interpolated_Position:
start_interpolated_pos_mode();
break;
default:
break;
}
Expand All @@ -458,6 +533,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())
Expand Down