16 from fin_model
import FinModel
20 from tf_quaternion.transformations
import quaternion_matrix
21 from uuv_thrusters.models
import Thruster
22 from uuv_auv_control_allocator.msg
import AUVCommand
23 from uuv_gazebo_ros_plugins_msgs.msg
import FloatStamped
24 from geometry_msgs.msg
import Wrench, WrenchStamped
35 self.
namespace = rospy.get_namespace().replace(
'/',
'')
36 rospy.loginfo(
'Initialize control allocator for vehicle <%s>' % self.
namespace)
40 tf_trans_ned_to_enu =
None 44 source =
'%s/base_link_ned' % self.
namespace 45 rospy.loginfo(
'Lookup transfrom from %s to %s' % (source, target))
46 tf_trans_ned_to_enu = self.tf_buffer.lookup_transform(
47 target, source, rospy.Time(), rospy.Duration(1))
49 print(
'No transform found between base_link and base_link_ned' 54 if tf_trans_ned_to_enu
is not None:
56 (tf_trans_ned_to_enu.transform.rotation.x,
57 tf_trans_ned_to_enu.transform.rotation.y,
58 tf_trans_ned_to_enu.transform.rotation.z,
59 tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
63 self.
base_link = rospy.get_param(
'~base_link',
'base_link')
66 if not rospy.has_param(
'~thruster_config'):
67 raise rospy.ROSException(
'Thruster configuration not available')
73 thruster_params = [
'conversion_fcn_params',
'conversion_fcn',
74 'topic_prefix',
'topic_suffix',
'frame_base',
'max_thrust']
75 for p
in thruster_params:
77 raise rospy.ROSException(
78 'Parameter <%s> for thruster conversion function is missing' % p)
87 if not rospy.has_param(
'~fin_config'):
88 raise rospy.ROSException(
'Fin configuration is not available')
94 fin_params = [
'fluid_density',
'lift_coefficient',
'fin_area',
95 'topic_prefix',
'topic_suffix',
'frame_base']
99 raise rospy.ROSException(
100 'Parameter <%s> for fin configuration is missing' % p)
111 raise rospy.ROSException(
'Fin angle limits are invalid')
118 raise rospy.ROSException(
'No thruster and/or fins found')
121 """Calculate the control allocation matrix, if one is not given.""" 124 rospy.loginfo(
'ControlAllocator: updating thruster poses')
130 rospy.loginfo(
'Lookup: Thruster transform found %s -> %s' % (base, frame))
131 trans = self.tf_buffer.lookup_transform(base, frame, rospy.Time(), rospy.Duration(1))
132 pos = np.array([trans.transform.translation.x,
133 trans.transform.translation.y,
134 trans.transform.translation.z])
135 quat = np.array([trans.transform.rotation.x,
136 trans.transform.rotation.y,
137 trans.transform.rotation.z,
138 trans.transform.rotation.w])
139 rospy.loginfo(
'Thruster transform found %s -> %s' % (base, frame))
140 rospy.loginfo(
'pos=' + str(pos))
141 rospy.loginfo(
'rot=' + str(quat))
144 self.
thruster = Thruster.create_thruster(
153 rospy.loginfo(
'Lookup: Fin transform found %s -> %s' % (base, frame))
154 trans = self.tf_buffer.lookup_transform(base, frame, rospy.Time(), rospy.Duration(1))
155 pos = np.array([trans.transform.translation.x,
156 trans.transform.translation.y,
157 trans.transform.translation.z])
158 quat = np.array([trans.transform.rotation.x,
159 trans.transform.rotation.y,
160 trans.transform.rotation.z,
161 trans.transform.rotation.w])
162 rospy.loginfo(
'Fin transform found %s -> %s' % (base, frame))
163 rospy.loginfo(
'pos=' + str(pos))
164 rospy.loginfo(
'quat=' + str(quat))
166 fin_topic =
'/%s/%s/%d/%s' % (self.
namespace,
169 self.
fins[i] = FinModel(
175 except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
176 rospy.loginfo(
'Could not get transform from %s to %s ' % (base, frame))
179 self.
n_fins = len(self.fins.keys())
180 rospy.loginfo(
'# fins found: %d' % len(self.fins.keys()))
182 for i
in range(self.
n_fins):
184 rospy.loginfo(self.
fins[i].pos)
185 rospy.loginfo(self.
fins[i].rot)
191 actuator_model = self.thruster.tam_column.reshape((6, 1)) * thrust
193 f_lift = (0.5 * self.
fin_config[
'fluid_density'] *
198 tau[0:3] = f_lift * self.
fins[i].lift_vector
199 tau[3::] = np.cross(self.
fins[i].pos, f_lift)
201 actuator_model += tau
202 return actuator_model
205 self.thruster.publish_command(command[0])
207 for i
in range(self.
n_fins):
208 self.
fins[i].publish_command(command[i + 1])
def compute_control_force(self, thrust, delta, u)
def publish_commands(self, command)