def add_meshes(self, models):
for model in models:
if model in self._model_paths:
print('Model %s already exists' % model)
continue
new_model = dict()
new_model['position'] = [0, 0, 0]
new_model['orientation'] = [0, 0, 0, 1]
new_model['scale'] = [1, 1, 1]
if 'pose' in models[model]:
if 'position' in models[model]['pose']:
if len(models[model]['pose']['position']) == 3:
new_model['position'] = models[model]['pose']['position']
if 'orientation' in models[model]['pose']:
if len(models[model]['pose']['orientation']) == 3:
new_model['orientation'] = quaternion_from_euler(*models[model]['pose']['orientation'])
if 'scale' in models[model]:
if len(models[model]['scale']) == 3:
new_model['scale'] = models[model]['scale']
if 'mesh' in models[model]:
new_model['mesh'] = models[model]['mesh']
if 'model' in models[model]:
model_name = models[model]['model']
prop = self._get_model_state(model_name, '')
if prop.success:
new_model['position'] = [prop.pose.position.x,
prop.pose.position.y,
prop.pose.position.z]
new_model['orientation'] = [prop.pose.orientation.x,
prop.pose.orientation.y,
prop.pose.orientation.z,
prop.pose.orientation.w]
else:
print('Model %s not found in the current Gazebo scenario' % model)
else:
print('Information about the model %s for the mesh %s could not be '
'retrieved' % (model, models[model]['mesh']))
elif 'plane' in models[model]:
new_model['plane'] = [1, 1, 1]
if 'plane' in models[model]:
if len(models[model]['plane']) == 3:
new_model['plane'] = models[model]['plane']
else:
print('Invalid scale vector for ' + model)
else:
continue
self._model_paths[model] = new_model
print('New model being published: %s' % model)
print('\t Position: ' + str(self._model_paths[model]['position']))
print('\t Orientation: ' + str(self._model_paths[model]['orientation']))
print('\t Scale: ' + str(self._model_paths[model]['scale']))
def from_dict(self, data):
"""Initialize the trajectory point attributes from a `dict`.
> *Input arguments*
* `data` (*type:* `dict`): Trajectory point as a `dict`
"""
self._t = data['time']
self._pos = np.array(data['pos'])
if self._rot.size == 3:
self._rot = quaternion_from_euler(data['rot'])
else:
self._rot = np.array(data['rot'])
self._vel = np.array(data['vel'])
self._acc = np.array(data['acc'])
def send(self, msg):
"""
Assuming that
forward is +y
lateral right is +x
depth up is +z
"""
data = PoseStamped()
data.header.stamp = self.get_clock().now().to_msg()
data.pose.position = Point(x=msg[0], y=msg[1], z=msg[2])
ori = transf.quaternion_from_euler(msg[3], msg[4], msg[5], 'sxyz')
data.pose.orientation = Quaternion(x=ori[0],
y=ori[1],
z=ori[2],
w=ori[3])
self.pub.publish(data)
def __init__(self, inertial_frame_id='world'):
"""Class constructor."""
assert inertial_frame_id in ['world', 'world_ned']
# Reading current namespace
self._namespace = rospy.get_namespace()
self._inertial_frame_id = inertial_frame_id
self._body_frame_id = None
self._logger = get_logger()
if self._inertial_frame_id == 'world':
self._body_frame_id = 'base_link'
else:
self._body_frame_id = 'base_link_ned'
try:
import tf2_ros
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
'world', 'world_ned', rospy.Time(),
rospy.Duration(10))
self.q_ned_to_enu = np.array(
[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])
except Exception as ex:
self._logger.warning(
'Error while requesting ENU to NED transform'
', message={}'.format(ex))
self.q_ned_to_enu = quaternion_from_euler(2 * np.pi, 0, np.pi)
self.transform_ned_to_enu = quaternion_matrix(
self.q_ned_to_enu)[0:3, 0:3]
if self.transform_ned_to_enu is not None:
self._logger.info('Transform world_ned (NED) to world (ENU)=\n' +
str(self.transform_ned_to_enu))
self._mass = 0
if rospy.has_param('~mass'):
self._mass = rospy.get_param('~mass')
if self._mass <= 0:
raise rospy.ROSException('Mass has to be positive')
self._inertial = dict(ixx=0, iyy=0, izz=0, ixy=0, ixz=0, iyz=0)
if rospy.has_param('~inertial'):
inertial = rospy.get_param('~inertial')
for key in self._inertial:
if key not in inertial:
raise rospy.ROSException('Invalid moments of inertia')
self._inertial = inertial
self._cog = [0, 0, 0]
if rospy.has_param('~cog'):
self._cog = rospy.get_param('~cog')
if len(self._cog) != 3:
raise rospy.ROSException('Invalid center of gravity vector')
self._cob = [0, 0, 0]
if rospy.has_param('~cog'):
self._cob = rospy.get_param('~cob')
if len(self._cob) != 3:
raise rospy.ROSException('Invalid center of buoyancy vector')
self._body_frame = 'base_link'
if rospy.has_param('~base_link'):
self._body_frame = rospy.get_param('~base_link')
self._volume = 0.0
if rospy.has_param('~volume'):
self._volume = rospy.get_param('~volume')
if self._volume <= 0:
raise rospy.ROSException('Invalid volume')
# Fluid density
self._density = 1028.0
if rospy.has_param('~density'):
self._density = rospy.get_param('~density')
if self._density <= 0:
raise rospy.ROSException('Invalid fluid density')
# Bounding box
self._height = 0.0
self._length = 0.0
self._width = 0.0
if rospy.has_param('~height'):
self._height = rospy.get_param('~height')
if self._height <= 0:
raise rospy.ROSException('Invalid height')
if rospy.has_param('~length'):
self._length = rospy.get_param('~length')
if self._length <= 0:
raise rospy.ROSException('Invalid length')
if rospy.has_param('~width'):
self._width = rospy.get_param('~width')
if self._width <= 0:
raise rospy.ROSException('Invalid width')
# Calculating the rigid-body mass matrix
self._M = np.zeros(shape=(6, 6), dtype=float)
self._M[0:3, 0:3] = self._mass * np.eye(3)
self._M[0:3, 3:6] = - self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 0:3] = self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 3:6] = self._calc_inertial_tensor()
# Loading the added-mass matrix
self._Ma = np.zeros((6, 6))
if rospy.has_param('~Ma'):
self._Ma = np.array(rospy.get_param('~Ma'))
if self._Ma.shape != (6, 6):
raise rospy.ROSException('Invalid added mass matrix')
# Sum rigid-body and added-mass matrices
self._Mtotal = np.zeros(shape=(6, 6))
self._calc_mass_matrix()
# Acceleration of gravity
self._gravity = 9.81
# Initialize the Coriolis and centripetal matrix
self._C = np.zeros((6, 6))
# Vector of restoring forces
self._g = np.zeros(6)
# Loading the linear damping coefficients
self._linear_damping = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping'):
self._linear_damping = np.array(rospy.get_param('~linear_damping'))
if self._linear_damping.shape == (6,):
self._linear_damping = np.diag(self._linear_damping)
if self._linear_damping.shape != (6, 6):
raise rospy.ROSException('Linear damping must be given as a 6x6 matrix or the diagonal coefficients')
# Loading the nonlinear damping coefficients
self._quad_damping = np.zeros(shape=(6,))
if rospy.has_param('~quad_damping'):
self._quad_damping = np.array(rospy.get_param('~quad_damping'))
if self._quad_damping.shape != (6,):
raise rospy.ROSException('Quadratic damping must be given defined with 6 coefficients')
# Loading the linear damping coefficients proportional to the forward speed
self._linear_damping_forward_speed = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping_forward_speed'):
self._linear_damping_forward_speed = np.array(
rospy.get_param('~linear_damping_forward_speed'))
if self._linear_damping_forward_speed.shape == (6,):
self._linear_damping_forward_speed = np.diag(self._linear_damping_forward_speed)
if self._linear_damping_forward_speed.shape != (6, 6):
raise rospy.ROSException(
'Linear damping proportional to the '
'forward speed must be given as a 6x6 '
'matrix or the diagonal coefficients')
# Initialize damping matrix
self._D = np.zeros((6, 6))
# Vehicle states
self._pose = dict(pos=np.zeros(3),
rot=quaternion_from_euler(0, 0, 0))
# Velocity in the body frame
self._vel = np.zeros(6)
# Acceleration in the body frame
self._acc = np.zeros(6)
# Generalized forces
self._gen_forces = np.zeros(6)
def rot(self, new_rot):
self._rot = quaternion_from_euler(*new_rot)
def add_meshes(self, models):
for model in models:
if model in self._model_paths:
self.get_logger().info('Model %s already exists' % model)
continue
new_model = dict()
new_model['position'] = [0., 0., 0.]
new_model['orientation'] = [0., 0., 0., 1.]
new_model['scale'] = [1., 1., 1.]
# It would probably be cleaner not to consider maps of maps, but rather
# to create a function to parse arg1.arg2.arg3
if 'pose' in models[model]:
if 'position' in models[model]['pose']:
val = models[model]['pose']['position'].value
if len(val) == 3:
new_model['position'] = models[model]['pose'][
'position'].value
if 'orientation' in models[model]['pose']:
if len(models[model]['pose']['orientation'].value) == 3:
new_model['orientation'] = quaternion_from_euler(
*models[model]['pose']['orientation'].value)
if 'scale' in models[model]:
if len(models[model]['scale'].value) == 3:
new_model['scale'] = models[model]['scale'].value
if 'mesh' in models[model]:
new_model['mesh'] = models[model]['mesh'].value
if 'model' in models[model]:
model_name = models[model]['model'].value
req = GetEntityState.Request()
req._name = model_name
req.reference_frame = ''
self.get_logger().debug('Asking to: ' +
self.get_entity_state.srv_name +
' model: ' + model_name)
future = self.get_entity_state.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
prop = future.result()
if prop.success:
new_model['position'] = [
prop.state.pose.position.x,
prop.state.pose.position.y,
prop.state.pose.position.z
]
new_model['orientation'] = [
prop.state.pose.orientation.x,
prop.state.pose.orientation.y,
prop.state.pose.orientation.z,
prop.state.pose.orientation.w
]
else:
self.get_logger().info(
'Model %s not found in the current Gazebo scenario'
% model)
else:
raise RuntimeError(
'Exception while calling service: %s' %
future.exception())
else:
self.get_logger().info(
'Information about the model %s for the mesh %s could not be '
'retrieved' % (model, models[model]['mesh'].value))
elif 'plane' in models[model]:
new_model['plane'] = [1, 1, 1]
if 'plane' in models[model]:
if len(models[model]['plane'].value) == 3:
new_model['plane'] = models[model]['plane'].value
else:
self.get_logger().warn('Invalid scale vector for ' +
model)
else:
continue
self._model_paths[model] = new_model
self.get_logger().info(
'\nNew model being published: %s' % model + '\n\t Position: ' +
str(self._model_paths[model]['position']) +
'\n\t Orientation: ' +
str(self._model_paths[model]['orientation']) + '\n\t Scale: ' +
str(self._model_paths[model]['scale']))
def __init__(self,
node: Node,
inertial_frame_id='world',
tf_trans_world_ned_to_enu: TransformStamped = None):
"""Class constructor."""
self.node = node
assert inertial_frame_id in ['world', 'world_ned']
# Reading current namespace
self._namespace = self.node.get_namespace()
self._inertial_frame_id = inertial_frame_id
self._body_frame_id = None
self._logger = get_logger()
if self._inertial_frame_id == 'world':
self._body_frame_id = 'base_link'
else:
self._body_frame_id = 'base_link_ned'
# Handle tf2 results here
if tf_trans_world_ned_to_enu is not None:
self.q_ned_to_enu = np.array([
tf_trans_world_ned_to_enu.transform.rotation.x,
tf_trans_world_ned_to_enu.transform.rotation.y,
tf_trans_world_ned_to_enu.transform.rotation.z,
tf_trans_world_ned_to_enu.transform.rotation.w
])
else:
self._logger.info(
'No world ned to enu was provided. Setting to default')
self.q_ned_to_enu = quaternion_from_euler(2 * np.pi, 0, np.pi)
# try:
# import tf2_ros
# tf_buffer = tf2_ros.Buffer()
# listener = tf2_ros.TransformListener(tf_buffer, self.node)
# tf_trans_ned_to_enu = tf_buffer.lookup_transform(
# 'world', 'world_ned', rclpy.time.Time(),
# rclpy.time.Duration(seconds=10))
# self.q_ned_to_enu = np.array(
# [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])
# except Exception as ex:
# self._logger.warning(
# 'Error while requesting ENU to NED transform'
# ', message={}'.format(ex))
# self.q_ned_to_enu = quaternion_from_euler(2 * np.pi, 0, np.pi)
self.transform_ned_to_enu = quaternion_matrix(self.q_ned_to_enu)[0:3,
0:3]
if self.transform_ned_to_enu is not None:
self._logger.info('Transform world_ned (NED) to world (ENU)=\n' +
str(self.transform_ned_to_enu))
self._mass = 0.0
if self.node.has_parameter('mass'):
self._mass = self.node.get_parameter('mass').value
if self._mass <= 0:
raise RuntimeError('Mass has to be positive')
self._inertial = dict(ixx=0, iyy=0, izz=0, ixy=0, ixz=0, iyz=0)
if self.node.has_parameter('inertial'):
#inertial = self.node.get_parameter('inertial').value
inertial = self.node.get_parameters_by_prefix('inertial')
inertial = {key: val.value for key, val in inertial.items()}
for key in self._inertial:
if key not in inertial:
raise RuntimeError('Invalid moments of inertia')
self._inertial = inertial
self._cog = [0, 0, 0]
if self.node.has_parameter('cog'):
self._cog = self.node.get_parameter('cog').value
if len(self._cog) != 3:
raise RuntimeError('Invalid center of gravity vector')
self._cob = [0, 0, 0]
# bug fix wrt the original code
if self.node.has_parameter('cob'):
self._cob = self.node.get_parameter('cob').value
if len(self._cob) != 3:
raise RuntimeError('Invalid center of buoyancy vector')
self._body_frame = 'base_link'
if self.node.has_parameter('base_link'):
self._body_frame = self.node.get_parameter(
'base_link').get_parameter_value().string_value
self._volume = 0.0
if self.node.has_parameter('volume'):
self._volume = self.node.get_parameter('volume').value
if self._volume <= 0:
raise RuntimeError('Invalid volume')
# Fluid density
self._density = 1028.0
if self.node.has_parameter('density'):
self._density = self.node.get_parameter('density').value
if self._density <= 0:
raise RuntimeError('Invalid fluid density')
# Bounding box
self._height = 0.0
self._length = 0.0
self._width = 0.0
if self.node.has_parameter('height'):
self._height = self.node.get_parameter('height').value
if self._height <= 0:
raise RuntimeError('Invalid height')
if self.node.has_parameter('length'):
self._length = self.node.get_parameter('length').value
if self._length <= 0:
raise RuntimeError('Invalid length')
if self.node.has_parameter('width'):
self._width = self.node.get_parameter('width').value
if self._width <= 0:
raise RuntimeError('Invalid width')
# Calculating the rigid-body mass matrix
self._M = np.zeros(shape=(6, 6), dtype=float)
self._M[0:3, 0:3] = self._mass * np.eye(3)
self._M[0:3, 3:6] = - self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 0:3] = self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 3:6] = self._calc_inertial_tensor()
# Loading the added-mass matrix
self._Ma = np.zeros((6, 6))
if self.node.has_parameter('Ma'):
# Get 1D array
Ma = self.node.get_parameter('Ma').value
# Reshape the array.
self._Ma = np.reshape(Ma, (6, 6))
if self._Ma.shape != (6, 6):
raise RuntimeError('Invalid added mass matrix')
# Sum rigid-body and added-mass matrices
self._Mtotal = np.zeros(shape=(6, 6))
self._calc_mass_matrix()
# Acceleration of gravity
self._gravity = 9.81
# Initialize the Coriolis and centripetal matrix
self._C = np.zeros((6, 6))
# Vector of restoring forces
self._g = np.zeros(6)
# Loading the linear damping coefficients
self._linear_damping = np.zeros(shape=(6, 6))
if self.node.has_parameter('linear_damping'):
self._linear_damping = np.array(
self.node.get_parameter('linear_damping').value)
if self._linear_damping.shape == (6, ):
self._linear_damping = np.diag(self._linear_damping)
if self._linear_damping.shape != (6, 6):
raise RuntimeError(
'Linear damping must be given as a 6x6 matrix or the diagonal coefficients'
)
# Loading the nonlinear damping coefficients
self._quad_damping = np.zeros(shape=(6, ))
if self.node.has_parameter('quad_damping'):
self._quad_damping = np.array(
self.node.get_parameter('quad_damping').value)
if self._quad_damping.shape != (6, ):
raise RuntimeError(
'Quadratic damping must be given defined with 6 coefficients'
)
# Loading the linear damping coefficients proportional to the forward speed
self._linear_damping_forward_speed = np.zeros(shape=(6, 6))
if self.node.has_parameter('linear_damping_forward_speed'):
self._linear_damping_forward_speed = np.array(
self.node.get_parameter('linear_damping_forward_speed').value)
if self._linear_damping_forward_speed.shape == (6, ):
self._linear_damping_forward_speed = np.diag(
self._linear_damping_forward_speed)
if self._linear_damping_forward_speed.shape != (6, 6):
raise RuntimeError('Linear damping proportional to the '
'forward speed must be given as a 6x6 '
'matrix or the diagonal coefficients')
# Initialize damping matrix
self._D = np.zeros((6, 6))
# Vehicle states
self._pose = dict(pos=np.zeros(3), rot=quaternion_from_euler(0, 0, 0))
# Velocity in the body frame
self._vel = np.zeros(6)
# Acceleration in the body frame
self._acc = np.zeros(6)
# Generalized forces
self._gen_forces = np.zeros(6)