def __init__(self, *args, **kwargs):
rospy.loginfo('Initializing zaber_stage_node...')
self._initialized = False
self._rate = rospy.Rate(4)
self._cmd_vel_sub = rospy.Subscriber('~cmd_vel', Twist,
self._cmd_vel_callback)
self._stop_x_sub = rospy.Subscriber('~stop_x', Empty,
self._stop_x_callback)
self._stop_y_sub = rospy.Subscriber('~stop_y', Empty,
self._stop_y_callback)
self._stop_z_sub = rospy.Subscriber('~stop_z', Empty,
self._stop_z_callback)
self._stop_sub = rospy.Subscriber('~stop', Empty, self._stop_callback)
self._get_pose_srv = rospy.Service('~get_pose', GetPose,
self._get_pose_callback)
self._moving_srv = rospy.Service('~moving', Moving,
self._moving_callback)
self._get_pose_and_debug_info_srv = rospy.Service(
'~get_pose_and_debug_info', GetPoseAndDebugInfo,
self._get_pose_and_debug_info_callback)
self._home_action = actionlib.SimpleActionServer(
'~home', EmptyAction, self._home_callback, False)
self._move_relative_action = actionlib.SimpleActionServer(
'~move_relative', MoveAction, self._move_relative_callback, False)
self._move_absolute_action = actionlib.SimpleActionServer(
'~move_absolute', MoveAction, self._move_absolute_callback, False)
self._move_relative_percent_action = actionlib.SimpleActionServer(
'~move_relative_percent', MoveAction,
self._move_relative_percent_callback, False)
self._move_absolute_percent_action = actionlib.SimpleActionServer(
'~move_absolute_percent', MoveAction,
self._move_absolute_percent_callback, False)
x_serial_number = rospy.get_param('~x_serial_number', None)
if x_serial_number == '':
x_serial_number = None
x_alias = rospy.get_param('~x_alias', None)
if x_alias == '':
x_alias = None
x_microstep_size = rospy.get_param('~x_microstep_size', 1)
x_travel = rospy.get_param('~x_travel', None)
if x_travel == '':
x_travel = None
y_serial_number = rospy.get_param('~y_serial_number', None)
if y_serial_number == '':
y_serial_number = None
y_alias = rospy.get_param('~y_alias', None)
if y_alias == '':
y_alias = None
y_microstep_size = rospy.get_param('~y_microstep_size', 1)
y_travel = rospy.get_param('~y_travel', None)
if y_travel == '':
y_travel = None
z_serial_number = rospy.get_param('~z_serial_number', None)
if z_serial_number == '':
z_serial_number = None
z_alias = rospy.get_param('~z_alias', None)
if z_alias == '':
z_alias = None
z_microstep_size = rospy.get_param('~z_microstep_size', 1)
z_travel = rospy.get_param('~z_travel', None)
if z_travel == '':
z_travel = None
axis_set = False
if (x_serial_number is not None) and (x_alias is not None):
axis_set = True
if (y_serial_number is not None) and (y_alias is not None):
axis_set = True
if (z_serial_number is not None) and (z_alias is not None):
axis_set = True
if not axis_set:
err_str = "Not enough zaber_stage_node axis arguments specified! (x_serial_number,x_alias,y_serial_number,y_alias,z_serial_number,z_alias)"
rospy.signal_shutdown(err_str)
rospy.logerr(err_str)
else:
try:
serial_port = rospy.get_param('~serial_port', None)
if (serial_port is not None) and (len(serial_port) > 0):
self._stage = ZaberStage(use_ports=[serial_port])
else:
self._stage = ZaberStage()
if (x_serial_number is not None) and (x_alias is not None):
self._stage.set_x_axis(x_serial_number, x_alias)
if (y_serial_number is not None) and (y_alias is not None):
self._stage.set_y_axis(y_serial_number, y_alias)
if (z_serial_number is not None) and (z_alias is not None):
self._stage.set_z_axis(z_serial_number, z_alias)
self._stage.set_x_microstep_size(x_microstep_size)
self._stage.set_y_microstep_size(y_microstep_size)
self._stage.set_z_microstep_size(z_microstep_size)
if x_travel is not None:
self._stage.set_x_travel(x_travel)
if y_travel is not None:
self._stage.set_y_travel(y_travel)
if z_travel is not None:
self._stage.set_z_travel(z_travel)
rospy.loginfo('zaber_stage_node initialized!')
self._home_action.start()
self._move_relative_action.start()
self._move_absolute_action.start()
self._move_relative_percent_action.start()
self._move_absolute_percent_action.start()
self._initialized = True
except ZaberError:
rospy.logwarn('zaber_stage_node init ZaberError')