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:
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
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')
class ZaberStageController(object):
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:
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
def _cmd_vel_callback(self, data):
if self._initialized:
x_vel = data.linear.x
y_vel = data.linear.y
z_vel = data.linear.z
self._stage.move_x_at_speed(x_vel)
self._stage.move_y_at_speed(y_vel)
self._stage.move_z_at_speed(z_vel)
def _stop_x_callback(self, data):
if self._initialized:
self._stage.stop_x()
def _stop_y_callback(self, data):
if self._initialized:
self._stage.stop_y()
def _stop_z_callback(self, data):
if self._initialized:
self._stage.stop_z()
def _stop_callback(self, data):
if self._initialized:
self.stop()
def stop(self):
if self._initialized:
self._stage.stop_x()
self._stage.stop_y()
self._stage.stop_z()
def _get_pose_callback(self, req):
while not self._initialized:
self._rate.sleep()
pose = Pose()
positions = self._stage.get_positions()
pose.position.x = positions[0]
pose.position.y = positions[1]
pose.position.z = positions[2]
return GetPoseResponse(pose)
def _moving_callback(self, req):
while not self._initialized:
self._rate.sleep()
res = MovingResponse()
moving = self._stage.moving()
res.x = moving[0]
res.y = moving[1]
res.z = moving[2]
return res
def _get_pose_and_debug_info_callback(self, req):
while not self._initialized:
self._rate.sleep()
res = GetPoseAndDebugInfoResponse()
positions_and_debug = self._stage.get_positions_and_debug_info()
res.pose_and_debug_info.pose.position.x = positions_and_debug["position"][0]
res.pose_and_debug_info.pose.position.y = positions_and_debug["position"][1]
res.pose_and_debug_info.pose.position.z = positions_and_debug["position"][2]
res.pose_and_debug_info.pose_microstep.position.x = positions_and_debug["position_microstep"][0]
res.pose_and_debug_info.pose_microstep.position.y = positions_and_debug["position_microstep"][1]
res.pose_and_debug_info.pose_microstep.position.z = positions_and_debug["position_microstep"][2]
res.pose_and_debug_info.zaber_response = positions_and_debug["response"]
return res
def _home_callback(self, req):
while not self._initialized:
self._rate.sleep()
self._stage.home()
rospy.sleep(2)
finished = False
while not finished:
homed = self._stage.homed()
print(homed)
finished = all(homed)
if not finished:
self._rate.sleep()
self._home_action.set_succeeded()
def _move_relative_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo(
"move_relative: x={0}, y={1}, z={2}".format(req.pose.position.x, req.pose.position.y, req.pose.position.z)
)
self._stage.move_x_relative(req.pose.position.x)
self._stage.move_y_relative(req.pose.position.y)
self._stage.move_z_relative(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_relative_action.set_succeeded()
def _move_absolute_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo(
"move_absolute: x={0}, y={1}, z={2}".format(req.pose.position.x, req.pose.position.y, req.pose.position.z)
)
self._stage.move_x_absolute(req.pose.position.x)
self._stage.move_y_absolute(req.pose.position.y)
self._stage.move_z_absolute(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_absolute_action.set_succeeded()
def _move_relative_percent_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo(
"move_relative_percent: x={0}%, y={1}%, z={2}%".format(
req.pose.position.x, req.pose.position.y, req.pose.position.z
)
)
self._stage.move_x_relative_percent(req.pose.position.x)
self._stage.move_y_relative_percent(req.pose.position.y)
self._stage.move_z_relative_percent(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_relative_percent_action.set_succeeded()
def _move_absolute_percent_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo(
"move_absolute_percent: x={0}%, y={1}%, z={2}%".format(
req.pose.position.x, req.pose.position.y, req.pose.position.z
)
)
self._stage.move_x_absolute_percent(req.pose.position.x)
self._stage.move_y_absolute_percent(req.pose.position.y)
self._stage.move_z_absolute_percent(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_absolute_percent_action.set_succeeded()
class ZaberStageController(object):
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')
def _cmd_vel_callback(self, data):
if self._initialized:
try:
x_vel = data.linear.x
y_vel = data.linear.y
z_vel = data.linear.z
self._stage.move_x_at_speed(x_vel)
self._stage.move_y_at_speed(y_vel)
self._stage.move_z_at_speed(z_vel)
except ZaberError:
rospy.logwarn('zaber_stage_node _cmd_vel_callback ZaberError')
def _stop_x_callback(self, data):
if self._initialized:
try:
self._stage.stop_x()
except ZaberError:
rospy.logwarn('zaber_stage_node _stop_x_callback ZaberError')
def _stop_y_callback(self, data):
if self._initialized:
try:
self._stage.stop_y()
except ZaberError:
rospy.logwarn('zaber_stage_node _stop_y_callback ZaberError')
def _stop_z_callback(self, data):
if self._initialized:
try:
self._stage.stop_z()
except ZaberError:
rospy.logwarn('zaber_stage_node _stop_z_callback ZaberError')
def _stop_callback(self, data):
if self._initialized:
self.stop()
def stop(self):
if self._initialized:
try:
self._stage.stop_x()
self._stage.stop_y()
self._stage.stop_z()
except ZaberError:
rospy.logwarn('zaber_stage_node stop ZaberError')
def _get_pose_callback(self, req):
while not self._initialized:
self._rate.sleep()
pose = Pose()
try:
positions = self._stage.get_positions()
pose.position.x = positions[0]
pose.position.y = positions[1]
pose.position.z = positions[2]
except ZaberError:
rospy.logwarn('zaber_stage_node _get_pose_callback ZaberError')
return GetPoseResponse(pose)
def _moving_callback(self, req):
while not self._initialized:
self._rate.sleep()
res = MovingResponse()
try:
moving = self._stage.moving()
res.x = moving[0]
res.y = moving[1]
res.z = moving[2]
except ZaberError:
rospy.logwarn('zaber_stage_node _moving_callback ZaberError')
return res
def _get_pose_and_debug_info_callback(self, req):
while not self._initialized:
self._rate.sleep()
res = GetPoseAndDebugInfoResponse()
try:
positions_and_debug = self._stage.get_positions_and_debug_info()
res.pose_and_debug_info.pose.position.x = positions_and_debug[
'position'][0]
res.pose_and_debug_info.pose.position.y = positions_and_debug[
'position'][1]
res.pose_and_debug_info.pose.position.z = positions_and_debug[
'position'][2]
res.pose_and_debug_info.pose_microstep.position.x = positions_and_debug[
'position_microstep'][0]
res.pose_and_debug_info.pose_microstep.position.y = positions_and_debug[
'position_microstep'][1]
res.pose_and_debug_info.pose_microstep.position.z = positions_and_debug[
'position_microstep'][2]
res.pose_and_debug_info.zaber_response = positions_and_debug[
'response']
res.pose_and_debug_info.zaber_response_time = positions_and_debug[
'response_time']
res.pose_and_debug_info.zaber_response_error = False
except ZaberError:
rospy.logwarn(
'zaber_stage_node _get_pose_and_debug_info_callback ZaberError'
)
res.pose_and_debug_info.zaber_response_error = True
return res
def _home_callback(self, req):
while not self._initialized:
self._rate.sleep()
try:
self._stage.home()
rospy.sleep(2)
finished = False
while not finished:
homed = self._stage.homed()
print(homed)
finished = all(homed)
if not finished:
self._rate.sleep()
self._home_action.set_succeeded()
except ZaberError:
rospy.logwarn('zaber_stage_node _home_callback ZaberError')
def _move_relative_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo("move_relative: x={0}, y={1}, z={2}".format(
req.pose.position.x, req.pose.position.y, req.pose.position.z))
try:
self._stage.move_x_relative(req.pose.position.x)
self._stage.move_y_relative(req.pose.position.y)
self._stage.move_z_relative(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_relative_action.set_succeeded()
except ZaberError:
rospy.logwarn(
'zaber_stage_node _move_relative_callback ZaberError')
def _move_absolute_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo("move_absolute: x={0}, y={1}, z={2}".format(
req.pose.position.x, req.pose.position.y, req.pose.position.z))
try:
self._stage.move_x_absolute(req.pose.position.x)
self._stage.move_y_absolute(req.pose.position.y)
self._stage.move_z_absolute(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_absolute_action.set_succeeded()
except ZaberError:
rospy.logwarn(
'zaber_stage_node _move_absolute_callback ZaberError')
def _move_relative_percent_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo("move_relative_percent: x={0}%, y={1}%, z={2}%".format(
req.pose.position.x, req.pose.position.y, req.pose.position.z))
try:
self._stage.move_x_relative_percent(req.pose.position.x)
self._stage.move_y_relative_percent(req.pose.position.y)
self._stage.move_z_relative_percent(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_relative_percent_action.set_succeeded()
except ZaberError:
rospy.logwarn(
'zaber_stage_node _move_relative_percent_callback ZaberError')
def _move_absolute_percent_callback(self, req):
while not self._initialized:
self._rate.sleep()
rospy.loginfo("move_absolute_percent: x={0}%, y={1}%, z={2}%".format(
req.pose.position.x, req.pose.position.y, req.pose.position.z))
try:
self._stage.move_x_absolute_percent(req.pose.position.x)
self._stage.move_y_absolute_percent(req.pose.position.y)
self._stage.move_z_absolute_percent(req.pose.position.z)
finished = False
while not finished:
moving = self._stage.moving()
finished = not any(moving)
if not finished:
self._rate.sleep()
self._move_absolute_percent_action.set_succeeded()
except ZaberError:
rospy.logwarn(
'zaber_stage_node _move_absolute_percent_callback ZaberError')
