Skip to content

Modified to check the status of safety function of each safety inputs #29

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

Open
shamada-idec wants to merge 2 commits into
base: main
Choose a base branch
from
+65 −25
Open

Modified to check the status of safety function of each safety inputs #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d86c055
Modified to check the status of safety function of each safety inputs…
shamada-idec Mar 12, 2025
1be154e
Modified comments of getLeftSafetyValue() and getRightSafetyValue()
shamada-idec Mar 13, 2025
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
2 changes: 1 addition & 1 deletion include/diff_drive_controller/DiffDriveController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,6 @@ namespace ezw::swd {

ezw::smcservice::DBusClient m_left_controller, m_right_controller;

std::map<ezw::smccore::ISafeMotionService::SafetyFunctionId, int8_t> m_left_safety_functions, m_right_safety_functions;
std::multimap<ezw::smccore::ISafeMotionService::SafetyFunctionId, int8_t> m_left_safety_functions, m_right_safety_functions;
};
} // namespace ezw::swd
88 changes: 64 additions & 24 deletions src/diff_drive_controller/DiffDriveController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -216,12 +216,14 @@ namespace ezw::swd {
(int)err);
throw std::runtime_error("Failed reading the right motor SAFEIN_1 mapping");
}
m_right_safety_functions[safety_control_word_mapping.safety_function_0] = 0;
m_right_safety_functions[safety_control_word_mapping.safety_function_1] = 1;
m_right_safety_functions[safety_control_word_mapping.safety_function_2] = 2;
m_right_safety_functions[safety_control_word_mapping.safety_function_3] = 3;
m_right_safety_functions[safety_control_word_mapping.safety_function_4] = 4;
m_right_safety_functions[safety_control_word_mapping.safety_function_5] = 5;
m_right_safety_functions = {
{safety_control_word_mapping.safety_function_0, 0},
{safety_control_word_mapping.safety_function_1, 1},
{safety_control_word_mapping.safety_function_2, 2},
{safety_control_word_mapping.safety_function_3, 3},
{safety_control_word_mapping.safety_function_4, 4},
{safety_control_word_mapping.safety_function_5, 5},
};

err = m_left_controller.getSafetyControlWordMapping(ezw::smccore::ISafeMotionService::SafetyControlWordId::SAFEIN_1, safety_control_word_mapping);
if (ERROR_NONE != err) {
Expand All @@ -231,12 +233,14 @@ namespace ezw::swd {
(int)err);
throw std::runtime_error("Failed reading the left motor SAFEIN_1 mapping");
}
m_left_safety_functions[safety_control_word_mapping.safety_function_0] = 0;
m_left_safety_functions[safety_control_word_mapping.safety_function_1] = 1;
m_left_safety_functions[safety_control_word_mapping.safety_function_2] = 2;
m_left_safety_functions[safety_control_word_mapping.safety_function_3] = 3;
m_left_safety_functions[safety_control_word_mapping.safety_function_4] = 4;
m_left_safety_functions[safety_control_word_mapping.safety_function_5] = 5;
m_left_safety_functions = {
{safety_control_word_mapping.safety_function_0, 0},
{safety_control_word_mapping.safety_function_1, 1},
{safety_control_word_mapping.safety_function_2, 2},
{safety_control_word_mapping.safety_function_3, 3},
{safety_control_word_mapping.safety_function_4, 4},
{safety_control_word_mapping.safety_function_5, 5},
};

// Start the timers
m_timer_watchdog = create_wall_timer(std::chrono::milliseconds(m_params->getWatchdogReceiveMs()), std::bind(&DiffDriveController::cbTimerWatchdogReceive, this));
Expand Down Expand Up @@ -829,18 +833,54 @@ namespace ezw::swd {
safein1_r[4] = safety_control_word.safety_function_4;
safein1_r[5] = safety_control_word.safety_function_5;

/**
* @brief Retrieve the status of safety inputs of the specified safety function id from the left SWD.
*
* @param id Id of the safety function.
*
* @return false if any one of them is false, otherwise true.
*/
auto getLeftSafetyValue = [this, &safein1_l](ezw::smccore::ISafeMotionService::SafetyFunctionId id) {
auto indexes = m_left_safety_functions.equal_range(id);
for (auto it = indexes.first; it != indexes.second; it++) {
if (safein1_l[it->second] == false) {
return false;
}
}

return true;
};

/**
* @brief Retrieve the status of safety inputs of the specified safety function id from the right SWD.
*
* @param id Id of the safety function.
*
* @return false if any one of them is false, otherwise true.
*/
auto getRightSafetyValue = [this, &safein1_r](ezw::smccore::ISafeMotionService::SafetyFunctionId id) {
auto indexes = m_right_safety_functions.equal_range(id);
for (auto it = indexes.first; it != indexes.second; it++) {
if (safein1_r[it->second] == false) {
return false;
}
}

return true;
};

// Reading SBC
res_l = m_left_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SBC_1) ? safein1_l[m_left_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SBC_1]] : true;
res_r = m_right_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SBC_1) ? safein1_r[m_right_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SBC_1]] : true;
res_l = getLeftSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SBC_1);
res_r = getRightSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SBC_1);

msg.safe_brake_control = static_cast<uint8_t>(!res_l || !res_r);
if (m_first_entry || msg.safe_brake_control != m_safety_msg.safe_brake_control) {
RCLCPP_INFO(get_logger(), msg.safe_brake_control ? "SBC enabled." : "SBC disabled.");
}

// Reading STO
res_l = m_left_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::STO) ? safein1_l[m_left_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::STO]] : true;
res_r = m_right_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::STO) ? safein1_r[m_right_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::STO]] : true;
res_l = getLeftSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::STO);
res_r = getRightSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::STO);

msg.safe_torque_off = static_cast<uint8_t>(!res_l || !res_r);
if (m_first_entry || msg.safe_torque_off != m_safety_msg.safe_torque_off) {
Expand All @@ -850,11 +890,11 @@ namespace ezw::swd {
// Reading SDI
bool sdi_l_p, sdi_l_n, sdi_r_p, sdi_r_n, sdi_p, sdi_n;

sdi_l_p = m_left_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIP_1) ? safein1_l[m_left_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIP_1]] : true;
sdi_r_p = m_right_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIP_1) ? safein1_r[m_right_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIP_1]] : true;
sdi_l_p = getLeftSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIP_1);
sdi_r_p = getRightSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIP_1);

sdi_l_n = m_left_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIN_1) ? safein1_l[m_left_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIN_1]] : true;
sdi_r_n = m_right_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIN_1) ? safein1_r[m_right_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIN_1]] : true;
sdi_l_n = getLeftSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIN_1);
sdi_r_n = getRightSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SDIN_1);

if (m_left_motor_polarity) {
sdi_p = !sdi_l_n || !sdi_r_p;
Expand All @@ -879,17 +919,17 @@ namespace ezw::swd {
// Reading SLS_1
// When an object is detected in the SLS area, res_r and res_l will be false.
// If both or either res_l or res_r are false, set safety_limited_speed to true.
res_l = m_left_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1) ? safein1_l[m_left_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1]] : true;
res_r = m_right_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1) ? safein1_r[m_right_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1]] : true;
res_l = getLeftSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1);
res_r = getRightSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1);

msg.safety_limited_speed_1 = static_cast<uint8_t>(!res_l || !res_r);
if (m_first_entry || msg.safety_limited_speed_1 != m_safety_msg.safety_limited_speed_1) {
RCLCPP_INFO(get_logger(), msg.safety_limited_speed_1 ? "SLS_1 enabled." : "SLS_1 disabled.");
}

// Reading SLS_2
res_l = m_left_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_2) ? safein1_l[m_left_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_2]] : true;
res_r = m_right_safety_functions.count(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_2) ? safein1_r[m_right_safety_functions[ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_2]] : true;
res_l = getLeftSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_2);
res_r = getRightSafetyValue(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_2);

msg.safety_limited_speed_2 = static_cast<uint8_t>(!res_l || !res_r);
if (m_first_entry || msg.safety_limited_speed_2 != m_safety_msg.safety_limited_speed_2) {
Expand Down