def __init__(self): port = rospy.get_param("~port_name") topic_prefix = rospy.get_param("~topic_prefix", "pololu/") topic_name = rospy.get_param("~topic_name", "command") # Use safety proxy to filter motor comamnds safety = rospy.get_param("~safety", False) # Use specific rate to publish motors commands self._origin_sync = self._sync = rospy.get_param("~sync", "off") self._dynamic_speed = rospy.get_param("~dyn_speed", "off") self._servo_rate = rospy.get_param("~servo_rate", 50) self._controller_type = rospy.get_param("~controller", "Maestro") self._hw_fail = rospy.get_param("~hw_required", False) self._motors = {} self._inputs = {} self.idle = False if rospy.has_param("~pololu_motors_yaml"): config_yaml = rospy.get_param("~pololu_motors_yaml") try: yaml_stream = open(config_yaml) config = yaml.load(yaml_stream) except: logger.warn("Error loading config files") # Get existing motors config and update those configs if callibration data changed for name, cfg in config.items(): if 'input_name' in cfg.keys(): self._inputs[name] = cfg self._inputs[name]['topic'] = rospy.Publisher( 'inputs/{}'.format(name), Int32, queue_size=1) else: self._motors[name] = PololuMotor(name, cfg) rospy.logerr(self._inputs) try: if self._controller_type == 'Maestro': self.controller = Maestro(port, writeTimeout=1.0) if self._controller_type == 'MicroSSC': self.controller = MicroSSC(port) except Exception as ex: if self._hw_fail: rospy.logfatal("Hardware needs to be attached") rospy.signal_shutdown("HW Failure") return logger.warn("Error creating the motor controller") logger.warn(ex) self.idle = True rospy.set_param(topic_prefix.strip("/") + "_enabled", False) return rospy.set_param(topic_prefix.strip("/") + "_enabled", True) # Listen for outputs from proxy self.states_pub = rospy.Publisher(topic_prefix + "motors_states", JointState, queue_size=10) if safety: topic_prefix = 'safe/' + topic_prefix rospy.Subscriber(topic_prefix + topic_name, MotorCommand, self.motor_command_callback) logger.info("ros_pololu Subscribed to %s" % (topic_prefix + topic_name)) # Topic to enable disable continou rospy.Subscriber('pololu_sync', String, self.sync_callback)
def __init__(self): port = rospy.get_param("~port_name") topic_prefix = rospy.get_param("~topic_prefix", "pololu/") topic_name = rospy.get_param("~topic_name", "command") # Use safety proxy to filter motor comamnds safety = rospy.get_param("~safety", False) # Use specific rate to publish motors commands self._sync = rospy.get_param("~sync", "off") self._dynamic_speed = rospy.get_param("~dyn_speed", "off") self._servo_rate = rospy.get_param("~servo_rate", 50) self._controller_type = rospy.get_param("~controller", "Maestro") self._motors = {} self.idle = False if rospy.has_param("~pololu_motors_yaml"): config_yaml = rospy.get_param("~pololu_motors_yaml") try: yaml_stream = open(config_yaml) config = yaml.load(yaml_stream) except: logger.warn("Error loading config files") # Get existing motors config and update those configs if callibration data changed motors = rospy.get_param('motors', []) for name, cfg in config.items(): self._motors[name] = PololuMotor(name, cfg) # cfg = self._motors[name].get_calibrated_config() # cfg['topic'] = topic_prefix.strip("/") # cfg['hardware'] = 'pololu' # for i, m in enumerate(motors): # if m['name'] == name: # motors[i] = cfg # break # else: # motors.append(cfg) # rospy.set_param('motors', motors) try: if self._controller_type == 'Maestro': self.controller = Maestro(port) if self._controller_type == 'MicroSSC': self.controller = MicroSSC(port) except Exception as ex: logger.warn("Error creating the motor controller") logger.warn(ex) self.idle = True rospy.set_param(topic_prefix.strip("/") + "_enabled", False) return rospy.set_param(topic_prefix.strip("/") + "_enabled", True) # Listen for outputs from proxy self.states_pub = rospy.Publisher(topic_prefix + "motors_states", JointState, queue_size=10) if safety: topic_prefix = 'safe/' + topic_prefix rospy.Subscriber(topic_prefix + topic_name, MotorCommand, self.motor_command_callback) logger.info("ros_pololu Subscribed to %s" % (topic_prefix + topic_name))
def test_port_io(self): controller = Maestro(self.port0) reader = PololuSerialReader(self.port1) for i in range(10): pulses = [6143, 6122, 5899, 5085, 5352, 5810, 6075, 6143] * 100 for pulse in pulses: controller.setAcceleration(12, 255) controller.setSpeed(12, 256) controller.setTarget(12, pulse) pulses_got = [] for i in range(3 * len(pulses)): id, cmd, value = reader.read() if cmd == 'speed': self.assertEqual(value, 256) if cmd == 'accelaration': self.assertEqual(value, 255) if cmd == 'position': pulses_got.append(value) self.assertListEqual(pulses, pulses_got)
import time from pololu.motors import Maestro def set_position(motors, pos_series): active_motors = pos_series.index.tolist() for motor in motors: if motor['name'] in active_motors: pos = pos_series[motor['name']] if not np.isnan(pos): motor['pos'] = pos print "set {} to {}".format(motor['name'], pos) if __name__ == '__main__': signal_file = 'signal.csv' signal_df = pd.read_csv(signal_file) motors = get_motors('head.yaml') init_position(motors) DEVICE = '/dev/ttyACM0' controller = None if os.path.exists(DEVICE): controller = Maestro(DEVICE) for index, row in signal_df.iterrows(): set_position(motors, row) if controller: set_servos(controller, motors) time.sleep(0.5)