20 from enum
import IntEnum
23 from sensor_msgs.msg
import JointState
24 from diagnostic_msgs.msg
import DiagnosticStatus, DiagnosticArray
27 from cob_light.msg
import LightMode, LightModes, SetLightModeGoal, SetLightModeAction
28 from cob_light.srv
import StopLightMode, StopLightModeRequest
30 from simple_script_server
import *
54 if not rospy.has_param(
"~light_components"):
55 rospy.logwarn(
"parameter light_components does not exist on ROS Parameter Server, aborting...")
61 self.
color_warn = rospy.get_param(
"~color_warn",
"yellow")
62 self.
color_ok = rospy.get_param(
"~color_ok",
"green")
63 self.
color_off = rospy.get_param(
"~color_off",
"black")
66 if not rospy.has_param(
"~sound_components"):
67 rospy.logwarn(
"parameter sound_components does not exist on ROS Parameter Server, aborting...")
89 rospy.Subscriber(
"/diagnostics_toplevel_state", DiagnosticStatus, self.
diagnostics_callback, queue_size=1)
98 diagnostics_errors = []
99 diagnostics_errors_detail = []
102 for status
in msg.status:
103 if "Safety" in status.name:
104 if status.level > DiagnosticStatus.OK:
105 diagnostics_errors.append(status)
108 for i, status
in enumerate(diagnostics_errors):
109 tmp = diagnostics_errors[:i]+diagnostics_errors[i+1:]
110 if not next((
True for status2
in tmp
if status2.name.startswith(status.name)),
False):
111 diagnostics_errors_detail.append(status)
113 if not diagnostics_errors_detail:
116 if self.
em_state == EMState.EM_BUTTON:
118 elif self.
em_state == EMState.EM_BRAKE:
120 elif self.
em_state == EMState.EM_LASER:
122 elif self.
em_state == EMState.EM_WIRELESS:
124 elif self.
em_state == EMState.EM_FALL:
126 elif self.
em_state == EMState.EM_INTERNAL:
132 em_state = EMState.EM_UNKNOWN
133 for status
in diagnostics_errors_detail:
134 if "Safety" in status.name:
135 for value
in status.values:
136 if (value.key ==
"button_stop_ok"):
137 if (value.value ==
"False"):
138 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_BUTTON:
140 em_state = EMState.EM_BUTTON
141 if (value.key ==
"brake_stop_ok"):
142 if (value.value ==
"False"):
143 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_BRAKE:
145 em_state = EMState.EM_BRAKE
146 if (value.key ==
"laser_stop_ok"):
147 if (value.value ==
"False"):
148 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_LASER:
150 em_state = EMState.EM_LASER
151 if (value.key ==
"wireless_stop_ok"):
152 if (value.value ==
"False"):
153 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_WIRELESS:
155 em_state = EMState.EM_WIRELESS
156 if (value.key ==
"fall_sensor_front_ok"):
157 if (value.value ==
"False"):
158 if em_state <= EMState.EM_FREE
or self.
em_state > EMState.EM_FALL:
160 em_state = EMState.EM_FALL
161 if (value.key ==
"fall_sensor_left_ok"):
162 if (value.value ==
"False"):
163 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_FALL:
165 em_state = EMState.EM_FALL
166 if (value.key ==
"fall_sensor_right_ok"):
167 if (value.value ==
"False"):
168 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_FALL:
170 em_state = EMState.EM_FALL
171 if (value.key ==
"fall_sensors_released"):
172 if (value.value ==
"False"):
173 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_FALL:
175 em_state = EMState.EM_FALL
176 if (value.key ==
"efi_bus_front_io_error"):
177 if (value.value ==
"True"):
178 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_INTERNAL:
180 em_state = EMState.EM_INTERNAL
181 if (value.key ==
"efi_bus_left_io_error"):
182 if (value.value ==
"True"):
183 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_INTERNAL:
185 em_state = EMState.EM_INTERNAL
186 if (value.key ==
"efi_bus_right_io_error"):
187 if (value.value ==
"True"):
188 if em_state <= EMState.EM_FREE
or em_state > EMState.EM_INTERNAL:
190 em_state = EMState.EM_INTERNAL
192 rospy.logdebug(
"self.em_state: %d, em_state: %d"%(self.
em_state, em_state))
193 if self.
em_state != em_state
and em_state > EMState.EM_FREE:
196 self.
say(sound_output)
198 rospy.logdebug(
"self.em_state: %d"%(self.
em_state))
208 rospy.loginfo(
"Diagnostics change to "+ str(self.
diag_status))
210 if msg.level == DiagnosticStatus.OK:
227 for v
in msg.velocity:
228 if abs(v) > threshold:
246 error_code, color_rgba = sss.compose_color(component, color)
248 action_server_name = component +
"/set_light" 251 rospy.logdebug(
"waiting for %s action server to start",action_server_name)
252 if not client.wait_for_server(rospy.Duration(5)):
254 rospy.logerr(
"%s action server not ready within timeout, aborting...", action_server_name)
256 rospy.logdebug(
"%s action server ready",action_server_name)
262 mode.colors.append(color_rgba)
264 mode.mode = LightModes.FLASH
267 mode.mode = LightModes.GLOW
268 mode.frequency = 10.0
270 goal = SetLightModeGoal()
272 client.send_goal(goal)
273 client.wait_for_result()
274 result = client.get_result()
282 srv_server_name = component +
"/stop_mode" 284 rospy.wait_for_service(srv_server_name, timeout=2)
285 srv_proxy = rospy.ServiceProxy(srv_server_name, StopLightMode)
286 req = StopLightModeRequest()
289 except Exception
as e:
290 rospy.logerr(
"%s service failed: %s",srv_server_name, e)
295 sss.say(component, [text])
297 if __name__ ==
"__main__":
298 rospy.init_node(
"emergency_stop_monitor")
def diagnostics_agg_callback(self, msg)
sound_em_internal_released
def diagnostics_callback(self, msg)
Diagnostics monitoring.
sound_em_wireless_released
def jointstate_callback(self, msg)
Motion Monitoring.
def set_light(self, color, flashing=False)
set light