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