class RosPololuNode:
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 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()
if len(self._inputs) > 0:
for input in self._inputs.itervalues():
input['topic'].publish(self.get_position(input['motor_id']))
# 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 motor_command_callback(self, msg):
# Enable command processing for debuging
if self.idle:
return
pulse = 0
motor_id = 0
if msg.joint_name in self._motors.keys():
motor = self._motors[msg.joint_name]
motor_id = motor.id
pulse = motor.set_angle(msg.position)
motor.speed = min(max(0, msg.speed), 1)
motor.acceleration = min(max(0, msg.acceleration), 1)
if msg.speed > 1:
msg.speed = motor.speed
if msg.acceleration > 1:
msg.acceleration = motor.acceleration
elif msg.joint_name.isdigit():
try:
motor_id = int(msg.joint_name)
except:
logger.warn("Invalid motor specified")
return
pulse = PololuMotor.get_default_pulse(msg.position)
if not (self._sync == 'on' and
(msg.joint_name in self._motors.keys())):
self.set_speed(motor_id, min(max(0, msg.speed), 1))
self.set_acceleration(motor_id, min(max(0, msg.acceleration), 1))
self.set_pulse(motor_id, pulse)
def set_pulse(self, id, pulse):
try:
self.controller.setTarget(id, pulse)
except AttributeError:
pass
def set_speed(self, id, speed):
"""
Converting the speed 0-1 range to actual motor speed. Full speed would mean the motor would move full range in approx 0.1s. MicroSCC only allows twice slower speeds to be set.
0 Speed sets pulse immediately.
:param id: motor_id
:param speed: speed
"""
speed = int(512 * speed)
try:
self.controller.setSpeed(id, speed)
except AttributeError:
pass
def set_acceleration(self, id, acceleation):
# FIXME: disable acceleration because pololu may have problem with acceleration
acceleration = 0
try:
self.controller.setAcceleration(id, acceleration)
except AttributeError:
pass
def get_position(self, id):
try:
pos = self.controller.getPosition(id)
return pos
except:
return -1
def sync_callback(self, msg):
"""
Allows to pause continuous updates of motors while configs are updating
:param msg: String "off" or "on"
:return:
"""
if msg.data == 'on':
self._sync = self._origin_sync
if msg.data == 'off':
self._sync = 'off'
