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()