def test_thruster_custom(self):
input_values = np.linspace(-50, 50, 10)
output_values = np.linspace(-10000, 10000, 10)
gain = 20000.0 / 100.0
# Use random positions and quaternions
for axis in AXES:
pos = np.random.rand(3)
q = transformations.random_quaternion()
thruster = Thruster.create_thruster(
self.node,
'custom',
IDX,
TOPIC,
pos,
q,
axis,
input=input_values,
output=output_values)
self.assertEqual(thruster.index, IDX)
self.assertEqual(thruster.topic, TOPIC)
self.assertTrue((thruster.tam_column == get_force_vector(pos, q, axis)).all())
self.assertTrue(np.isclose(thruster.get_thrust_value(0), 0))
self.assertTrue(np.isclose(thruster.get_command_value(0), 0))
x = random.random() * 10
self.assertTrue(np.isclose(thruster.get_thrust_value(x), gain * x))
y = random.random() * 10000
self.assertTrue(np.isclose(thruster.get_command_value(y), y / gain))
def test_thruster_proportional(self):
# Use random positions and quaternions
for axis in AXES:
pos = np.random.rand(3)
q = transformations.random_quaternion()
gain = random.random()
thruster = Thruster.create_thruster(
self.node,
'proportional',
IDX,
TOPIC,
pos,
q,
axis,
gain=gain)
self.assertEqual(thruster.index, IDX)
self.assertEqual(thruster.topic, TOPIC)
self.assertTrue((thruster.tam_column == get_force_vector(pos, q, axis)).all())
self.assertEqual(thruster.get_thrust_value(0), 0)
self.assertEqual(thruster.get_command_value(0), 0)
command = np.linspace(-100, 100, 10)
for x in command:
y = thruster.get_thrust_value(x)
self.assertEqual(y, gain * np.abs(x) * x)
thrust = np.linspace(-50000, 50000, 10)
for x in thrust:
y = thruster.get_command_value(x)
self.assertEqual(y, np.sign(x) * np.sqrt(np.abs(x) / gain))
def _init_async_impl(self):
base = '%s/%s' % (self.namespace, self.base_link)
frame = '%s/%s%d' % (self.namespace,
self.thruster_config['frame_base'], 0)
self.get_logger().info('Lookup: Thruster transform found %s -> %s' %
(base, frame))
trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(),
rclpy.time.Duration(seconds=5))
pos = np.array([
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
])
quat = np.array([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])
self.get_logger().info('Thruster transform found %s -> %s' %
(base, frame))
self.get_logger().info('pos=' + str(pos))
self.get_logger().info('rot=' + str(quat))
# Read transformation from thruster
# params = {key: val.value for key, val in params.items()}
self.thruster = Thruster.create_thruster(
self, self.thruster_config['conversion_fcn'], 0,
self.thruster_topic, pos, quat,
**self.thruster_config['conversion_fcn_params'])
self.get_logger().info('Thruster configuration=\n' +
str(self.thruster_config))
self.get_logger().info('Thruster input topic=' + self.thruster_topic)
self.pub_cmd = list()
for i in range(self.n_fins):
topic = self.build_fin_topic_name(self.fin_topic_prefix, i,
self.fin_topic_suffix)
#topic = '%s/id_%d/%s' % (self.fin_topic_prefix, i, self.fin_topic_suffix)
self.pub_cmd.append(self.create_publisher(FloatStamped, topic, 10))
self.odometry_sub = self.create_subscription(Odometry, 'odom',
self.odometry_callback,
10)
self.reference_pub = self.create_publisher(TrajectoryPoint,
'reference', 1)
# Publish error (for debugging)
self.error_pub = self.create_publisher(TrajectoryPoint, 'error', 1)
self._ready = True
self.get_logger().info('AUV geometric tracking controller: ready')
self.init_future.set_result(True)
def test_thruster(self):
# Use random positions and quaternions
for axis in AXES:
pos = np.random.rand(3)
q = transformations.random_quaternion()
thruster = Thruster(index=IDX,
topic=TOPIC,
pos=pos,
orientation=q,
axis=axis)
self.assertEqual(thruster.index, IDX)
self.assertEqual(thruster.topic, TOPIC)
self.assertTrue(
(thruster.tam_column == get_force_vector(pos, q, axis)).all())
def __init__(self):
super().__init__('finned_uuv_teleop',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
#Default sim_time to True
sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
self.set_parameters([sim_time])
self.get_logger().info('FinnedUUVControllerNode: initializing node')
self._ready = False
# Test if any of the needed parameters are missing
param_labels = ['n_fins', 'gain_roll', 'gain_pitch', 'gain_yaw',
'thruster_model.max_thrust', 'thruster_model.name',
'fin_topic_prefix',
'fin_topic_suffix', 'thruster_topic',
'axis_thruster', 'axis_roll', 'axis_pitch', 'axis_yaw']
for label in param_labels:
if not self.has_parameter('%s' % label):
raise Exception('Parameter missing, label=%s' % label)
# Number of fins
self._n_fins = self.get_parameter('n_fins').get_parameter_value().integer_value
# Thruster joy axis gain
self._thruster_joy_gain = 1
if self.has_parameter('thruster_joy_gain'):
self._thruster_joy_gain = self.get_parameter('thruster_joy_gain').value
# Read the vector for contribution of each fin on the change on
# orientation
gain_roll = self.get_parameter('gain_roll').value
gain_pitch = self.get_parameter('gain_pitch').value
gain_yaw = self.get_parameter('gain_yaw').value
if len(gain_roll) != self._n_fins or len(gain_pitch) != self._n_fins \
or len(gain_yaw) != self._n_fins:
raise Exception('Input gain vectors must have length '
'equal to the number of fins')
# Create the command angle to fin angle mapping
self._rpy_to_fins = numpy.vstack((gain_roll, gain_pitch, gain_yaw)).T
# Read the joystick mapping
self._joy_axis = dict(axis_thruster=self.get_parameter('axis_thruster').get_parameter_value().integer_value,
axis_roll=self.get_parameter('axis_roll').get_parameter_value().integer_value,
axis_pitch=self.get_parameter('axis_pitch').get_parameter_value().integer_value,
axis_yaw=self.get_parameter('axis_yaw').get_parameter_value().integer_value)
# Subscribe to the fin angle topics
self._pub_cmd = list()
self._fin_topic_prefix = self.get_parameter('fin_topic_prefix').get_parameter_value().string_value
self._fin_topic_suffix = self.get_parameter('fin_topic_suffix').get_parameter_value().string_value
for i in range(self._n_fins):
topic = self.build_topic_name(self._fin_topic_prefix, i, self._fin_topic_suffix)
#topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
self._pub_cmd.append(self.create_publisher(FloatStamped, topic, 10))
# Create the thruster model object
try:
self._thruster_topic = self.get_parameter('thruster_topic').get_parameter_value().string_value
self._thruster_params = self.get_parameters_by_prefix('thruster_model')
self._thruster_params = parse_nested_params_to_dict(self._thruster_params, '.')
if 'params' not in self._thruster_params:
raise Exception('No thruster params given')
#Should not happen
if 'max_thrust' not in self._thruster_params:
raise Exception('No limit to thruster output was given')
self._thruster_model = Thruster.create_thruster(
self, self._thruster_params['name'].value, 0,
self._thruster_topic, None, None,
**{key: val.value for key, val in self._thruster_params['params'].items()})
except Exception as e:
raise RuntimeError('Thruster model could not be initialized: ' + str(e))
# Subscribe to the joystick topic
self.sub_joy = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
self._ready = True
def find_actuators(self):
"""Calculate the control allocation matrix, if one is not given."""
self.ready = False
self.get_logger().infos('ControlAllocator: updating thruster poses')
base = '%s/%s' % (self.namespace, self.base_link)
frame = '%s/%s%d' % (self.namespace,
self.thruster_config['frame_base'], 0)
self.get_logger().info('Lookup: Thruster transform found %s -> %s' %
(base, frame))
trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(),
rclpy.time.Duration(seconds=1))
pos = np.array([
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
])
quat = np.array([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])
self.get_logger().info('Thruster transform found %s -> %s' %
(base, frame))
self.get_logger().info('pos=' + str(pos))
self.get_logger().info('rot=' + str(quat))
# Read transformation from thruster
#params = {key: val.value for key, val in params.items()}
self.thruster = Thruster.create_thruster(
self.thruster_config['conversion_fcn'], 0, self.thruster_topic,
pos, quat, **self.thruster_config['conversion_fcn_params'])
for i in range(self.MAX_FINS):
try:
frame = '%s/%s%d' % (self.namespace,
self.fin_config['frame_base'], i)
self.get_logger().info('Lookup: Fin transform found %s -> %s' %
(base, frame))
trans = self.tf_buffer.lookup_transform(
base, frame, rclpy.time.Time(),
rclpy.time.Duration(seconds=1))
pos = np.array([
trans.transform.translation.x,
trans.transform.translation.y,
trans.transform.translation.z
])
quat = np.array([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])
self.get_logger().info('Fin transform found %s -> %s' %
(base, frame))
self.get_logger().info('pos=' + str(pos))
self.get_logger().info('quat=' + str(quat))
fin_topic = build_topic_name(self.namespace,
self.fin_config['topic_prefix'],
i,
self.fin_config['topic_suffix'])
self.fins[i] = FinModel(i, pos, quat, fin_topic, self)
except (tf2.LookupException, tf2.ConnectivityException,
tf2.ExtrapolationException):
self.get_logger().info(
'Could not get transform from %s to %s ' % (base, frame))
break
self.n_fins = len(self.fins.keys())
self.get_logger().info('# fins found: %d' % len(self.fins.keys()))
for i in range(self.n_fins):
self.get_logger().info(i)
self.get_logger().info(self.fins[i].pos)
self.get_logger().info(self.fins[i].rot)
self.ready = True
return True
def __init__(self, node_name, **kwargs):
super().__init__(node_name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# self.set_parameters([sim_time])
self.namespace = self.get_namespace().replace('/', '')
self.get_logger().info('Initialize control for vehicle <%s>' %
self.namespace)
self.local_planner = DPControllerLocalPlanner(full_dof=True,
thrusters_only=False,
stamped_pose_only=False)
self.base_link = self.get_parameter_or_helper(
'base_link', 'base_link').get_parameter_value().string_value
# Reading the minimum thrust generated
self.min_thrust = self.get_parameter_or_helper('min_thrust', 0.0).value
assert self.min_thrust >= 0
self.get_logger().info('Min. thrust [N]=%.2f' % self.min_thrust)
# Reading the maximum thrust generated
self.max_thrust = self.get_parameter_or_helper('max_thrust', 0.0).value
assert self.max_thrust > 0 and self.max_thrust > self.min_thrust
self.get_logger().info('Max. thrust [N]=%.2f' % self.max_thrust)
# Reading the thruster topic
self.thruster_topic = self.get_parameter_or_helper(
'thruster_topic',
'thrusters/id_0/input').get_parameter_value().string_value
assert len(self.thruster_topic) > 0
# Reading the thruster gain
self.p_gain_thrust = self.get_parameter_or_helper(
'thrust_p_gain', 0.0).value
assert self.p_gain_thrust > 0
self.d_gain_thrust = self.get_parameter_or_helper(
'thrust_d_gain', 0.0).value
assert self.d_gain_thrust >= 0
# Reading the roll gain
self.p_roll = self.get_parameter_or_helper('p_roll', 0.0).value
assert self.p_roll > 0
# Reading the pitch P gain
self.p_pitch = self.get_parameter_or_helper('p_pitch', 0.0).value
assert self.p_pitch > 0
# Reading the pitch D gain
self.d_pitch = self.get_parameter_or_helper('d_pitch', 0.0).value
assert self.d_pitch >= 0
# Reading the yaw P gain
self.p_yaw = self.get_parameter_or_helper('p_yaw', 0.0).value
assert self.p_yaw > 0
# Reading the yaw D gain
self.d_yaw = self.get_parameter_or_helper('d_yaw', 0.0).value
assert self.d_yaw >= 0
# Reading the saturation for the desired pitch
self.desired_pitch_limit = self.get_parameter_or_helper(
'desired_pitch_limit', 15 * np.pi / 180).value
assert self.desired_pitch_limit > 0
# Reading the saturation for yaw error
self.yaw_error_limit = self.get_parameter_or_helper(
'yaw_error_limit', 1.0).value
assert self.yaw_error_limit > 0
# Reading the number of fins
self.n_fins = self.get_parameter_or_helper(
'n_fins', 0).get_parameter_value().integer_value
assert self.n_fins > 0
# Reading the mapping for roll commands
self.map_roll = self.get_parameter_or_helper('map_roll',
[0, 0, 0, 0]).value
assert isinstance(self.map_roll, list)
assert len(self.map_roll) == self.n_fins
# Reading the mapping for the pitch commands
self.map_pitch = self.get_parameter_or_helper('map_pitch',
[0, 0, 0, 0]).value
assert isinstance(self.map_pitch, list)
assert len(self.map_pitch) == self.n_fins
# Reading the mapping for the yaw commands
self.map_yaw = self.get_parameter_or_helper('map_yaw',
[0, 0, 0, 0]).value
assert isinstance(self.map_yaw, list)
assert len(self.map_yaw) == self.n_fins
# Retrieve the thruster configuration parameters
self.thruster_config = self.get_parameter_by_prefix('thruster_config')
#Parse parameters to dictionary and unpack params to values
self.thruster_config = parse_nested_params_to_dict(
thruster_config, '.', True)
# Check if all necessary thruster model parameter are available
thruster_params = [
'conversion_fcn_params', 'conversion_fcn', 'topic_prefix',
'topic_suffix', 'frame_base', 'max_thrust'
]
for p in thruster_params:
if p not in self.thruster_config:
raise RuntimeError(
'Parameter <%s> for thruster conversion function is '
'missing' % p)
# Setting up the thruster topic name
self.thruster_topic = build_thruster_topic_name(
self.namespace, self.thruster_config['topic_prefix'], 0,
self.thruster_config['topic_suffix'])
# self.thruster_topic = '/%s/%s/id_%d/%s' % (self.namespace,
# self.thruster_config['topic_prefix'], 0,
# self.thruster_config['topic_suffix'])
base = '%s/%s' % (self.namespace, self.base_link)
frame = '%s/%s%d' % (self.namespace,
self.thruster_config['frame_base'], 0)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer, self)
self.get_logger().info('Lookup: Thruster transform found %s -> %s' %
(base, frame))
trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(),
rclpy.time.Duration(5))
pos = np.array([
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
])
quat = np.array([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])
self.get_logger().info('Thruster transform found %s -> %s' %
(base, frame))
self.get_logger().info('pos=' + str(pos))
self.get_logger().info('rot=' + str(quat))
# Read transformation from thruster
#params = {key: val.value for key, val in params.items()}
self.thruster = Thruster.create_thruster(
self.thruster_config['conversion_fcn'], 0, self.thruster_topic,
pos, quat, **self.thruster_config['conversion_fcn_params'])
self.get_logger().info('Thruster configuration=\n' +
str(self.thruster_config))
self.get_logger().info('Thruster input topic=' + self.thruster_topic)
self.max_fin_angle = self.get_parameter_or_helper(
'max_fin_angle', 0.0).value
assert self.max_fin_angle > 0
# Reading the fin input topic prefix
self.fin_topic_prefix = self.get_parameter_or_helper(
'fin_topic_prefix', 'fins').get_parameter_value().string_value
self.fin_topic_suffix = self.get_parameter_or_helper(
'fin_topic_suffix', 'input').get_parameter_value().string_value
self.rpy_to_fins = np.vstack(
(self.map_roll, self.map_pitch, self.map_yaw)).T
self.pub_cmd = list()
for i in range(self.n_fins):
topic = self.build_fin_topic_name(self.fin_topic_prefix, i,
self.fin_topic_suffix)
#topic = '%s/id_%d/%s' % (self.fin_topic_prefix, i, self.fin_topic_suffix)
self.pub_cmd.append(self.create_publisher(FloatStamped, topic, 10))
self.odometry_sub = self.create_subscription(Odometry, 'odom',
self.odometry_callback,
10)
self.reference_pub = self.create_publisher(TrajectoryPoint,
'reference', 1)
# Publish error (for debugging)
self.error_pub = self.create_publisher(TrajectoryPoint, 'error', 1)
def __init__(self):
self.namespace = rospy.get_namespace().replace('/', '')
rospy.loginfo('Initialize control for vehicle <%s>' % self.namespace)
self.local_planner = DPControllerLocalPlanner(full_dof=True,
thrusters_only=False,
stamped_pose_only=False)
self.base_link = rospy.get_param('~base_link', 'base_link')
# Reading the minimum thrust generated
self.min_thrust = rospy.get_param('~min_thrust', 0)
assert self.min_thrust >= 0
rospy.loginfo('Min. thrust [N]=%.2f', self.min_thrust)
# Reading the maximum thrust generated
self.max_thrust = rospy.get_param('~max_thrust', 0)
assert self.max_thrust > 0 and self.max_thrust > self.min_thrust
rospy.loginfo('Max. thrust [N]=%.2f', self.max_thrust)
# Reading the thruster topic
self.thruster_topic = rospy.get_param('~thruster_topic',
'thrusters/0/input')
assert len(self.thruster_topic) > 0
# Reading the thruster gain
self.p_gain_thrust = rospy.get_param('~thrust_p_gain', 0.0)
assert self.p_gain_thrust > 0
self.d_gain_thrust = rospy.get_param('~thrust_d_gain', 0.0)
assert self.d_gain_thrust >= 0
# Reading the roll gain
self.p_roll = rospy.get_param('~p_roll', 0.0)
assert self.p_roll > 0
# Reading the pitch P gain
self.p_pitch = rospy.get_param('~p_pitch', 0.0)
assert self.p_pitch > 0
# Reading the pitch D gain
self.d_pitch = rospy.get_param('~d_pitch', 0.0)
assert self.d_pitch >= 0
# Reading the yaw P gain
self.p_yaw = rospy.get_param('~p_yaw', 0.0)
assert self.p_yaw > 0
# Reading the yaw D gain
self.d_yaw = rospy.get_param('~d_yaw', 0.0)
assert self.d_yaw >= 0
# Reading the saturation for the desired pitch
self.desired_pitch_limit = rospy.get_param('~desired_pitch_limit',
15 * np.pi / 180)
assert self.desired_pitch_limit > 0
# Reading the saturation for yaw error
self.yaw_error_limit = rospy.get_param('~yaw_error_limit', 1.0)
assert self.yaw_error_limit > 0
# Reading the number of fins
self.n_fins = rospy.get_param('~n_fins', 0)
assert self.n_fins > 0
# Reading the mapping for roll commands
self.map_roll = rospy.get_param('~map_roll', [0, 0, 0, 0])
assert isinstance(self.map_roll, list)
assert len(self.map_roll) == self.n_fins
# Reading the mapping for the pitch commands
self.map_pitch = rospy.get_param('~map_pitch', [0, 0, 0, 0])
assert isinstance(self.map_pitch, list)
assert len(self.map_pitch) == self.n_fins
# Reading the mapping for the yaw commands
self.map_yaw = rospy.get_param('~map_yaw', [0, 0, 0, 0])
assert isinstance(self.map_yaw, list)
assert len(self.map_yaw) == self.n_fins
# Retrieve the thruster configuration parameters
self.thruster_config = rospy.get_param('~thruster_config')
# Check if all necessary thruster model parameter are available
thruster_params = [
'conversion_fcn_params', 'conversion_fcn', 'topic_prefix',
'topic_suffix', 'frame_base', 'max_thrust'
]
for p in thruster_params:
if p not in self.thruster_config:
raise rospy.ROSException(
'Parameter <%s> for thruster conversion function is '
'missing' % p)
# Setting up the thruster topic name
self.thruster_topic = '/%s/%s/%d/%s' % (
self.namespace, self.thruster_config['topic_prefix'], 0,
self.thruster_config['topic_suffix'])
base = '%s/%s' % (self.namespace, self.base_link)
frame = '%s/%s%d' % (self.namespace,
self.thruster_config['frame_base'], 0)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
rospy.loginfo('Lookup: Thruster transform found %s -> %s' %
(base, frame))
trans = self.tf_buffer.lookup_transform(base, frame, rospy.Time(),
rospy.Duration(5))
pos = np.array([
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
])
quat = np.array([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])
rospy.loginfo('Thruster transform found %s -> %s' % (base, frame))
rospy.loginfo('pos=' + str(pos))
rospy.loginfo('rot=' + str(quat))
# Read transformation from thruster
self.thruster = Thruster.create_thruster(
self.thruster_config['conversion_fcn'], 0, self.thruster_topic,
pos, quat, **self.thruster_config['conversion_fcn_params'])
rospy.loginfo('Thruster configuration=\n' + str(self.thruster_config))
rospy.loginfo('Thruster input topic=' + self.thruster_topic)
self.max_fin_angle = rospy.get_param('~max_fin_angle', 0.0)
assert self.max_fin_angle > 0
# Reading the fin input topic prefix
self.fin_topic_prefix = rospy.get_param('~fin_topic_prefix', 'fins')
self.fin_topic_suffix = rospy.get_param('~fin_topic_suffix', 'input')
self.rpy_to_fins = np.vstack(
(self.map_roll, self.map_pitch, self.map_yaw)).T
self.pub_cmd = list()
for i in range(self.n_fins):
topic = '%s/%d/%s' % (self.fin_topic_prefix, i,
self.fin_topic_suffix)
self.pub_cmd.append(
rospy.Publisher(topic, FloatStamped, queue_size=10))
self.odometry_sub = rospy.Subscriber('odom', Odometry,
self.odometry_callback)
self.reference_pub = rospy.Publisher('reference',
TrajectoryPoint,
queue_size=1)
# Publish error (for debugging)
self.error_pub = rospy.Publisher('error',
TrajectoryPoint,
queue_size=1)
def __init__(self, *args):
# Start the super class
DPControllerBase.__init__(self, *args)
self._logger.info('Initializing: AUV Geometric Tracking Controller')
# Thrust gain
self._thrust_gain = 0.0
if rospy.has_param('~thrust_gain'):
self._thrust_gain = rospy.get_param('~thrust_gain')
if self._thrust_gain <= 0:
raise rospy.ROSException('Thrust gain must be greater than zero')
self._logger.info('Thrust gain=' + str(self._thrust_gain))
# Maximum absolute fin angle displacement should be given in radians
self._max_fin_angle = 0.0
if rospy.has_param('~max_fin_angle'):
self._max_fin_angle = rospy.get_param('~max_fin_angle')
if self._max_fin_angle <= 0.0:
raise rospy.ROSException(
'Max. fin displacement angle must be greater than zero')
# Proportional gain for the roll angle
self._roll_gain = 0.0
if rospy.has_param('~roll_gain'):
self._roll_gain = rospy.get_param('~roll_gain')
if self._roll_gain <= 0:
raise rospy.ROSException('Roll gain must be greater than zero')
self._logger.info('Roll gain=' + str(self._roll_gain))
# Proportional gain for the pitch angle
self._pitch_gain = 0.0
if rospy.has_param('~pitch_gain'):
self._pitch_gain = rospy.get_param('~pitch_gain')
if self._pitch_gain <= 0:
raise rospy.ROSException('Pitch gain must be greater than zero')
self._logger.info('Pitch gain=' + str(self._pitch_gain))
# Proportional gain for the yaw angle
self._yaw_gain = 0.0
if rospy.has_param('~yaw_gain'):
self._yaw_gain = rospy.get_param('~yaw_gain')
if self._yaw_gain <= 0:
raise rospy.ROSException('Yaw gain must be greater than zero')
self._logger.info('Yaw gain=' + str(self._yaw_gain))
# Number of fins
self._n_fins = 4
if rospy.has_param('~n_fins'):
self._n_fins = rospy.get_param('~n_fins')
if self._n_fins <= 0:
raise rospy.ROSException(
'Number of fins must be greater than zero')
self._logger.info('Number of fins=' + str(self._n_fins))
# Contribution of each fin to the orientation angles (roll, pitch and
# yaw)
self._fin_contribution_roll = [0, 0, 0, 0]
self._fin_contribution_pitch = [0, 0, 0, 0]
self._fin_contribution_yaw = [0, 0, 0, 0]
if rospy.has_param('~fin_contribution_roll'):
self._fin_contribution_roll = np.array(
rospy.get_param('~fin_contribution_roll'))
if self._fin_contribution_roll.size != self._n_fins:
raise rospy.ROSException(
'Number of elements in the roll contribution vector must be'
' equal to the number of fins')
if np.max(self._fin_contribution_roll) > 1 or \
np.min(self._fin_contribution_roll) < -1:
raise rospy.ROSException('Values in the roll contribution vector'
' must be -1 <= i <= 1')
if rospy.has_param('~fin_contribution_pitch'):
self._fin_contribution_pitch = np.array(
rospy.get_param('~fin_contribution_pitch'))
if self._fin_contribution_pitch.size != self._n_fins:
raise rospy.ROSException(
'Number of elements in the pitch contribution vector must be'
' equal to the number of fins')
if np.max(self._fin_contribution_pitch) > 1 or \
np.min(self._fin_contribution_pitch) < -1:
raise rospy.ROSException('Values in the pitch contribution vector'
' must be -1 <= i <= 1')
if rospy.has_param('~fin_contribution_yaw'):
self._fin_contribution_yaw = np.array(
rospy.get_param('~fin_contribution_yaw'))
if self._fin_contribution_yaw.size != self._n_fins:
raise rospy.ROSException(
'Number of elements in the yaw contribution vector must be'
' equal to the number of fins')
if np.max(self._fin_contribution_yaw) > 1 or \
np.min(self._fin_contribution_yaw) < -1:
raise rospy.ROSException('Values in the yaw contribution vector'
' must be -1 <= i <= 1')
# Generating vehicle orientation to fins conversion matrix
self._rpy_to_fins = np.vstack(
(self._fin_contribution_roll, self._fin_contribution_pitch,
self._fin_contribution_yaw)).T
# Prefix to the fin command topic relative to the robot model
self._fin_topic_prefix = 'fins/'
if rospy.has_param('~fin_topic_prefix'):
self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix')
# Fin command topic suffix
self._fin_topic_suffix = '/input'
if rospy.has_param('~fin_topic_suffix'):
self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix')
# Initialize fin command publishers
self._fin_pubs = list()
for i in range(self._n_fins):
topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
self._fin_pubs.append(
rospy.Publisher(topic, FloatStamped, queue_size=10))
self._logger.info('Publishing command to #%i fin=%s' % (i, topic))
# Suffix for the thruster topic relative to the robot model
self._thruster_topic = 'thrusters/0/input'
if rospy.has_param('~thruster_topic'):
self._thruster_topic = rospy.get_param('~thruster_topic')
# Get thruster parameters
if not rospy.has_param('~thruster_model'):
raise rospy.ROSException('Thruster parameters were not provided')
self._thruster_params = rospy.get_param('~thruster_model')
if 'max_thrust' not in self._thruster_params:
raise rospy.ROSException('No limit to thruster output was given')
self._thruster_model = Thruster.create_thruster(
self._thruster_params['name'], 0, self._thruster_topic, None, None,
**self._thruster_params['params'])
self._logger.info('Publishing to thruster=%s' % self._thruster_topic)
self._is_init = True
self._logger.info('AUV Geometric Tracking Controller ready!')
def __init__(self):
print('FinnedUUVControllerNode: initializing node')
self._ready = False
# Test if any of the needed parameters are missing
param_labels = [
'n_fins', 'gain_roll', 'gain_pitch', 'gain_yaw', 'thruster_model',
'fin_topic_prefix', 'fin_topic_suffix', 'thruster_topic',
'axis_thruster', 'axis_roll', 'axis_pitch', 'axis_yaw'
]
for label in param_labels:
if not rospy.has_param('~%s' % label):
raise rospy.ROSException('Parameter missing, label=%s' % label)
# Number of fins
self._n_fins = rospy.get_param('~n_fins')
# Thruster joy axis gain
self._thruster_joy_gain = 1
if rospy.has_param('~thruster_joy_gain'):
self._thruster_joy_gain = rospy.get_param('~thruster_joy_gain')
# Read the vector for contribution of each fin on the change on
# orientation
gain_roll = rospy.get_param('~gain_roll')
gain_pitch = rospy.get_param('~gain_pitch')
gain_yaw = rospy.get_param('~gain_yaw')
if len(gain_roll) != self._n_fins or len(gain_pitch) != self._n_fins \
or len(gain_yaw) != self._n_fins:
raise rospy.ROSException('Input gain vectors must have length '
'equal to the number of fins')
# Create the command angle to fin angle mapping
self._rpy_to_fins = numpy.vstack((gain_roll, gain_pitch, gain_yaw)).T
# Read the joystick mapping
self._joy_axis = dict(axis_thruster=rospy.get_param('~axis_thruster'),
axis_roll=rospy.get_param('~axis_roll'),
axis_pitch=rospy.get_param('~axis_pitch'),
axis_yaw=rospy.get_param('~axis_yaw'))
# Subscribe to the fin angle topics
self._pub_cmd = list()
self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix')
self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix')
for i in range(self._n_fins):
topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
self._pub_cmd.append(
rospy.Publisher(topic, FloatStamped, queue_size=10))
# Create the thruster model object
try:
self._thruster_topic = rospy.get_param('~thruster_topic')
self._thruster_params = rospy.get_param('~thruster_model')
if 'max_thrust' not in self._thruster_params:
raise rospy.ROSException(
'No limit to thruster output was given')
self._thruster_model = Thruster.create_thruster(
self._thruster_params['name'], 0, self._thruster_topic, None,
None, **self._thruster_params['params'])
except:
raise rospy.ROSException('Thruster model could not be initialized')
# Subscribe to the joystick topic
self.sub_joy = rospy.Subscriber('joy', numpy_msg(Joy),
self.joy_callback)
self._ready = True
def __init__(self, *args):
# Start the super class
DPControllerBase.__init__(self, *args)
self._logger.info('Initializing: AUV Geometric Tracking Controller')
# Thrust gain
self._thrust_gain = 0.0
if rospy.has_param('~thrust_gain'):
self._thrust_gain = rospy.get_param('~thrust_gain')
self._min_thrust = 0.0
if rospy.has_param('~min_thrust'):
self._min_thrust = rospy.get_param('~min_thrust')
self._min_thrust = max(0.0, self._min_thrust)
if self._thrust_gain <= 0:
raise rospy.ROSException('Thrust gain must be greater than zero')
self._logger.info('Thrust gain=' + str(self._thrust_gain))
# Maximum absolute fin angle displacement should be given in radians
self._max_fin_angle = 0.0
if rospy.has_param('~max_fin_angle'):
self._max_fin_angle = rospy.get_param('~max_fin_angle')
if self._max_fin_angle <= 0.0:
raise rospy.ROSException('Max. fin displacement angle must be greater than zero')
# Proportional gain for the roll angle
self._roll_gain = 0.0
if rospy.has_param('~roll_gain'):
self._roll_gain = rospy.get_param('~roll_gain')
if self._roll_gain <= 0:
raise rospy.ROSException('Roll gain must be greater than zero')
self._logger.info('Roll gain=' + str(self._roll_gain))
# Proportional gain for the pitch angle
self._pitch_gain = 0.0
if rospy.has_param('~pitch_gain'):
self._pitch_gain = rospy.get_param('~pitch_gain')
if self._pitch_gain <= 0:
raise rospy.ROSException('Pitch gain must be greater than zero')
self._logger.info('Pitch gain=' + str(self._pitch_gain))
if not rospy.has_param('~pitch_abs_saturation'):
raise rospy.ROSException('Pitch absolute saturation missing')
self._pitch_abs_saturation = rospy.get_param('~pitch_abs_saturation')
if self._pitch_abs_saturation <= 0:
raise rospy.ROSException('Absolute saturation for pitch must be'
' greater than zero')
# Proportional gain for the yaw angle
self._yaw_gain = 0.0
if rospy.has_param('~yaw_gain'):
self._yaw_gain = rospy.get_param('~yaw_gain')
if self._yaw_gain <= 0:
raise rospy.ROSException('Yaw gain must be greater than zero')
self._logger.info('Yaw gain=' + str(self._yaw_gain))
if not rospy.has_param('~yaw_abs_saturation'):
raise rospy.ROSException('Yaw absolute saturation missing')
self._yaw_abs_saturation = rospy.get_param('~yaw_abs_saturation')
if self._yaw_abs_saturation <= 0:
raise rospy.ROSException('Absolute saturation for yaw must be'
' greater than zero')
# Number of fins
self._n_fins = 4
if rospy.has_param('~n_fins'):
self._n_fins = rospy.get_param('~n_fins')
if self._n_fins <= 0:
raise rospy.ROSException('Number of fins must be greater than zero')
self._logger.info('Number of fins=' + str(self._n_fins))
# Contribution of each fin to the orientation angles (roll, pitch and
# yaw)
self._fin_contribution_roll = [0, 0, 0, 0]
self._fin_contribution_pitch = [0, 0, 0, 0]
self._fin_contribution_yaw = [0, 0, 0, 0]
if rospy.has_param('~fin_contribution_roll'):
self._fin_contribution_roll = np.array(
rospy.get_param('~fin_contribution_roll'))
if self._fin_contribution_roll.size != self._n_fins:
raise rospy.ROSException(
'Number of elements in the roll contribution vector must be'
' equal to the number of fins')
if np.max(self._fin_contribution_roll) > 1 or \
np.min(self._fin_contribution_roll) < -1:
raise rospy.ROSException('Values in the roll contribution vector'
' must be -1 <= i <= 1')
if rospy.has_param('~fin_contribution_pitch'):
self._fin_contribution_pitch = np.array(
rospy.get_param('~fin_contribution_pitch'))
if self._fin_contribution_pitch.size != self._n_fins:
raise rospy.ROSException(
'Number of elements in the pitch contribution vector must be'
' equal to the number of fins')
if np.max(self._fin_contribution_pitch) > 1 or \
np.min(self._fin_contribution_pitch) < -1:
raise rospy.ROSException('Values in the pitch contribution vector'
' must be -1 <= i <= 1')
if rospy.has_param('~fin_contribution_yaw'):
self._fin_contribution_yaw = np.array(
rospy.get_param('~fin_contribution_yaw'))
if self._fin_contribution_yaw.size != self._n_fins:
raise rospy.ROSException(
'Number of elements in the yaw contribution vector must be'
' equal to the number of fins')
if np.max(self._fin_contribution_yaw) > 1 or \
np.min(self._fin_contribution_yaw) < -1:
raise rospy.ROSException('Values in the yaw contribution vector'
' must be -1 <= i <= 1')
# Generating vehicle orientation to fins conversion matrix
self._rpy_to_fins = np.vstack((self._fin_contribution_roll,
self._fin_contribution_pitch,
self._fin_contribution_yaw)).T
# Prefix to the fin command topic relative to the robot model
self._fin_topic_prefix = 'fins/'
if rospy.has_param('~fin_topic_prefix'):
self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix')
# Fin command topic suffix
self._fin_topic_suffix = '/input'
if rospy.has_param('~fin_topic_suffix'):
self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix')
# Initialize fin command publishers
self._fin_pubs = list()
for i in range(self._n_fins):
topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
self._fin_pubs.append(rospy.Publisher(topic, FloatStamped,
queue_size=10))
self._logger.info('Publishing command to #%i fin=%s' % (i, topic))
# Suffix for the thruster topic relative to the robot model
self._thruster_topic = 'thrusters/0/input'
if rospy.has_param('~thruster_topic'):
self._thruster_topic = rospy.get_param('~thruster_topic')
# Get thruster parameters
if not rospy.has_param('~thruster_model'):
raise rospy.ROSException('Thruster parameters were not provided')
self._thruster_params = rospy.get_param('~thruster_model')
if 'max_thrust' not in self._thruster_params:
raise rospy.ROSException('No limit to thruster output was given')
self._thruster_model = Thruster.create_thruster(
self._thruster_params['name'], 0,
self._thruster_topic, None, None,
**self._thruster_params['params'])
self._logger.info('Publishing to thruster=%s' % self._thruster_topic)
self._is_init = True
self._logger.info('AUV Geometric Tracking Controller ready!')
def __init__(self):
self.namespace = rospy.get_namespace().replace('/', '')
rospy.loginfo('Initialize control for vehicle <%s>' % self.namespace)
self.local_planner = DPControllerLocalPlanner(full_dof=True, thrusters_only=False,
stamped_pose_only=False)
self.base_link = rospy.get_param('~base_link', 'base_link')
# Reading the minimum thrust generated
self.min_thrust = rospy.get_param('~min_thrust', 0)
assert self.min_thrust >= 0
rospy.loginfo('Min. thrust [N]=%.2f', self.min_thrust)
# Reading the maximum thrust generated
self.max_thrust = rospy.get_param('~max_thrust', 0)
assert self.max_thrust > 0 and self.max_thrust > self.min_thrust
rospy.loginfo('Max. thrust [N]=%.2f', self.max_thrust)
# Reading the thruster topic
self.thruster_topic = rospy.get_param('~thruster_topic', 'thrusters/0/input')
assert len(self.thruster_topic) > 0
# Reading the thruster gain
self.p_gain_thrust = rospy.get_param('~thrust_p_gain', 0.0)
assert self.p_gain_thrust > 0
self.d_gain_thrust = rospy.get_param('~thrust_d_gain', 0.0)
assert self.d_gain_thrust >= 0
# Reading the roll gain
self.p_roll = rospy.get_param('~p_roll', 0.0)
assert self.p_roll > 0
# Reading the pitch P gain
self.p_pitch = rospy.get_param('~p_pitch', 0.0)
assert self.p_pitch > 0
# Reading the pitch D gain
self.d_pitch = rospy.get_param('~d_pitch', 0.0)
assert self.d_pitch >= 0
# Reading the yaw P gain
self.p_yaw = rospy.get_param('~p_yaw', 0.0)
assert self.p_yaw > 0
# Reading the yaw D gain
self.d_yaw = rospy.get_param('~d_yaw', 0.0)
assert self.d_yaw >= 0
# Reading the saturation for the desired pitch
self.desired_pitch_limit = rospy.get_param('~desired_pitch_limit', 15 * np.pi / 180)
assert self.desired_pitch_limit > 0
# Reading the saturation for yaw error
self.yaw_error_limit = rospy.get_param('~yaw_error_limit', 1.0)
assert self.yaw_error_limit > 0
# Reading the number of fins
self.n_fins = rospy.get_param('~n_fins', 0)
assert self.n_fins > 0
# Reading the mapping for roll commands
self.map_roll = rospy.get_param('~map_roll', [0, 0, 0, 0])
assert isinstance(self.map_roll, list)
assert len(self.map_roll) == self.n_fins
# Reading the mapping for the pitch commands
self.map_pitch = rospy.get_param('~map_pitch', [0, 0, 0, 0])
assert isinstance(self.map_pitch, list)
assert len(self.map_pitch) == self.n_fins
# Reading the mapping for the yaw commands
self.map_yaw = rospy.get_param('~map_yaw', [0, 0, 0, 0])
assert isinstance(self.map_yaw, list)
assert len(self.map_yaw) == self.n_fins
# Retrieve the thruster configuration parameters
self.thruster_config = rospy.get_param('~thruster_config')
# Check if all necessary thruster model parameter are available
thruster_params = ['conversion_fcn_params', 'conversion_fcn',
'topic_prefix', 'topic_suffix', 'frame_base', 'max_thrust']
for p in thruster_params:
if p not in self.thruster_config:
raise rospy.ROSException(
'Parameter <%s> for thruster conversion function is '
'missing' % p)
# Setting up the thruster topic name
self.thruster_topic = '/%s/%s/%d/%s' % (self.namespace,
self.thruster_config['topic_prefix'], 0,
self.thruster_config['topic_suffix'])
base = '%s/%s' % (self.namespace, self.base_link)
frame = '%s/%s%d' % (self.namespace, self.thruster_config['frame_base'], 0)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
rospy.loginfo('Lookup: Thruster transform found %s -> %s' % (base, frame))
trans = self.tf_buffer.lookup_transform(base, frame, rospy.Time(), rospy.Duration(5))
pos = np.array([trans.transform.translation.x,
trans.transform.translation.y,
trans.transform.translation.z])
quat = np.array([trans.transform.rotation.x,
trans.transform.rotation.y,
trans.transform.rotation.z,
trans.transform.rotation.w])
rospy.loginfo('Thruster transform found %s -> %s' % (base, frame))
rospy.loginfo('pos=' + str(pos))
rospy.loginfo('rot=' + str(quat))
# Read transformation from thruster
self.thruster = Thruster.create_thruster(
self.thruster_config['conversion_fcn'], 0,
self.thruster_topic, pos, quat,
**self.thruster_config['conversion_fcn_params'])
rospy.loginfo('Thruster configuration=\n' + str(self.thruster_config))
rospy.loginfo('Thruster input topic=' + self.thruster_topic)
self.max_fin_angle = rospy.get_param('~max_fin_angle', 0.0)
assert self.max_fin_angle > 0
# Reading the fin input topic prefix
self.fin_topic_prefix = rospy.get_param('~fin_topic_prefix', 'fins')
self.fin_topic_suffix = rospy.get_param('~fin_topic_suffix', 'input')
self.rpy_to_fins = np.vstack((self.map_roll, self.map_pitch, self.map_yaw)).T
self.pub_cmd = list()
for i in range(self.n_fins):
topic = '%s/%d/%s' % (self.fin_topic_prefix, i, self.fin_topic_suffix)
self.pub_cmd.append(
rospy.Publisher(topic, FloatStamped, queue_size=10))
self.odometry_sub = rospy.Subscriber(
'odom', Odometry, self.odometry_callback)
self.reference_pub = rospy.Publisher(
'reference', TrajectoryPoint, queue_size=1)
# Publish error (for debugging)
self.error_pub = rospy.Publisher(
'error', TrajectoryPoint, queue_size=1)
def __init__(self):
print('FinnedUUVControllerNode: initializing node')
self._ready = False
# Test if any of the needed parameters are missing
param_labels = ['n_fins', 'gain_roll', 'gain_pitch', 'gain_yaw',
'thruster_model', 'fin_topic_prefix',
'fin_topic_suffix', 'thruster_topic',
'axis_thruster', 'axis_roll', 'axis_pitch', 'axis_yaw']
for label in param_labels:
if not rospy.has_param('~%s' % label):
raise rospy.ROSException('Parameter missing, label=%s' % label)
# Number of fins
self._n_fins = rospy.get_param('~n_fins')
# Thruster joy axis gain
self._thruster_joy_gain = 1
if rospy.has_param('~thruster_joy_gain'):
self._thruster_joy_gain = rospy.get_param('~thruster_joy_gain')
# Read the vector for contribution of each fin on the change on
# orientation
gain_roll = rospy.get_param('~gain_roll')
gain_pitch = rospy.get_param('~gain_pitch')
gain_yaw = rospy.get_param('~gain_yaw')
if len(gain_roll) != self._n_fins or len(gain_pitch) != self._n_fins \
or len(gain_yaw) != self._n_fins:
raise rospy.ROSException('Input gain vectors must have length '
'equal to the number of fins')
# Create the command angle to fin angle mapping
self._rpy_to_fins = numpy.vstack((gain_roll, gain_pitch, gain_yaw)).T
# Read the joystick mapping
self._joy_axis = dict(axis_thruster=rospy.get_param('~axis_thruster'),
axis_roll=rospy.get_param('~axis_roll'),
axis_pitch=rospy.get_param('~axis_pitch'),
axis_yaw=rospy.get_param('~axis_yaw'))
# Subscribe to the fin angle topics
self._pub_cmd = list()
self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix')
self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix')
for i in range(self._n_fins):
topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
self._pub_cmd.append(
rospy.Publisher(topic, FloatStamped, queue_size=10))
# Create the thruster model object
try:
self._thruster_topic = rospy.get_param('~thruster_topic')
self._thruster_params = rospy.get_param('~thruster_model')
if 'max_thrust' not in self._thruster_params:
raise rospy.ROSException('No limit to thruster output was given')
self._thruster_model = Thruster.create_thruster(
self._thruster_params['name'], 0,
self._thruster_topic, None, None,
**self._thruster_params['params'])
except:
raise rospy.ROSException('Thruster model could not be initialized')
# Subscribe to the joystick topic
self.sub_joy = rospy.Subscriber('joy', numpy_msg(Joy),
self.joy_callback)
self._ready = True