class ManipulatorInterface(object):
def __init__(self):
self.is_initialized = False
rospy.init_node('manipulator_interface', anonymous=False)
self.pub = rospy.Publisher('pwm', Pwm, queue_size=1)
self.sub = rospy.Subscriber('manipulator_command', Manipulator, self.callback)
self.neutral_pulse_width = servo_position_to_microsecs(0)
rospy.sleep(0.1) # Initial set to zero seems to disappear without a short sleep here
self.servo_set_to_zero()
rospy.on_shutdown(self.shutdown)
self.claw_direction = 0.0
self.claw_position = 0.0 # 1 = open, -1 = closed
self.claw_speed = 1.0
try:
self.valve_stepper = Stepper(STEPPER_NUM_STEPS,
STEPPER_VALVE_PINS,
STEPPER_VALVE_ENABLE_PIN,
COMPUTER)
self.valve_direction = 0
self.agar_stepper = Stepper(STEPPER_NUM_STEPS,
STEPPER_AGAR_PINS,
STEPPER_AGAR_ENABLE_PIN,
COMPUTER)
self.agar_direction = 0
except NameError:
rospy.logfatal('Could not initialize stepper.py. Is /computer parameter set correctly? '
'Shutting down node...')
rospy.signal_shutdown('')
self.valve_stepper.disable()
self.agar_stepper.disable()
rospy.loginfo('Initialized with {0} RPM steppers.'.format(STEPPER_RPM))
self.is_initialized = True
self.spin()
def spin(self):
period = 60.0 / (STEPPER_NUM_STEPS * STEPPER_RPM)
rate = rospy.Rate(1/period)
prev_time = rospy.get_rostime()
min_pwm_interval = rospy.Duration(0.1)
while not rospy.is_shutdown():
# Accumulate claw position
self.claw_position += self.claw_speed * period * self.claw_direction
# Saturate claw position to [-1, 1]
self.claw_position = clip(self.claw_position, -1, 1)
# Step steppers if nonzero direction
if abs(self.valve_direction) == 1:
self.valve_stepper.step_once(self.valve_direction)
if abs(self.agar_direction) == 1:
self.agar_stepper.step_once(self.agar_direction)
# Move servo if nonzero direction
if abs(self.claw_direction) == 1:
if (rospy.get_rostime() - prev_time) > min_pwm_interval:
self.set_claw_pwm(self.claw_position)
prev_time = rospy.get_rostime()
rate.sleep()
def servo_set_to_zero(self):
msg = Pwm()
msg.pins.append(SERVO_PWM_PIN)
msg.positive_width_us.append(self.neutral_pulse_width)
self.pub.publish(msg)
if self.is_initialized:
rospy.loginfo("Setting servo position to zero")
def servo_disable(self):
msg = Pwm()
msg.pins.append(SERVO_PWM_PIN)
msg.positive_width_us.append(0)
self.pub.publish(msg)
def shutdown(self):
self.servo_disable()
self.valve_stepper.shutdown()
def callback(self, msg):
if not self.is_initialized:
rospy.logwarn('Callback before node initialized, ignoring...')
return
if not healthy_message(msg):
return
self.claw_direction = msg.claw_direction
if msg.valve_direction != self.valve_direction:
self.valve_direction = msg.valve_direction
if self.valve_direction == 0:
self.valve_stepper.disable()
else:
self.valve_stepper.enable()
if msg.agar_direction != self.agar_direction:
self.agar_direction = msg.agar_direction
if self.agar_direction == 0:
self.agar_stepper.disable()
else:
self.agar_stepper.enable()
def set_claw_pwm(self, position):
microsecs = servo_position_to_microsecs(position)
msg = Pwm()
msg.pins.append(SERVO_PWM_PIN)
msg.positive_width_us.append(microsecs)
self.pub.publish(msg)
class ManipulatorInterface(object):
def __init__(self):
self.is_initialized = False
rospy.init_node('manipulator_interface', anonymous=False)
self.sub = rospy.Subscriber('manipulator_command', Manipulator,
self.callback)
rospy.on_shutdown(self.shutdown)
try:
self.claw_stepper = Stepper(STEPPER_NUM_STEPS, STEPPER_CLAW_PINS,
STEPPER_CLAW_PWM_PINS, COMPUTER)
self.claw_direction = 0
self.vertical_stepper = Stepper(STEPPER_NUM_STEPS,
STEPPER_VERTICAL_PINS,
STEPPER_VERTICAL_PWM_PINS,
COMPUTER)
self.vertical_stepper_direction = 0
except NameError:
rospy.logfatal(
'Could not initialize stepper.py. Is /computer parameter set correctly? '
'Shutting down node...')
rospy.signal_shutdown('')
self.claw_stepper.disable()
self.vertical_stepper.disable()
rospy.loginfo('Initialized with {0} RPM steppers.'.format(STEPPER_RPM))
self.is_initialized = True
self.spin()
def spin(self):
period = 60.0 / (STEPPER_NUM_STEPS * STEPPER_RPM)
rate = rospy.Rate(1 / period)
while not rospy.is_shutdown():
# Step steppers if nonzero direction
if abs(self.claw_direction) == 1:
self.claw_stepper.step_once(self.claw_direction)
if abs(self.vertical_stepper_direction) == 1:
self.vertical_stepper.step_once(
self.vertical_stepper_direction)
rate.sleep()
def shutdown(self):
self.claw_stepper.shutdown()
self.vertical_stepper.shutdown()
def callback(self, msg):
if not self.is_initialized:
rospy.logwarn('Callback before node initialized, ignoring...')
return
if not healthy_message(msg):
return
if msg.claw_direction != self.claw_direction:
self.claw_direction = msg.claw_direction
if self.claw_direction == 0:
self.claw_stepper.disable()
else:
self.claw_stepper.enable()
if msg.vertical_stepper_direction != self.vertical_stepper_direction:
self.vertical_stepper_direction = msg.vertical_stepper_direction
if self.vertical_stepper_direction == 0:
self.vertical_stepper.disable()
else:
self.vertical_stepper.enable()
