Skip to content

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

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 1 commit into
base: main
Choose a base branch
from
Open

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

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
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 @@ -73,7 +73,7 @@ namespace ezw {
ros::Timer m_timer_odom, m_timer_watchdog, m_timer_pds, m_timer_safety;
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;

std::mutex m_safety_msg_mtx;
swd_ros_controllers::SafetyFunctions m_safety_msg;
Expand Down
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 @@ -321,12 +321,14 @@ namespace ezw {
(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 @@ -336,12 +338,14 @@ namespace ezw {
(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},
};

// Set m_motor_max_speed_rpm from wheel_sls and motor_reduction
m_motor_max_speed_rpm = static_cast<int32_t>(max_wheel_speed_rpm * m_l_motor_reduction);
Expand Down Expand Up @@ -971,18 +975,54 @@ namespace ezw {
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) {
ROS_INFO(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 @@ -992,11 +1032,11 @@ namespace ezw {
// 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 @@ -1019,17 +1059,17 @@ namespace ezw {
}

// Reading SLS_1
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) {
ROS_INFO(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