def get_pololu_power(self, port, channel):
try:
m = Maestro(port);
pos = m.getPosition(channel)
m.close()
if pos > 100:
return 0
return 1
except:
return 1
def publish_motor_states(self):
if self.idle:
return
if self._sync == 'on':
for i, m in self._motors.items():
try:
if self._dynamic_speed == "on":
# Get speed required and normalize it
speed = Maestro.calculateSpeed(
m.last_pulse, m.pulse, 1.0 / COMMAND_RATE,
1.0 / self._servo_rate) / 512.0
m.last_pulse = m.pulse
else:
speed = m.speed
self.set_speed(m.id, speed)
self.set_pulse(m.id, m.pulse)
self.set_acceleration(m.id, 0)
except Exception as ex:
logger.error("Error %s" % ex)
time.sleep(0.01)
self.controller.clean()
self.controller.clean()
# Publish the states
msg = JointState()
for i, m in self._motors.items():
msg.name.append(m.name)
msg.position.append(m.get_angle())
self.states_pub.publish(msg)
def publish_motor_states(self):
if self.idle:
return
if self._sync == 'on':
for i, m in self._motors.items():
try:
if self._dynamic_speed == "on":
# Get speed required and normalize it
speed = Maestro.calculateSpeed(m.last_pulse, m.pulse, 1.0/COMMAND_RATE, 1.0/self._servo_rate) / 512.0
m.last_pulse = m.pulse
else:
speed = m.speed
self.set_speed(m.id, speed)
self.set_pulse(m.id, m.pulse)
self.set_acceleration(m.id, 0)
except Exception as ex:
logger.error("Error %s" % ex)
time.sleep(0.01)
self.controller.clean()
self.controller.clean()
# Publish the states
msg = JointState()
for i, m in self._motors.items():
msg.name.append(m.name)
msg.position.append(m.get_angle())
self.states_pub.publish(msg)
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)
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)
