def init_state(self):
self.pan_pos = 0 # deg
self.tilt_pos = 0 # deg
self.pan_speed = 0 # deg
self.tilt_speed = 0 # deg
self.last_ptz_msg = ptz()
self.last_reset_axis = rospy.Time(0)
self.status = PantiltStatus()
self.status.pan_pos = self.pan_pos
self.status.tilt_pos = self.tilt_pos
self.max_pan_pos = 168.00 # deg
self.min_pan_pos = -167.99 # deg
self.max_tilt_pos = 30.00 # deg
self.min_tilt_pos = -89.99 # deg
# Pantilt encoders resolution (deg/pos)
if (self.ptu_model == "PTU-5"):
self.pan_resolution = 0.05
self.tilt_resolution = 0.05
elif (self.ptu_model == "PTU-D48E"):
self.pan_resolution = 0.025714
self.tilt_resolution = 0.012857
else:
rospy.logerr('%s:init_state: %s is not a valid model' % (rospy.get_name(), self.ptu_model))
self.switch_to_state(State.FAILURE_STATE)
return RComponent.init_state(self)
def homeService(self, req):
# Set home values
home_command = ptz()
home_command.relative = False
home_command.pan = self.home_pan_value
home_command.tilt = self.home_tilt_value
home_command.zoom = 0
self.setCommandPTZ(home_command)
return {}
def homeService(self, req):
# Set home values
home_command = ptz()
home_command.relative = False
home_command.pan = self.home_pan_value
home_command.tilt = self.home_tilt_value
home_command.zoom = 0
self.setCommandPTZ(home_command)
return {}
def __init__(self, args): self.enable_auth = args['enable_auth'] self.hostname = args['hostname'] self.username = args['username'] self.password = args['password'] self.rate = args['ptz_rate'] self.autoflip = args['autoflip'] self.eflip = args['eflip'] self.eflip = args['eflip'] self.tilt_joint = args['tilt_joint'] self.pan_joint = args['pan_joint'] self.min_pan_value = args['min_pan_value'] self.max_pan_value = args['max_pan_value'] self.min_tilt_value = args['min_tilt_value'] self.max_tilt_value = args['max_tilt_value'] self.min_zoom_value = args['min_zoom_value'] self.max_zoom_value = args['max_zoom_value'] self.home_pan_value = args['home_pan_value'] self.home_tilt_value = args['home_tilt_value'] self.error_pos = args['error_pos'] self.error_zoom = args['error_zoom'] self.joint_states_topic = args['joint_states_topic'] self.use_control_timeout = args['use_control_timeout'] self.control_timeout_value = args['control_timeout_value'] self.invert_ptz = args['invert_ptz'] self.initialization_delay = args['initialization_delay'] self.current_ptz = AxisMsg() self.last_msg = ptz() threading.Thread.__init__(self) self.daemon = True self.run_control = True # Flag to know if the current params of the camera has been read self.ptz_syncronized = False # used in control position (degrees) self.desired_pan = 0.0 self.desired_tilt = 0.0 self.desired_zoom = 0.0 self.error_reading = False self.error_reading_msg = '' # Timer to get/release ptz control if(self.use_control_timeout): self.last_command_time = rospy.Time(0) self.command_timeout = rospy.Duration(self.control_timeout_value)