def get_parameter_or_helper(self, name, default_value):
return get_parameter_or_helper(self, name, default_value)
def _init_async_impl(self):
# try to retrieve the thrust axes from the robot description
timeout_robot_desc = get_parameter_or_helper(
self, 'timeout_robot_description', 5).value
self.get_axis_from_robot_description(timeout_robot_desc)
# try to retrieve some tf transforms
tf_trans_ned_to_enu = None
try:
if self.namespace != '':
target = '{}base_link'.format(self.namespace)
target = target[1::]
source = '{}base_link_ned'.format(self.namespace)
else:
target = 'base_link'
source = 'base_link_ned'
source = source[1::]
tf_trans_ned_to_enu = self.tf_buffer.lookup_transform(
target, source, rclpy.time.Time(),
rclpy.time.Duration(seconds=10))
except Exception as e:
self.get_logger().warn(
'No transform found between base_link and base_link_ned'
' for vehicle {}, message={}'.format(self.namespace, e))
self.base_link_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.base_link_ned_to_enu = transformations.quaternion_matrix(
(tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
self.get_logger().info('base_link transform NED to ENU=\n' +
str(self.base_link_ned_to_enu))
self.get_logger().info('ThrusterManager::update_rate=' +
str(self.config['update_rate'].value))
# Set the tf_prefix parameter
#TODO probably remove
#self.set_parameters(['thruster_manager/tf_prefix'], [self.namespace])
# param_tf_prefix = rclpy.parameter.Parameter('thruster_manager.tf_prefix', rclpy.Parameter.Type.STRING, self.namespace)
# self.set_parameters([param_tf_prefix])
# Retrieve the output file path to store the TAM
# matrix for future use
self.output_dir = None
if self.has_parameter('output_dir'):
self.output_dir = self.get_parameter(
'output_dir').get_parameter_value().string_value
if not isdir(self.output_dir):
raise RuntimeError('Invalid output directory, output_dir=' +
self.output_dir)
self.get_logger().info('output_dir=' + self.output_dir)
# Number of thrusters
self.n_thrusters = 0
# Thruster objects used to calculate the right angular velocity command
self.thrusters = list()
# Thrust forces vector
self.thrust = None
# Thruster allocation matrix: transform thruster inputs to force/torque
self.configuration_matrix = None
if self.has_parameter('tam'):
tam = self.get_parameter('tam').value
tam = numpy.array(tam)
# Reshape the array. #TODO Unsure
self.configuration_matrix = numpy.reshape(tam, (6, -1))
# Set number of thrusters from the number of columns
self.n_thrusters = self.configuration_matrix.shape[1]
# Create publishing topics to each thruster
params = self.config['conversion_fcn_params']
conv_fcn = self.config['conversion_fcn'].value
if type(params) == list and type(conv_fcn) == list:
if len(params) != self.n_thrusters or len(
conv_fcn) != self.n_thrusters:
raise RuntimeError('Lists conversion_fcn and '
'conversion_fcn_params must have '
'the same number of items as of '
'thrusters')
for i in range(self.n_thrusters):
# topic = self.config['thruster_topic_prefix'].value + 'id_' + str(i) + \
# self.config['thruster_topic_suffix'].value
topic = self.build_topic_name(
self.config['thruster_topic_prefix'].value, i,
self.config['thruster_topic_suffix'].value)
if list not in [type(params), type(conv_fcn)]:
# Unpack parameters to values
deduced_params = {
key: val.value
for key, val in params.items()
}
thruster = Thruster.create_thruster(
self, conv_fcn, i, topic, None, None, **deduced_params)
else:
# Unpack parameters to values
deduced_params = {
key: val.value
for key, val in params[i].items()
}
thruster = Thruster.create_thruster(
self, conv_fcn[i], i, topic, None, None,
**deduced_params)
if thruster is None:
RuntimeError('Invalid thruster conversion '
'function=%s' %
self.config['conversion_fcn'].value)
self.thrusters.append(thruster)
self.get_logger().info('Thruster allocation matrix provided!')
self.get_logger().info('TAM=')
self.get_logger().info(str(self.configuration_matrix))
self.thrust = numpy.zeros(self.n_thrusters)
if not self.update_tam():
raise RuntimeError('No thrusters found')
# (pseudo) inverse: force/torque to thruster inputs
self.inverse_configuration_matrix = None
if self.configuration_matrix is not None:
self.inverse_configuration_matrix = numpy.linalg.pinv(
self.configuration_matrix)
# If an output directory was provided, store matrix for further use
if self.output_dir is not None:
with open(join(self.output_dir, 'TAM.yaml'), 'w') as yaml_file:
yaml_file.write(
yaml.safe_dump(
dict(tam=self.configuration_matrix.tolist())))
self._ready = True
self.init_future.set_result(True)
self.get_logger().info('ThrusterManager: ready')