Program Listing for File cia402_slave.hpp
↰ Return to documentation for file (include/canopen_fake_slaves/cia402_slave.hpp
)
// 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 <lely/coapp/slave.hpp>
#include <lely/ev/co_task.hpp>
#include <lely/ev/loop.hpp>
#include <lely/io2/linux/can.hpp>
#include <lely/io2/posix/poll.hpp>
#include <lely/io2/sys/io.hpp>
#include <lely/io2/sys/sigset.hpp>
#include <lely/io2/sys/timer.hpp>
#include <atomic>
#include <mutex>
#include <thread>
#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();
}
if (interpolated_position_mode.joinable())
{
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();
}
if (profiled_velocity_mode.joinable())
{
RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread.");
profiled_velocity_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<bool> is_relative;
std::atomic<bool> is_running;
std::atomic<bool> is_halt;
std::atomic<bool> is_new_set_point;
std::atomic<int8_t> operation_mode;
std::atomic<int8_t> old_operation_mode;
std::mutex w_mutex;
uint16_t status_word;
uint16_t control_word;
std::atomic<InternalState> state;
std::thread profiled_position_mode;
std::thread profiled_velocity_mode;
std::thread cyclic_position_mode;
std::thread cyclic_velocity_mode;
std::thread interpolated_position_mode;
std::thread homing_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<double>(((uint32_t)(*this)[0x6081][0])) / 1000;
double profile_accerlation = static_cast<double>(((uint32_t)(*this)[0x6083][0])) / 1000;
double actual_position = static_cast<double>(((int32_t)(*this)[0x6064][0])) / 1000.0;
double target_position = static_cast<double>(((int32_t)(*this)[0x607A][0])) / 1000.0;
double actual_speed = static_cast<double>(((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<double>(((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<std::mutex> 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<std::mutex> 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_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_profile_velocity_mode()
{
RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_profile_velocity_mode");
double actual_position = static_cast<double>(((int32_t)(*this)[0x6064][0])) / 1000.0;
double target_velocity = static_cast<double>(((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<double>(((int32_t)(*this)[0x6064][0])) / 1000.0;
target_velocity = static_cast<double>(((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.0))));
}
RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Leaving run_profile_velocity_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<std::mutex> 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<int32_t>(0);
(*this)[0x606C][0] = static_cast<int32_t>(0);
homed = true;
}
}
}
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<std::mutex> lock(w_mutex);
status_word |= 1UL << bit;
}
void clear_status_bit(int bit)
{
std::scoped_lock<std::mutex> 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<std::mutex> 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();
}
if (interpolated_position_mode.joinable())
{
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();
}
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())
{
case Cyclic_Synchronous_Position:
start_sync_pos_mode();
break;
case Profiled_Position:
start_profile_pos_mode();
break;
case Interpolated_Position:
start_interpolated_pos_mode();
break;
case Homing:
start_homing_mode();
break;
case Profiled_Velocity:
start_profile_velocity_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 start_interpolated_pos_mode()
{
interpolated_position_mode =
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 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())
{
set_operation_enabled();
}
if (is_disable_voltage())
{
set_switch_on_disabled();
}
}
bool is_shutdown()
{
std::scoped_lock<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(w_mutex);
control_word = (*this)[0x6040][0];
}
set_new_status_word_and_state();
{
std::scoped_lock<std::mutex> 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