def __init__(self, args):
self._node_name = rospy.get_name()
self.rosReadParams()
self.timer_threshold = args['timer_threshold']
self.rosmon_service = args['rosmon_service']
self.rostful_client_node = args['rostful_client_node']
self.monitored_topic = args['monitored_topic']
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()
# Timer to publish state
self._publish_state_timer = 1
self.t_publish_state = threading.Timer(self._publish_state_timer,
self.publishROSstate)
self.last_message_time = rospy.Time.now()
rospy.Timer(rospy.Duration(5), self.check_timer_callback)
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()
# Timer to publish state
self.publish_state_timer = 1
self.start_slam_gmapping = False
self.start_amcl = False
self.start_robot_ctrller = False
self.start_move_base = False
self.start_teleop = False
self.start_map_server = False
self.is_slam_gmapping = False
self.is_amcl = False
self.is_map_server = False
self.is_move_base = False
self.is_robot_ctrllr = False
self.is_teleop = False
self.slam_gmapping_process_name = ""
self.amcl_process_name = ""
self.map_server_process_name = ""
self.move_base_process_name = ""
self.robot_ctrllr_process_name = ""
self.teleop_process_name = ""
self.t_publish_state = threading.Timer(
self.publish_state_timer,
self.publishROSstate) #Each second publish state
def __init__(self, args):
self.node_name = rospy.get_name().replace('/', '')
self.host_ = args['host']
self.timeout_ = args['timeout']
self.count_ = args['count']
# desired freq has to be equal to the max time to get the ping result
self.desired_freq = DEFAULT_FREQ
if self.timeout_ < 0.0:
rospy.loginfo(
'%s::init: timeout (%f) is not possible. Setting timeout to %f'
% (self.node_name, self.timeout_, MIN_TIMEOUT))
self.timeout_ = MIN_TIMEOUT
if self.count_ <= 0:
rospy.loginfo(
'%s::init: count (%f) is not possible. Setting count to %f' %
(self.node_name, self.count_, MIN_COUNT))
self.count_ = MIN_COUNT
# 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, MAX_FREQ))
self.desired_freq = MAX_FREQ
# desired freq has to be equal to the max time to get the ping result
self.desired_freq = max(0.00001, 1.0 / (self.timeout_ * self.count_))
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()
# Timer to publish state
self.publish_state_timer = 1
self.ping_status = PingStatus()
self.ping_status.count = self.count_
self.ping_status.timeout = self.timeout_
self.ping_status.host = self.host_
self.t_publish_state = threading.Timer(self.publish_state_timer,
self.publishROSstate)
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:
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
if 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, MAX_FREQ))
self.desired_freq = MAX_FREQ
self.real_freq = 0.0
# Saves the state of the component
self.state = None # State.INIT_STATE # is it better to use the switchToState because it is printed to log
# # Saves the previous state
self.previous_state = None # 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
# Timer to publish state
self.publish_state_timer = 1
self.port = args['port']
self.serial_device = None
self.read_errors = 0
self.command_stop = 'C100000000000000\r'
self.command_start = 'C010000000000000\r'
self.command_query = 'Q\r'
self.query_response_size = 61 # message is 60 bytes + 1 of line return
rospy.loginfo('%s:init: Serial port = %s', self.node_name, self.port)
self.state_msg = State()
self.inverter_status_msg = InverterStatus()
self.switchToState(State.INIT_STATE)
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:
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
if 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, MAX_FREQ))
self.desired_freq = MAX_FREQ
self.real_freq = 0.0
# Saves the state of the component
self.state = None # State.INIT_STATE # is it better to use the switchToState because it is printed to log
# # Saves the previous state
self.previous_state = None # State.INIT_STATE
self.design_capacity = 50
#self.design_capacity = args['design_capacity']
#if self.design_capacity <= 0.0:
# rospy.loginfo('%s::init: Design capacity (%f) is not possible. Setting design capacity to %f'%(self.node_name,self.design_capacity, DESIGN_CAPACITY))
# self.desired_freq = DESIGN_CAPACITY
# 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
# Timer to publish state
self.publish_state_timer = 1
self.port = args['port']
self.serial_device = None
self.read_errors = 0
rospy.loginfo('%s:init: Serial port = %s', self.node_name, self.port)
self.state_msg = State()
self.battery_monitor_status_msg = BatteryState()
self.battery_alarm = 'ON'
self.switchToState(State.INIT_STATE)
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()
# Timer to publish state
self.publish_state_timer = 1
self._action_feedback = rostful_tests.msg.WaitFeedback()
self._action_result = rostful_tests.msg.WaitResult()
self._action_goal = rostful_tests.msg.WaitActionGoal()
self._action_init_time = rospy.Time.now()
self.t_publish_state = threading.Timer(self.publish_state_timer,
self.publishROSstate)
self._action_name = rospy.get_name() + '/wait'
def __init__(self, args):
self.node_name = rospy.get_name() #.replace('/','')
self.desired_freq = args['desired_freq']
self.digital_output = args['digital_output']
self.service_io = args['service_io']
# 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
# Variable to control the sound time
self.time_enabled = float("inf")
# State msg to publish
self.msg_state = State()
# Timer to publish state
self.publish_state_timer = 1
# Variable of buzzer
self.buzzer_enable = False
self.buzzer_freq = 1
self.buzzer_called = True
def __init__(self):
self._node_name = rospy.get_name()
self.rosReadParams()
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()
# Timer to publish state
self._publish_state_timer = 1
self._t_publish_state = threading.Timer(self._publish_state_timer,
self.publishROSstate)
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
def __init__(self, args):
self.node_name = rospy.get_name() #.replace('/','')
self.desired_freq = args['desired_freq']
self.desired_bias = args['desired_bias']
self.desired_stoppedtime = args['desired_stoppedtime']
self.desired_gyrotime = args['desired_gyrotime']
self.desired_standbytime = args['desired_standbytime']
self.desired_temperaturevariation = args[
'desired_temperaturevariation']
self.calibrationEnds = True
self.temperature = 0.0
self.bias = [0, 0]
rospack = rospkg.RosPack()
self.path = rospack.get_path('imu_calibrator')
# 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
if self.desired_bias <= 0.0:
rospy.loginfo(
'%s::init: Desired bias (%f) is not possible. Setting desired_bias to %f'
% (self.node_name, self.desired_bias, DEFAULT_BIAS))
self.desired_bias = DEFAULT_BIAS
if self.desired_stoppedtime <= 0.0:
rospy.loginfo(
'%s::init: Desired stopped time (%f) is not possible. Setting desired_stoppedtime to %f'
% (self.node_name, self.desired_stoppedtime,
DEFAULT_STOPPEDTIME))
self.desired_stoppedtime = DEFAULT_STOPPEDTIME
if self.desired_gyrotime <= 0.0:
rospy.loginfo(
'%s::init: Desired gyro time (%f) is not possible. Setting desired_gyrotime to %f'
% (self.node_name, self.desired_gyrotime, DEFAULT_GYROTIME))
self.desired_gyrotime = DEFAULT_GYROTIME
if self.desired_standbytime <= 0.0:
rospy.loginfo(
'%s::init: Desired standby time (%f) is not possible. Setting desired_standbytime to %f'
% (self.node_name, self.desired_standbytime,
DEFAULT_STANDBYTIME))
self.desired_standbytime = DEFAULT_STANDBYTIME
if self.desired_temperaturevariation <= 0.0:
rospy.loginfo(
'%s::init: Desired temperature variation (%f) is not possible. Setting desired_temperaturevariation to %f'
% (self.node_name, self.desired_temperaturevariation,
DEFAULT_TEMPERATUREVARIATION))
self.desired_temperaturevariation = DEFAULT_TEMPERATUREVARIATION
self.status = CalibratorStatus()
self.status.state = "idle"
self.status.remaining_time = self.desired_stoppedtime
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()
# Timer to publish state
self.publish_state_timer = 1
# The robot is changing his position
self.mov = True
# Position of the robot (linear.x, angular.z)
self.pos = [0, 0, 0, 0, 0, 0]
# The robot needs calibration
self.calibrate = False
# Last inclination of the robot
self.incMin = [100, 100, 100]
self.incMax = [-100, -100, -100]
# True if the robot has been stopped the required time
self.checkedMovement = False
# Time when the search has started
self.t1 = 0.0
def __init__(self, args):
self.node_name = rospy.get_name() #.replace('/','')
self.desired_freq = args['desired_freq']
# saves the state of all the gazebo models related to robots
self._gazebo_robots = {}
# saves the state of all the gazebo models related to pickable objects
self._gazebo_objects = {}
'''
expected format:
-
model: rb2cart
default_link: link_0
'''
self._robot_models_config = args['robots']
if self._robot_models_config is None or len(
self._robot_models_config) == 0:
rospy.logerr('%s::__init__: param robots is mandatory',
self.node_name)
exit()
for cfg in self._robot_models_config:
default_link = ''
model = ''
if cfg.has_key('default_link') and len(cfg['default_link']) > 0:
default_link = cfg['default_link']
else:
rospy.logerr(
'%s::__init__: param default_link has to be defined for every model: %s',
self.node_name, str(cfg))
exit()
if cfg.has_key('model') and len(cfg['model']) > 0:
model = cfg['model']
else:
rospy.logerr(
'%s::__init__: param model has to be defined for every model: %s',
self.node_name, str(cfg))
exit()
if cfg.has_key('elevation_z'):
elevation_z = cfg['elevation_z']
else:
rospy.logerr(
'%s::__init__: param elevation_z has to be defined for every model: %s',
self.node_name, str(cfg))
exit()
self._gazebo_robots[model] = {
'model': GazeboModelState(),
'links': {},
'default_link': '%s::%s' % (model, default_link),
'elevation_z': elevation_z
}
self._object_models_config = args['objects']
if self._object_models_config is None or len(
self._object_models_config) == 0:
rospy.logerr('%s::__init__: param objects is mandatory',
self.node_name)
exit()
for cfg in self._object_models_config:
default_link = ''
model = ''
if cfg.has_key('default_link') and len(cfg['default_link']) > 0:
default_link = cfg['default_link']
else:
rospy.logerr(
'%s::__init__: param default_link has to be defined for every model: %s',
self.node_name, str(cfg))
exit()
if cfg.has_key('model') and len(cfg['model']) > 0:
model = cfg['model']
else:
rospy.logerr(
'%s::__init__: param model has to be defined for every model: %s',
self.node_name, str(cfg))
exit()
self._gazebo_objects[model] = {
'model': GazeboModelState(),
'links': {},
'default_link': '%s::%s' % (model, default_link)
}
self._global_config = args['config']
if self._global_config is None or len(self._object_models_config) == 0:
rospy.logwarn(
'%s::__init__: no config defined. Setting default params',
self.node_name)
self._global_config = {}
self._global_config[
'min_picking_distance'] = DEFAULT_MIN_PICKING_DISTANCE
rospy.logwarn(
'%s::__init__: Setting min_picking_distance to default value: %.3lf',
self.node_name, self._global_config['min_picking_distance'])
else:
if not self._global_config.has_key('min_picking_distance'):
self._global_config[
'min_picking_distance'] = DEFAULT_MIN_PICKING_DISTANCE
rospy.logwarn(
'%s::__init__: Setting min_picking_distance to default value: %.3lf',
self.node_name,
self._global_config['min_picking_distance'])
# 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()
# Timer to publish state
self.publish_state_timer = 1
self.t_publish_state = threading.Timer(self.publish_state_timer,
self.publishROSstate)
# save the current links/picking between robot & objects (GazeboPickAndPlace)
self._current_picks = {}
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._motors = args['motors']
print self._motors
#exit()
#self._joint_names = args['joint_name']
#self._joint_can_ids = args['joint_can_id']
if self._motors == None or len(self._motors) == 0:
rospy.logerr('%s::init: no motors defined'%(self.node_name))
exit()
self._battery_voltage = args['battery_voltage']
if len(self._battery_voltage) == 0:
raise('battery_voltage argument has to be set')
self._current_battery_voltage = self._battery_voltage[0][0]
self._battery_amperes = args['battery_amperes']
self._current_battery_amperes = self._battery_amperes
self._battery_alarm_voltage = args['battery_alarm_voltage']
self._num_inputs_per_driver = args['num_inputs_per_driver']
self._num_analog_inputs_per_driver = args['num_analog_inputs_per_driver']
self._num_analog_outputs_per_driver = args['num_analog_outputs_per_driver']
self._num_outputs_per_driver = args['num_outputs_per_driver']
self._power_consumption = args['power_consumption']
if self._power_consumption <= 0:
raise("power_consumption argument has to be > 0")
self._current_power_consumption = self._power_consumption
self._power_charge = args['power_charge']
if self._power_charge <= 0:
raise("power_charge argument has to be > 0")
self._k_analog_inputs_multipliers = args['k_analog_inputs_multipliers']
self._voltage_analog_input_number = args['voltage_analog_input_number']
self._current_analog_input_number = args['current_analog_input_number']
self._charge_digital_output_number = args['charge_digital_output_number']
self.real_freq = 1.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()
# Timer to publish state
self.publish_state_timer = 1
self.t_publish_state = threading.Timer(self.publish_state_timer, self.publishROSstate)
self._io = inputs_outputs()
for i in range(len(self._motors)*self._num_inputs_per_driver):
self._io.digital_inputs.append(False)
for i in range(len(self._motors)*self._num_outputs_per_driver):
self._io.digital_outputs.append(False)
for i in range(len(self._motors)*self._num_analog_inputs_per_driver):
self._io.analog_inputs.append(0.0)
for i in range(len(self._motors)*self._num_analog_outputs_per_driver):
self._io.analog_outputs.append(0.0)
if len(self._k_analog_inputs_multipliers) != len(self._io.analog_inputs):
rospy.logwarn('%s::__init__: len of param k_analog_inputs_multipliers is different from the len of analog inputs',self.node_name)
self._k_analog_inputs_multipliers = [1] * len(self._io.analog_inputs)
if self._voltage_analog_input_number <= 0 or self._voltage_analog_input_number > len(self._io.analog_inputs):
rospy.logerr('%s::__init__: voltage_analog_input_number (%d) out of range [1, %d].',self.node_name, self._voltage_analog_input_number, len(self._io.analog_inputs))
sys.exit()
if self._current_analog_input_number <= 0 or self._current_analog_input_number > len(self._io.analog_inputs):
rospy.logerr('%s::__init__: current_analog_input_number (%d) out of range [1, %d].',self.node_name, self._current_analog_input_number, len(self._io.analog_inputs))
sys.exit()
if self._charge_digital_output_number <= 0 or self._charge_digital_output_number > len(self._io.digital_outputs):
rospy.logerr('%s::__init__: charge_digital_output_number (%d) out of range [1, %d].',self.node_name, self._charge_digital_output_number, len(self._io.digital_outputs))
sys.exit()
self._motor_status = RobotnikMotorsStatus()
for i in self._motors:
ms = MotorStatus()
ms.state = "READY"
ms.joint = self._motors[i]['joint']
ms.can_id = self._motors[i]['can_id']
ms.status = "OPERATION_ENABLED"
ms.communicationstatus = "OPERATIONAL"
ms.statusword = "1110110001100000"
ms.driveflags = "10000000000000000000000000000000000010000110000000000000000000000000"
ms.activestatusword = ['SW_READY_TO_SWITCH_ON', 'SW_SWITCHED_ON', 'SW_OP_ENABLED', 'SW_VOLTAGE_ENABLED', 'SW_QUICK_STOP',
'UNKNOWN', 'SW_TARGET_REACHED']
ms.activedriveflags = ['BRIDGE_ENABLED', 'NONSINUSOIDAL_COMMUTATION', 'ZERO_VELOCITY', 'AT_COMMAND']
ms.digitaloutputs = [False for i in range(self._num_inputs_per_driver)]
ms.digitalinputs = [False for i in range(self._num_outputs_per_driver)]
ms.analoginputs = [0.0 for i in range(self._num_analog_inputs_per_driver)]
self._motor_status.motor_status.append(ms)
self._battery_alarm = False
self._emergency_stop = False