17 from tf_quaternion
import transformations
18 from uuv_gazebo_ros_plugins_msgs.msg
import FloatStamped
19 from std_msgs.msg
import Header
23 """Abstract function to all the thruster models avaialble. 24 The instance of a thruster model must use the factory method. 28 * `index` (*type:* `int`): Thruster's ID. 29 * `topic` (*type:* `str`): Thruster's command topic. 30 * `pos` (*type:* `numpy.array` or `list`): Position vector 31 of the thruster with respect to the vehicle's frame. 32 * `orientation` (*type:* `numpy.array` or `list`): Quaternion 33 with the orientation of the thruster with respect to the vehicle's 34 frame as `(qx, qy, qz, qw)`. 35 * `axis` (*type:* `numpy.array`): Axis of rotation of the thruster. 38 DEFAULT_AXIS=numpy.array([1, 0, 0, 0])
40 def __init__(self, index, topic, pos, orientation, axis=DEFAULT_AXIS):
46 if pos
is not None and orientation
is not None:
50 thrust_body = transformations.quaternion_matrix(orientation).dot(
51 axis.transpose())[0:3]
52 torque_body = numpy.cross(pos, thrust_body)
54 thrust_body, torque_body)).transpose()
60 rospy.loginfo(
'Thruster #{} - {} - {}'.format(
77 """Factory method for the thruster models. 81 * `model_name` (*type:* `str`): Name identifier of 86 Thruster model instance. 88 for thruster
in Thruster.__subclasses__():
89 if model_name == thruster.LABEL:
90 return thruster(*args, **kwargs)
91 raise rospy.ROSException(
'Invalid thruster model')
94 """Convert desired thrust force to input command according to this 95 function. Overwrite this method to implement custom models.""" 96 raise NotImplementedError()
99 """Computes the thrust force for the given command.""" 100 raise NotImplementedError()
103 """Sample the conversion curve and return the values.""" 104 if min_value >= max_value
or n_points <= 0:
106 input_values = numpy.linspace(min_value, max_value, n_points)
108 for value
in input_values:
110 return input_values.tolist(), output_values
113 """Convert the desired thrust force into angular velocity 114 command according using a gain.""" 122 """Publish the thrust force command set-point 127 * `thrust` (*type:* `float`): Thrust force set-point 131 output = FloatStamped()
132 output.header.stamp = rospy.Time.now()
134 self._command_pub.publish(output)
def get_curve(self, min_value, max_value, n_points)
def _update(self, thrust)
def create_thruster(model_name, args, kwargs)
def get_thrust_value(self, command)
def __init__(self, index, topic, pos, orientation, axis=DEFAULT_AXIS)
def publish_command(self, thrust)
def get_command_value(self, thrust)