@@ -652,27 +652,29 @@ void HardwareInterface::extractRobotStatus()
652652{
653653 using namespace rtde_interface ;
654654
655- robot_status_resource_.mode =
656- robot_status_bits_[UrRtdeRobotStatusBits::IS_TEACH_BUTTON_PRESSED] ? RobotMode::MANUAL : RobotMode::AUTO;
655+ robot_status_resource_.mode = robot_status_bits_[toUnderlying (UrRtdeRobotStatusBits::IS_TEACH_BUTTON_PRESSED)] ?
656+ RobotMode::MANUAL :
657+ RobotMode::AUTO;
657658
658- robot_status_resource_.e_stopped =
659- safety_status_bits_[UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED] ? TriState::TRUE : TriState::FALSE ;
659+ robot_status_resource_.e_stopped = safety_status_bits_[toUnderlying (UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED)] ?
660+ TriState::TRUE :
661+ TriState::FALSE ;
660662
661663 // Note that this is true as soon as the drives are powered,
662664 // even if the breakes are still closed
663665 // which is in slight contrast to the comments in the
664666 // message definition
665667 robot_status_resource_.drives_powered =
666- robot_status_bits_[UrRtdeRobotStatusBits::IS_POWER_ON] ? TriState::TRUE : TriState::FALSE ;
668+ robot_status_bits_[toUnderlying ( UrRtdeRobotStatusBits::IS_POWER_ON) ] ? TriState::TRUE : TriState::FALSE ;
667669
668670 robot_status_resource_.motion_possible =
669671 robot_mode_ == ur_dashboard_msgs::RobotMode::RUNNING ? TriState::TRUE : TriState::FALSE ;
670672
671673 // I found no way to reliably get information if the robot is moving
672674 robot_status_resource_.in_motion = TriState::UNKNOWN;
673675
674- if (safety_status_bits_[UrRtdeSafetyStatusBits::IS_PROTECTIVE_STOPPED] ||
675- safety_status_bits_[UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED])
676+ if (safety_status_bits_[toUnderlying ( UrRtdeSafetyStatusBits::IS_PROTECTIVE_STOPPED) ] ||
677+ safety_status_bits_[toUnderlying ( UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED) ])
676678 {
677679 robot_status_resource_.in_error = TriState::TRUE ;
678680 }
0 commit comments