Skip to content

Commit 4ca573f

Browse files
mergify[bot]mergify[bot]
authored
Use integer representation of SafetyStatus.msg (backport #1734) (#1741)
This matches the other interfaces more closely: - RTDE - Primary - urinterfaces package The necessary conversion in the ROS dashboard client has been added.
1 parent dced45d commit 4ca573f

File tree

3 files changed

+89
-25
lines changed

3 files changed

+89
-25
lines changed

‎ur_dashboard_msgs/msg/SafetyStatus.msg‎

Lines changed: 22 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,23 @@
1-
string NORMAL = NORMAL
2-
string REDUCED = REDUCED
3-
string PROTECTIVE_STOP = PROTECTIVE_STOP
4-
string RECOVERY = RECOVERY
5-
string SAFEGUARD_STOP = SAFEGUARD_STOP
6-
string SYSTEM_EMERGENCY_STOP = SYSTEM_EMERGENCY_STOP
7-
string ROBOT_EMERGENCY_STOP = ROBOT_EMERGENCY_STOP
8-
string VIOLATION = VIOLATION
9-
string FAULT = FAULT
10-
string AUTOMATIC_MODE_SAFEGUARD_STOP = AUTOMATIC_MODE_SAFEGUARD_STOP
11-
string SYSTEM_THREE_POSITION_ENABLING_STOP = SYSTEM_THREE_POSITION_ENABLING_STOP
1+
int8 NORMAL=1
2+
int8 REDUCED=2
3+
int8 PROTECTIVE_STOP=3
4+
int8 RECOVERY=4
5+
int8 SAFEGUARD_STOP=5
6+
int8 SYSTEM_EMERGENCY_STOP=6
7+
int8 ROBOT_EMERGENCY_STOP=7
8+
int8 VIOLATION=8
9+
int8 FAULT=9
10+
int8 VALIDATE_JOINT_ID=10
11+
int8 UNDEFINED_SAFETY_MODE=11
12+
int8 AUTOMATIC_MODE_SAFEGUARD_STOP=12
13+
int8 SYSTEM_THREE_POSITION_ENABLING_STOP=13
14+
int8 TP_THREE_POSITION_ENABLING_STOP=14
15+
int8 IMMI_EMERGENCY_STOP=15
16+
int8 IMMI_SAFEGUARD_STOP=16
17+
int8 PROFISAFE_WAITING_FOR_PARAMETERS=17
18+
int8 PROFISAFE_AUTOMATIC_MODE_SAFEGUARD_STOP=18
19+
int8 PROFISAFE_SAFEGUARD_STOP=19
20+
int8 PROFISAFE_EMERGENCY_STOP=20
21+
int8 SAFETY_API_SAFEGUARD_STOP=22
1222

13-
string status
23+
int8 status

‎ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -187,6 +187,9 @@ class DashboardClientROS
187187
bool handleSafetyModeQuery(ur_dashboard_msgs::srv::GetSafetyMode::Request::SharedPtr req,
188188
ur_dashboard_msgs::srv::GetSafetyMode::Response::SharedPtr resp);
189189

190+
bool handleSafetyStatusQuery(ur_dashboard_msgs::srv::GetSafetyStatus::Request::SharedPtr req,
191+
ur_dashboard_msgs::srv::GetSafetyStatus::Response::SharedPtr resp);
192+
190193
bool handleRobotModeQuery(ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req,
191194
ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp);
192195

‎ur_robot_driver/src/dashboard_client_ros.cpp‎

Lines changed: 64 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -437,19 +437,8 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons
437437

438438
// Service to get robot safety status as a string
439439
get_safety_status_service_ = node->create_service<ur_dashboard_msgs::srv::GetSafetyStatus>(
440-
"~/get_safety_status", [&](const ur_dashboard_msgs::srv::GetSafetyStatus::Request::SharedPtr /*unused*/,
441-
ur_dashboard_msgs::srv::GetSafetyStatus::Response::SharedPtr resp) {
442-
auto dashboard_response =
443-
dashboardCallWithChecks([this]() { return client_->commandSafetyStatusWithResponse(); }, resp);
444-
if (resp->success) {
445-
handleDashboardResponseData(
446-
[dashboard_response, resp]() {
447-
resp->safety_status.status = std::get<std::string>(dashboard_response.data.at("safety_status"));
448-
},
449-
resp, dashboard_response);
450-
}
451-
return true;
452-
});
440+
"~/get_safety_status",
441+
std::bind(&DashboardClientROS::handleSafetyStatusQuery, this, std::placeholders::_1, std::placeholders::_2));
453442

454443
// Service to generate flight report, defaults to system type
455444
generate_flight_report_service_ = node->create_service<ur_dashboard_msgs::srv::GenerateFlightReport>(
@@ -572,6 +561,68 @@ bool DashboardClientROS::handleSafetyModeQuery(const ur_dashboard_msgs::srv::Get
572561
return true;
573562
}
574563

564+
bool DashboardClientROS::handleSafetyStatusQuery(const ur_dashboard_msgs::srv::GetSafetyStatus::Request::SharedPtr req,
565+
ur_dashboard_msgs::srv::GetSafetyStatus::Response::SharedPtr resp)
566+
{
567+
auto dashboard_response =
568+
dashboardCallWithChecks([this]() { return client_->commandSafetyStatusWithResponse(); }, resp);
569+
if (resp->success) {
570+
handleDashboardResponseData(
571+
[dashboard_response, resp]() {
572+
const std::string safety_status_str = std::get<std::string>(dashboard_response.data.at("safety_status"));
573+
{
574+
// Note: This list implements all safety status values. The Dashboard server
575+
// documentation doesn't name all of them, so some of them might not be used.
576+
if (safety_status_str == "NORMAL") {
577+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::NORMAL;
578+
} else if (safety_status_str == "REDUCED") {
579+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::REDUCED;
580+
} else if (safety_status_str == "PROTECTIVE_STOP") {
581+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::PROTECTIVE_STOP;
582+
} else if (safety_status_str == "RECOVERY") {
583+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::RECOVERY;
584+
} else if (safety_status_str == "SAFEGUARD_STOP") {
585+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::SAFEGUARD_STOP;
586+
} else if (safety_status_str == "SYSTEM_EMERGENCY_STOP") {
587+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::SYSTEM_EMERGENCY_STOP;
588+
} else if (safety_status_str == "ROBOT_EMERGENCY_STOP") {
589+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::ROBOT_EMERGENCY_STOP;
590+
} else if (safety_status_str == "VIOLATION") {
591+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::VIOLATION;
592+
} else if (safety_status_str == "FAULT") {
593+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::FAULT;
594+
} else if (safety_status_str == "VALIDATE_JOINT") {
595+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::VALIDATE_JOINT_ID;
596+
} else if (safety_status_str == "UNDEFINED_SAFETY_MODE") {
597+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::UNDEFINED_SAFETY_MODE;
598+
} else if (safety_status_str == "AUTOMATIC_MODE_SAFEGUARD_STOP") {
599+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::AUTOMATIC_MODE_SAFEGUARD_STOP;
600+
} else if (safety_status_str == "SYSTEM_THREE_POSITION_ENABLING_STOP") {
601+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::SYSTEM_THREE_POSITION_ENABLING_STOP;
602+
} else if (safety_status_str == "TP_THREE_POSITION_ENABLING_STOP") {
603+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::TP_THREE_POSITION_ENABLING_STOP;
604+
} else if (safety_status_str == "IMMI_EMERGENCY_STOP") {
605+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::IMMI_EMERGENCY_STOP;
606+
} else if (safety_status_str == "IMMI_SAFEGUARD_STOP") {
607+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::IMMI_SAFEGUARD_STOP;
608+
} else if (safety_status_str == "PROFISAFE_WAITING_FOR_PARAMETERS") {
609+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::PROFISAFE_WAITING_FOR_PARAMETERS;
610+
} else if (safety_status_str == "PROFISAFE_AUTOMATIC_MODE_SAFEGUARD_STOP") {
611+
resp->safety_status.status =
612+
ur_dashboard_msgs::msg::SafetyStatus::PROFISAFE_AUTOMATIC_MODE_SAFEGUARD_STOP;
613+
} else if (safety_status_str == "PROFISAFE_SAFEGUARD_STOP") {
614+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::PROFISAFE_SAFEGUARD_STOP;
615+
} else if (safety_status_str == "PROFISAFE_EMERGENCY_STOP") {
616+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::PROFISAFE_EMERGENCY_STOP;
617+
} else if (safety_status_str == "SAFETY_API_SAFEGUARD_STOP") {
618+
resp->safety_status.status = ur_dashboard_msgs::msg::SafetyStatus::SAFETY_API_SAFEGUARD_STOP;
619+
}
620+
}
621+
},
622+
resp, dashboard_response);
623+
}
624+
return true;
625+
}
575626
bool DashboardClientROS::handleRobotModeQuery(const ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req,
576627
ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp)
577628
{

0 commit comments

Comments
 (0)