def inputs_callback(self, data):
'''
Publish the input data through a topic
'''
msg = named_inputs_outputs()
msg.digital_inputs = []
for key in self._i_dict:
named_io = named_input_output()
named_io.name = key
named_io.value = data.digital_inputs[self._i_dict[key][0]['number']
- 1]
msg.digital_inputs.append(named_io)
self._pub.publish(msg)
def updateNamedIO(self): msg = named_inputs_outputs() for signal in self.inputs_: m = named_input_output() m.name = signal m.value = self.inputs_[signal]['value'] msg.digital_inputs.append(m) for signal in self.outputs_: m = named_input_output() m.name = signal m.value = self.outputs_[signal]['value'] msg.digital_outputs.append(m) self.named_io_msg = msg
def __init__(self, args):
self.node_name = rospy.get_name()
#.replace('/','')
self.desired_freq = args['desired_freq']
# Checks value of freq
if self.desired_freq <= 0.0 or self.desired_freq > MAX_FREQ:
rospy.loginfo('%s::init: Desired freq (%f) is not possible. Setting desired_freq to %f'%(self.node_name,self.desired_freq, DEFAULT_FREQ))
self.desired_freq = DEFAULT_FREQ
self.real_freq = 0.0
# Saves the state of the component
self.state = State.INIT_STATE
# Saves the previous state
self.previous_state = State.INIT_STATE
# flag to control the initialization of the component
self.initialized = False
# flag to control the initialization of ROS stuff
self.ros_initialized = False
# flag to control that the control loop is running
self.running = False
# Variable used to control the loop frequency
self.time_sleep = 1.0 / self.desired_freq
# State msg to publish
self.msg_state = State()
self.publish_state_timer = 1
self.lasers = args['lasers']
self.set_modbus_registers_service_name = args['set_modbus_registers_service_name']
self.set_digital_outputs_service_name = args['set_digital_outputs_service_name']
self.laser_mode_output_addr = args['address_registers/laser_mode_output']
self.current_speed_addr = args['address_registers/current_speed']
self.standby_output = args['outputs/standby'] # If set, laser changes mode to standby (to use less power when stop). It does not send any data in this mode
self.watchdog_signals_output = args['outputs/watchdog_signals'] #
self.watchdog_signals_frequency = args['watchdog_signals_frequency'] #
if self.watchdog_signals_frequency <= 0.0:
self.watchdog_signals_frequency = 1.0
self.watchdog_signals_period = 1.0/self.watchdog_signals_frequency
self.emergency_stop_input = args['inputs/emergency_stop'] # If set, e-stop is not pressed
self.safety_stop_input = args['inputs/safety_stop'] # If set, robot is stopped due to the safety system (either e-stop or obstacle inside protection area)
self.laser_mute_input = args['inputs/laser_mute'] # If set, safety mode is in emergency (elevator lowers, brakes are free, it can be pushed, key to the right)
self.laser_enabled_input = args['inputs/laser_enabled'] # If set, safety is enabled (key is to the left)
self.laser_on_standby_input = args['inputs/standby'] # If set, safety is overrided in safety overridable mode (key in the middle)
# structure to save inputs
self.inputs_ = {}
self.inputs_['wheels_power_enabled'] = {'number': args['inputs/wheels_power_enabled'], 'value': False}
self.inputs_['laser_ok'] = {'number': args['inputs/laser_ok'], 'value': False}
self.inputs_['edm_fault'] = {'number': args['inputs/edm_fault'], 'value': False}
self.inputs_['emergency_stop_fault'] = {'number': args['inputs/emergency_stop_fault'], 'value': False}
self.inputs_['motion_enabled'] = {'number': args['inputs/motion_enabled'], 'value': False}
self.inputs_['selector_fault'] = {'number': args['inputs/selector_fault'], 'value': False}
self.inputs_['watchdog_ok'] = {'number': args['inputs/watchdog_ok'], 'value': False}
self.inputs_['emergency_stop_sw'] = {'number': args['inputs/emergency_stop_sw'], 'value': False}
# structure to save laser modes
self.laser_modes_available_ = args['laser_modes']
if len(self.laser_modes_available_) == 0:
rospy.logwarn('%s::init: no laser modes defined!'%(self.node_name))
else:
for mode in self.laser_modes_available_:
if not self.laser_modes_available_[mode].has_key('input'):
rospy.logerr('%s::init: laser_modes format has to have the key input. Invalid format: %s', self.node_name, str(self.laser_modes_available_))
exit(-1)
if not self.laser_modes_available_[mode].has_key('output'):
rospy.logerr('%s::init: laser_modes format has to have the key output. Invalid format: %s', self.node_name, str(self.laser_modes_available_))
exit(-1)
# add field to save the input value
self.laser_modes_available_[mode]['input_value'] = False
# custom inputs
for ci in args['custom_inputs']:
if ci not in self.inputs_:
self.inputs_[ci] = {'number': args['custom_inputs'][ci], 'value': False}
else:
rospy.logerr('%s::init: custom input %s already defined as standard'%(self.node_name, ci))
if len(args['custom_inputs']) == 0:
rospy.logwarn('%s::init: no custom inputs defined'%(self.node_name))
# new structure to save the outputs
self.outputs_ = {}
self.outputs_['emergency_stop_sw'] = {'number': args['outputs/emergency_stop_sw'], 'value': False, 'desired_value': False}
# custom outputs
for co in args['custom_outputs']:
if co not in self.outputs_:
self.outputs_[co] = {'number': args['custom_outputs'][co], 'value': False, 'desired_value': False}
else:
rospy.logerr('%s::init: custom output %s already defined as standard'%(self.node_name, co))
if len(args['custom_outputs']) == 0:
rospy.logwarn('%s::init: no custom outputs defined'%(self.node_name))
self.set_speed_feedback = args['set_speed_feedback_to_safety_module'] # sets the speed into current_speed_addr
self.laser_mode = "unknown"
self.inputs_outputs_msg = inputs_outputs()
self.emergency_stop_msg = Bool()
self.safety_stop_msg = Bool()
self.last_enable_charge_msg_sent = False
self.laser_enabled = False
self.laser_mute = False
self.safety_mode = "unknown"
self.sm_status_msg = SafetyModuleStatus()
self.emergency_stop = False
self.safety_stop = False
self.safety_overrided = False
self.laser_on_standby = False
self.named_io_msg = named_inputs_outputs()
if len(self.watchdog_signals_output) == 2:
self.watchdog_signals_enabled = True
self.watchdog_signals_registers = [True, False]
#self.watchdog_frequency
rospy.loginfo('%s::init: watchdog signals is enabled! outputs %s'%(self.node_name, str(self.watchdog_signals_output)))
else:
self.watchdog_signals_enabled = False
rospy.logwarn('%s::init: watchdog signals is disabled! outputs %s'%(self.node_name, str(self.watchdog_signals_output)))
self.t_publish_state = threading.Timer(self.publish_state_timer, self.publishROSstate)
# current absolute speed in cm/s
self.current_speed = 0.0
# flag true if receiving speed
self.receiving_speed = False
# Saves time of last msg received
self.odom_last_stamp = rospy.Time(0)
# Saves time of last watchdog
self.watchdog_last_stamp = rospy.Time(0)
# saves the desired laser mode: by default no actions performed. Only on demand
self.set_desired_laser_mode = None
# saves the desired value of standby mode: by default no actions performed. Only on demand
self.set_desired_standby_mode = None