def _create_vehicle_model(self):
"""Create a new instance of a vehicle model. If controller is not model
based, this model will have its parameters set to 0 and will be used
to receive and transform the odometry data.
"""
if self._vehicle_model is not None:
del self._vehicle_model
self._vehicle_model = Vehicle(
inertial_frame_id=self._local_planner.inertial_frame_id)
def _create_vehicle_model(self):
"""
Create a new instance of a vehicle model. If controller is not model
based, this model will have its parameters set to 0 and will be used
to receive and transform the odometry data.
"""
if self._vehicle_model is not None:
del self._vehicle_model
self._vehicle_model = Vehicle(
inertial_frame_id=self._local_planner.inertial_frame_id)
class DPControllerBase(object):
"""General abstract class for DP controllers for underwater vehicles.
This is an abstract class, must be inherited by a controller module that
overrides the update_controller method. If the controller is set to be
model based (is_model_based=True), than the vehicle parameters are going
to be read from the ROS parameter server.
> *Input arguments*
* `is_model_based` (*type:* `bool`, *default:* `False`): If `True`, the
controller uses a model of the vehicle, `False` if it is non-model-based.
* `list_odometry_callbacks` (*type:* `list`, *default:* `None`): List of
function handles or `lambda` functions that will be called after each
odometry update.
* `planner_full_dof` (*type:* `bool`, *default:* `False`): Set the local
planner to generate 6 DoF trajectories. Otherwise, the planner will only
generate 4 DoF trajectories `(x, y, z, yaw)`.
> *ROS parameters*
* `use_stamped_poses_only` (*type:* `bool`, *default:* `False`): If `True`,
the reference path will be generated with stamped poses only, velocity
and acceleration references are set to zero.
* `thrusters_only` (*type:* `bool`, *default:* `True`): If `True`, the
vehicle only has thrusters as actuators and a thruster manager node is
running and the control output will be published as `geometry_msgs/WrenchStamped`.
If `False`, the AUV force allocation should be used to compute
the control output for each actuator and the output control will be
generated as a `uuv_auv_control_allocator.AUVCommand` message.
* `saturation` (*type:* `float`, *default:* `5000`): Absolute saturation
of the control signal.
* `use_auv_control_allocator` (*type:* `bool`, *default:* `False`): If `True`,
the output control will be `AUVCommand` message.
* `min_thrust` (*type:* `float`, *default:* `40`): Min. thrust set-point output,
(parameter only valid for AUVs).
> *ROS publishers*
* `thruster_output` (*message:* `geometry_msgs/WrenchStamped`): Control set-point
for the thruster manager node (requirement: ROS parameters must be `thrusters_only`
must be set as `True` and a thruster manager from `uuv_thruster_manager` node must
be running).
* `auv_command_output` (*message:* `uuv_auv_control_allocator.AUVCommand`): Control
set-point for the AUV allocation node (requirement: ROS parameters must be
`thrusters_only` must be set as `False` and a AUV control allocation node from
`uuv_auv_control_allocator` node must be running).
* `reference` (*message:* `uuv_control_msgs/TrajectoryPoint`): Current reference
trajectory point.
* `error` (*message:* `uuv_control_msgs/TrajectoryPoint`): Current trajectory error.
> *ROS services*
* `reset_controller` (*service:* `uuv_control_msgs/ResetController`): Reset all
variables, including error and reference signals.
"""
_LABEL = ''
def __init__(self,
is_model_based=False,
list_odometry_callbacks=None,
planner_full_dof=False):
# Flag will be set to true when all parameters are initialized correctly
self._is_init = False
self._logger = get_logger()
# Reading current namespace
self._namespace = rospy.get_namespace()
# Configuration for the vehicle dynamic model
self._is_model_based = is_model_based
if self._is_model_based:
self._logger.info('Setting controller as model-based')
else:
self._logger.info('Setting controller as non-model-based')
self._use_stamped_poses_only = False
if rospy.has_param('~use_stamped_poses_only'):
self._use_stamped_poses_only = rospy.get_param(
'~use_stamped_poses_only')
# Flag indicating if the vehicle has only thrusters, otherwise
# the AUV allocation node will be used
self.thrusters_only = rospy.get_param('~thrusters_only', True)
# Instance of the local planner for local trajectory generation
self._local_planner = LocalPlanner(
full_dof=planner_full_dof,
stamped_pose_only=self._use_stamped_poses_only,
thrusters_only=self.thrusters_only)
self._control_saturation = 5000
# TODO: Fix the saturation term and how it is applied
if rospy.has_param('~saturation'):
self._thrust_saturation = rospy.get_param('~saturation')
if self._control_saturation <= 0:
raise rospy.ROSException('Invalid control saturation forces')
# Flag indicating either use of the AUV control allocator or
# direct command of fins and thruster
self.use_auv_control_allocator = False
if not self.thrusters_only:
self.use_auv_control_allocator = rospy.get_param(
'~use_auv_control_allocator', False)
# Remap the following topics, if needed
# Publisher for thruster allocator
if self.thrusters_only:
self._thrust_pub = rospy.Publisher('thruster_output',
WrenchStamped,
queue_size=1)
else:
self._thrust_pub = None
if not self.thrusters_only:
self._auv_command_pub = rospy.Publisher('auv_command_output',
AUVCommand,
queue_size=1)
else:
self._auv_command_pub = None
self._min_thrust = rospy.get_param('~min_thrust', 40.0)
self._reference_pub = rospy.Publisher('reference',
TrajectoryPoint,
queue_size=1)
# Publish error (for debugging)
self._error_pub = rospy.Publisher('error',
TrajectoryPoint,
queue_size=1)
self._init_reference = False
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6),
acc=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6))
# Time step
self._dt = 0
self._prev_time = rospy.get_time()
self._services = dict()
self._services['reset'] = rospy.Service('reset_controller',
ResetController,
self.reset_controller_callback)
# Time stamp for the received trajectory
self._stamp_trajectory_received = rospy.get_time()
# Instance of the vehicle model
self._vehicle_model = None
# If list of callbacks is empty, set the default
if list_odometry_callbacks is not None and \
isinstance(list_odometry_callbacks, list):
self._odometry_callbacks = list_odometry_callbacks
else:
self._odometry_callbacks = [
self.update_errors, self.update_controller
]
# Initialize vehicle, if model based
self._create_vehicle_model()
# Flag to indicate that odometry topic is receiving data
self._init_odom = False
# Subscribe to odometry topic
self._odom_topic_sub = rospy.Subscriber('odom', numpy_msg(Odometry),
self._odometry_callback)
# Stores last simulation time
self._prev_t = -1.0
self._logger.info('DP controller successfully initialized')
def __del__(self):
# Removing logging message handlers
while self._logger.handlers:
self._logger.handlers.pop()
@staticmethod
def get_controller(name, *args):
"""Create instance of a specific DP controller."""
for controller in DPControllerBase.__subclasses__():
if name == controller.__name__:
self._logger.info('Creating controller={}'.format(name))
return controller(*args)
@staticmethod
def get_list_of_controllers():
"""Return list of DP controllers using this interface."""
return [
controller.__name__
for controller in DPControllerBase.__subclasses__()
]
@property
def label(self):
"""`str`: Identifier name of the controller"""
return self._LABEL
@property
def odom_is_init(self):
"""`bool`: `True` if the first odometry message was received"""
return self._init_odom
@property
def error_pos_world(self):
"""`numpy.array`: Position error wrt world frame"""
return np.dot(self._vehicle_model.rotBtoI, self._errors['pos'])
@property
def error_orientation_quat(self):
"""`numpy.array`: Orientation error"""
return deepcopy(self._errors['rot'][0:3])
@property
def error_orientation_rpy(self):
"""`numpy.array`: Orientation error in Euler angles."""
e1 = self._errors['rot'][0]
e2 = self._errors['rot'][1]
e3 = self._errors['rot'][2]
eta = self._errors['rot'][3]
rot = np.array([[
1 - 2 * (e2**2 + e3**2), 2 * (e1 * e2 - e3 * eta),
2 * (e1 * e3 + e2 * eta)
],
[
2 * (e1 * e2 + e3 * eta), 1 - 2 * (e1**2 + e3**2),
2 * (e2 * e3 - e1 * eta)
],
[
2 * (e1 * e3 - e2 * eta), 2 * (e2 * e3 + e1 * eta),
1 - 2 * (e1**2 + e2**2)
]])
# Roll
roll = np.arctan2(rot[2, 1], rot[2, 2])
# Pitch, treating singularity cases
den = np.sqrt(1 - rot[2, 1]**2)
pitch = -np.arctan(rot[2, 1] / max(0.001, den))
# Yaw
yaw = np.arctan2(rot[1, 0], rot[0, 0])
return np.array([roll, pitch, yaw])
@property
def error_pose_euler(self):
"""`numpy.array`: Pose error with orientation represented in Euler angles."""
return np.hstack((self._errors['pos'], self.error_orientation_rpy))
@property
def error_vel_world(self):
"""`numpy.array`: Linear velocity error"""
return np.dot(self._vehicle_model.rotBtoI, self._errors['vel'])
def __str__(self):
msg = 'Dynamic positioning controller\n'
msg += 'Controller= ' + self._LABEL + '\n'
msg += 'Is model based? ' + str(self._is_model_based) + '\n'
msg += 'Vehicle namespace= ' + self._namespace
return msg
def _create_vehicle_model(self):
"""Create a new instance of a vehicle model. If controller is not model
based, this model will have its parameters set to 0 and will be used
to receive and transform the odometry data.
"""
if self._vehicle_model is not None:
del self._vehicle_model
self._vehicle_model = Vehicle(
inertial_frame_id=self._local_planner.inertial_frame_id)
def _update_reference(self):
"""Call the local planner interpolator to retrieve a trajectory
point and publish the reference message as `uuv_control_msgs/TrajectoryPoint`.
"""
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(self._vehicle_model.pos,
self._vehicle_model.quat)
t = rospy.get_time()
reference = self._local_planner.interpolate(t)
if reference is not None:
self._reference['pos'] = reference.p
self._reference['rot'] = reference.q
self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['acc'] = np.hstack((reference.a, reference.alpha))
if reference is not None and self._reference_pub.get_num_connections(
) > 0:
# Publish current reference
msg = TrajectoryPoint()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self._local_planner.inertial_frame_id
msg.pose.position = Vector3(*self._reference['pos'])
msg.pose.orientation = Quaternion(*self._reference['rot'])
msg.velocity.linear = Vector3(*self._reference['vel'][0:3])
msg.velocity.angular = Vector3(*self._reference['vel'][3:6])
msg.acceleration.linear = Vector3(*self._reference['acc'][0:3])
msg.acceleration.angular = Vector3(*self._reference['acc'][3:6])
self._reference_pub.publish(msg)
return True
def _update_time_step(self):
"""Update time step."""
t = rospy.get_time()
self._dt = t - self._prev_time
self._prev_time = t
def _reset_controller(self):
"""Reset reference and and error vectors."""
self._init_reference = False
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6),
acc=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6))
def reset_controller_callback(self, request):
"""Service handler function."""
self._reset_controller()
return ResetControllerResponse(True)
def update_controller(self):
"""This function must be implemented by derived classes
with the implementation of the control algorithm.
"""
# Does nothing, must be overloaded
raise NotImplementedError()
def update_errors(self):
"""Update error vectors."""
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - pos)
# Update orientation error
self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat),
self._reference['rot'])
# Velocity error with respect to the the BODY frame
self._errors['vel'] = np.hstack(
(np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
if self._error_pub.get_num_connections() > 0:
stamp = rospy.Time.now()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(
*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(
*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def publish_control_wrench(self, force):
"""Publish the thruster manager control set-point.
> *Input arguments*
* `force` (*type:* `numpy.array`): 6 DoF control
set-point wrench vector
"""
if not self.odom_is_init:
return
# Apply saturation
for i in range(6):
if force[i] < -self._control_saturation:
force[i] = -self._control_saturation
elif force[i] > self._control_saturation:
force[i] = self._control_saturation
if not self.thrusters_only:
surge_speed = self._vehicle_model.vel[0]
self.publish_auv_command(surge_speed, force)
return
force_msg = WrenchStamped()
force_msg.header.stamp = rospy.Time.now()
force_msg.header.frame_id = '%s/%s' % (
self._namespace, self._vehicle_model.body_frame_id)
force_msg.wrench.force.x = force[0]
force_msg.wrench.force.y = force[1]
force_msg.wrench.force.z = force[2]
force_msg.wrench.torque.x = force[3]
force_msg.wrench.torque.y = force[4]
force_msg.wrench.torque.z = force[5]
self._thrust_pub.publish(force_msg)
def publish_auv_command(self, surge_speed, wrench):
"""Publish the AUV control command message
> *Input arguments*
* `surge_speed` (*type:* `float`): Reference surge speed
* `wrench` (*type:* `numpy.array`): 6 DoF wrench vector
"""
if not self.odom_is_init:
return
surge_speed = max(0, surge_speed)
msg = AUVCommand()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/%s' % (self._namespace,
self._vehicle_model.body_frame_id)
msg.surge_speed = surge_speed
msg.command.force.x = max(self._min_thrust, wrench[0])
msg.command.force.y = wrench[1]
msg.command.force.z = wrench[2]
msg.command.torque.x = wrench[3]
msg.command.torque.y = wrench[4]
msg.command.torque.z = wrench[5]
self._auv_command_pub.publish(msg)
def _odometry_callback(self, msg):
"""Odometry topic subscriber callback function.
> *Input arguments*
* `msg` (*type:* `nav_msgs/Odometry`): Input odometry
message
"""
self._vehicle_model.update_odometry(msg)
if not self._init_odom:
self._init_odom = True
if len(self._odometry_callbacks):
for func in self._odometry_callbacks:
func()
class DPControllerBase(object):
"""General abstract class for DP controllers for underwater vehicles.
This is an abstract class, must be inherited by a controller module that
overrides the update_controller method. If the controller is set to be
model based (is_model_based=True), than the vehicle parameters are going
to be read from the ROS parameter server.
"""
_LABEL = ''
def __init__(self,
is_model_based=False,
list_odometry_callbacks=[],
planner_full_dof=False):
# Flag will be set to true when all parameters are initialized correctly
self._is_init = False
self._logger = logging.getLogger('dp_controller')
out_hdlr = logging.StreamHandler(sys.stdout)
out_hdlr.setFormatter(
logging.Formatter(
rospy.get_namespace().replace('/', '').upper() +
' -- %(asctime)s | %(levelname)s | %(module)s | %(message)s'))
out_hdlr.setLevel(logging.INFO)
self._logger.addHandler(out_hdlr)
self._logger.setLevel(logging.INFO)
# Reading current namespace
self._namespace = rospy.get_namespace()
# Configuration for the vehicle dynamic model
self._is_model_based = is_model_based
if self._is_model_based:
self._logger.info('Setting controller as model-based')
else:
self._logger.info('Setting controller as non-model-based')
self._use_stamped_poses_only = False
if rospy.has_param('~use_stamped_poses_only'):
self._use_stamped_poses_only = rospy.get_param(
'~use_stamped_poses_only')
# Flag indicating if the vehicle has only thrusters, otherwise
# the AUV allocation node will be used
self.thrusters_only = rospy.get_param('~thrusters_only', True)
# Instance of the local planner for local trajectory generation
self._local_planner = LocalPlanner(
full_dof=planner_full_dof,
stamped_pose_only=self._use_stamped_poses_only,
thrusters_only=self.thrusters_only)
self._control_saturation = 5000
# TODO: Fix the saturation term and how it is applied
if rospy.has_param('~saturation'):
self._thrust_saturation = rospy.get_param('~saturation')
if self._control_saturation <= 0:
raise rospy.ROSException('Invalid control saturation forces')
# Flag indicating either use of the AUV control allocator or
# direct command of fins and thruster
self.use_auv_control_allocator = False
if not self.thrusters_only:
self.use_auv_control_allocator = rospy.get_param(
'~use_auv_control_allocator', False)
# Remap the following topics, if needed
# Publisher for thruster allocator
if self.thrusters_only:
self._thrust_pub = rospy.Publisher('thruster_output',
WrenchStamped,
queue_size=1)
else:
self._thrust_pub = None
if not self.thrusters_only:
self._auv_command_pub = rospy.Publisher('auv_command_output',
AUVCommand,
queue_size=1)
else:
self._auv_command_pub = None
self._min_thrust = rospy.get_param('~min_thrust', 40.0)
self._reference_pub = rospy.Publisher('reference',
TrajectoryPoint,
queue_size=1)
# Publish error (for debugging)
self._error_pub = rospy.Publisher('error',
TrajectoryPoint,
queue_size=1)
self._init_reference = False
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6),
acc=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6))
# Time step
self._dt = 0
self._prev_time = rospy.get_time()
self._services = dict()
self._services['reset'] = rospy.Service('reset_controller',
ResetController,
self.reset_controller_callback)
# Time stamp for the received trajectory
self._stamp_trajectory_received = rospy.get_time()
# Instance of the vehicle model
self._vehicle_model = None
# If list of callbacks is empty, set the default
if len(list_odometry_callbacks):
self._odometry_callbacks = list_odometry_callbacks
else:
self._odometry_callbacks = [
self.update_errors, self.update_controller
]
# Initialize vehicle, if model based
self._create_vehicle_model()
# Flag to indicate that odometry topic is receiving data
self._init_odom = False
# Subscribe to odometry topic
self._odom_topic_sub = rospy.Subscriber('/anahita/pose_gt',
numpy_msg(Odometry),
self._odometry_callback)
# Stores last simulation time
self._prev_t = -1.0
self._logger.info('DP controller successfully initialized')
def __del__(self):
# Removing logging message handlers
while self._logger.handlers:
self._logger.handlers.pop()
@staticmethod
def get_controller(name, *args):
"""Create instance of a specific DP controller."""
for controller in DPControllerBase.__subclasses__():
if name == controller.__name__:
print 'Creating controller=', name
return controller(*args)
@staticmethod
def get_list_of_controllers():
"""Return list of DP controllers using this interface."""
return [
controller.__name__
for controller in DPControllerBase.__subclasses__()
]
@property
def label(self):
return self._LABEL
@property
def odom_is_init(self):
return self._init_odom
@property
def error_pos_world(self):
return np.dot(self._vehicle_model.rotBtoI, self._errors['pos'])
@property
def error_orientation_quat(self):
return deepcopy(self._errors['rot'][0:3])
@property
def error_orientation_rpy(self):
"""Return orientation error in Euler angles."""
e1 = self._errors['rot'][0]
e2 = self._errors['rot'][1]
e3 = self._errors['rot'][2]
eta = self._errors['rot'][3]
rot = np.array([[
1 - 2 * (e2**2 + e3**2), 2 * (e1 * e2 - e3 * eta),
2 * (e1 * e3 + e2 * eta)
],
[
2 * (e1 * e2 + e3 * eta), 1 - 2 * (e1**2 + e3**2),
2 * (e2 * e3 - e1 * eta)
],
[
2 * (e1 * e3 - e2 * eta), 2 * (e2 * e3 + e1 * eta),
1 - 2 * (e1**2 + e2**2)
]])
# Roll
roll = np.arctan2(rot[2, 1], rot[2, 2])
# Pitch, treating singularity cases
den = np.sqrt(1 - rot[2, 1]**2)
pitch = -np.arctan(rot[2, 1] / max(0.001, den))
# Yaw
yaw = np.arctan2(rot[1, 0], rot[0, 0])
return np.array([roll, pitch, yaw])
@property
def error_pose_euler(self):
"""Pose error with orientation represented in Euler angles."""
return np.hstack((self._errors['pos'], self.error_orientation_rpy))
@property
def error_vel_world(self):
return np.dot(self._vehicle_model.rotBtoI, self._errors['vel'])
def __str__(self):
msg = 'Dynamic positioning controller\n'
msg += 'Controller= ' + self._LABEL + '\n'
msg += 'Is model based? ' + str(self._is_model_based) + '\n'
msg += 'Vehicle namespace= ' + self._namespace
return msg
def _create_vehicle_model(self):
"""
Create a new instance of a vehicle model. If controller is not model
based, this model will have its parameters set to 0 and will be used
to receive and transform the odometry data.
"""
if self._vehicle_model is not None:
del self._vehicle_model
self._vehicle_model = Vehicle(
inertial_frame_id=self._local_planner.inertial_frame_id)
def _update_reference(self):
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(self._vehicle_model.pos,
self._vehicle_model.quat)
t = rospy.get_time()
reference = self._local_planner.interpolate(t)
if reference is not None:
self._reference['pos'] = reference.p
self._reference['rot'] = reference.q
self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['acc'] = np.hstack((reference.a, reference.alpha))
# print '------------------ REFERENCE\n'
# print reference
if reference is not None and self._reference_pub.get_num_connections(
) > 0:
# Publish current reference
msg = TrajectoryPoint()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self._local_planner.inertial_frame_id
msg.pose.position = Vector3(*self._reference['pos'])
msg.pose.orientation = Quaternion(*self._reference['rot'])
msg.velocity.linear = Vector3(*self._reference['vel'][0:3])
msg.velocity.angular = Vector3(*self._reference['vel'][3:6])
msg.acceleration.linear = Vector3(*self._reference['acc'][0:3])
msg.acceleration.angular = Vector3(*self._reference['acc'][3:6])
self._reference_pub.publish(msg)
return True
def _update_time_step(self):
t = rospy.get_time()
self._dt = t - self._prev_time
self._prev_time = t
def _reset_controller(self):
self._init_reference = False
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6),
acc=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6))
def reset_controller_callback(self, request):
self._reset_controller()
return ResetControllerResponse(True)
def update_controller(self):
# Does nothing, must be overloaded
raise NotImplementedError()
def update_errors(self):
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - pos)
# Update orientation error
self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat),
self._reference['rot'])
# Velocity error with respect to the the BODY frame
self._errors['vel'] = np.hstack(
(np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
if self._error_pub.get_num_connections() > 0:
stamp = rospy.Time.now()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(
*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(
*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def publish_control_wrench(self, force):
if not self.odom_is_init:
return
# Apply saturation
for i in range(6):
if force[i] < -self._control_saturation:
force[i] = -self._control_saturation
elif force[i] > self._control_saturation:
force[i] = self._control_saturation
# if not self.thrusters_only:
# surge_speed = self._vehicle_model.vel[0]
# self.publish_auv_command(surge_speed, force)
# return
force_msg = WrenchStamped()
force_msg.header.stamp = rospy.Time.now()
force_msg.header.frame_id = '%s/%s' % (
self._namespace, self._vehicle_model.body_frame_id)
force_msg.wrench.force.x = force[0]
force_msg.wrench.force.y = force[1]
force_msg.wrench.force.z = force[2]
force_msg.wrench.torque.x = force[3]
force_msg.wrench.torque.y = force[4]
force_msg.wrench.torque.z = force[5]
self._thrust_pub.publish(force_msg)
def publish_auv_command(self, surge_speed, wrench):
if not self.odom_is_init:
return
surge_speed = max(0, surge_speed)
msg = AUVCommand()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/%s' % (self._namespace,
self._vehicle_model.body_frame_id)
msg.surge_speed = surge_speed
msg.command.force.x = max(self._min_thrust, wrench[0])
msg.command.force.y = wrench[1]
msg.command.force.z = wrench[2]
msg.command.torque.x = wrench[3]
msg.command.torque.y = wrench[4]
msg.command.torque.z = wrench[5]
self._auv_command_pub.publish(msg)
def _odometry_callback(self, msg):
"""Odometry topic subscriber callback function."""
# rospy.loginfo(msg)
self._vehicle_model.update_odometry(msg)
if not self._init_odom:
self._init_odom = True
if len(self._odometry_callbacks):
for func in self._odometry_callbacks:
func()
class DPControllerBase1(object):
"""General abstract class for DP controllers for underwater vehicles.
This is an abstract class, must be inherited by a controller module that
overrides the update_controller method. If the controller is set to be
model based (is_model_based=True), than the vehicle parameters are going
to be read from the ROS parameter server.
> *Input arguments*
* `is_model_based` (*type:* `bool`, *default:* `False`): If `True`, the
controller uses a model of the vehicle, `False` if it is non-model-based.
* `list_odometry_callbacks` (*type:* `list`, *default:* `None`): List of
function handles or `lambda` functions that will be called after each
odometry update.
* `planner_full_dof` (*type:* `bool`, *default:* `False`): Set the local
planner to generate 6 DoF trajectories. Otherwise, the planner will only
generate 4 DoF trajectories `(x, y, z, yaw)`.
> *ROS parameters*
* `use_stamped_poses_only` (*type:* `bool`, *default:* `False`): If `True`,
the reference path will be generated with stamped poses only, velocity
and acceleration references are set to zero.
* `thrusters_only` (*type:* `bool`, *default:* `True`): If `True`, the
vehicle only has thrusters as actuators and a thruster manager node is
running and the control output will be published as `geometry_msgs/WrenchStamped`.
If `False`, the AUV force allocation should be used to compute
the control output for each actuator and the output control will be
generated as a `uuv_auv_control_allocator.AUVCommand` message.
* `saturation` (*type:* `float`, *default:* `5000`): Absolute saturation
of the control signal.
* `use_auv_control_allocator` (*type:* `bool`, *default:* `False`): If `True`,
the output control will be `AUVCommand` message.
* `min_thrust` (*type:* `float`, *default:* `40`): Min. thrust set-point output,
(parameter only valid for AUVs).
> *ROS publishers*
* `thruster_output` (*message:* `geometry_msgs/WrenchStamped`): Control set-point
for the thruster manager node (requirement: ROS parameters must be `thrusters_only`
must be set as `True` and a thruster manager from `uuv_thruster_manager` node must
be running).
* `auv_command_output` (*message:* `uuv_auv_control_allocator.AUVCommand`): Control
set-point for the AUV allocation node (requirement: ROS parameters must be
`thrusters_only` must be set as `False` and a AUV control allocation node from
`uuv_auv_control_allocator` node must be running).
* `reference` (*message:* `uuv_control_msgs/TrajectoryPoint`): Current reference
trajectory point.
* `error` (*message:* `uuv_control_msgs/TrajectoryPoint`): Current trajectory error.
> *ROS services*
* `reset_controller` (*service:* `uuv_control_msgs/ResetController`): Reset all
variables, including error and reference signals.
"""
_LABEL = ''
def __init__(self,
is_model_based=False,
list_odometry_callbacks=None,
planner_full_dof=False):
# Flag will be set to true when all parameters are initialized correctly
self._is_init = False
self._logger = get_logger()
# Reading current namespace
self._namespace = rospy.get_namespace()
# Configuration for the vehicle dynamic model
self._is_model_based = is_model_based
if self._is_model_based:
self._logger.info('Setting controller as model-based')
else:
self._logger.info('Setting controller as non-model-based')
self._use_stamped_poses_only = False
if rospy.has_param('~use_stamped_poses_only'):
self._use_stamped_poses_only = rospy.get_param(
'~use_stamped_poses_only')
# Flag indicating if the vehicle has only thrusters, otherwise
# the AUV allocation node will be used
self.thrusters_only = rospy.get_param('~thrusters_only', True)
# Instance of the local planner for local trajectory generation
self._local_planner = LocalPlanner(
full_dof=planner_full_dof,
stamped_pose_only=self._use_stamped_poses_only,
thrusters_only=self.thrusters_only)
self._control_saturation = 150000
# TODO: Fix the saturation term and how it is applied
if rospy.has_param('~saturation'):
self._thrust_saturation = rospy.get_param('~saturation')
if self._control_saturation <= 0:
raise rospy.ROSException('Invalid control saturation forces')
# Flag indicating either use of the AUV control allocator or
# direct command of fins and thruster
self.use_auv_control_allocator = False
if not self.thrusters_only:
self.use_auv_control_allocator = rospy.get_param(
'~use_auv_control_allocator', False)
# Remap the following topics, if needed
# Publisher for thruster allocator
if self.thrusters_only:
self._thrust_pub = rospy.Publisher('thruster_output',
WrenchStamped,
queue_size=1)
else:
self._thrust_pub = None
if not self.thrusters_only:
self._auv_command_pub = rospy.Publisher('auv_command_output',
AUVCommand,
queue_size=1)
else:
self._auv_command_pub = None
self._min_thrust = rospy.get_param('~min_thrust', 40.0)
self._reference_pub = rospy.Publisher('reference',
TrajectoryPointObjectFollow,
queue_size=1)
# Publish error (for debugging)
self._error_pub = rospy.Publisher('error',
TrajectoryPointObjectFollow,
queue_size=1)
self._init_reference = False
self._equivalentControl_pub = rospy.Publisher('equivalentControl',
Vector6,
queue_size=10)
self._generalForce_pub = rospy.Publisher('generalForce',
Vector6,
queue_size=10)
self._restoring_pub = rospy.Publisher('restoring',
Vector6,
queue_size=10)
self._sliding_pub = rospy.Publisher('slidingSurface',
WrenchStamped,
queue_size=1)
self._count_pub = rospy.Publisher('count', Int32, queue_size=10)
self._dt_pub = rospy.Publisher('dt', Float32, queue_size=10)
self._dt_pub1 = rospy.Publisher('dt1', Float32, queue_size=10)
#self._ref_u_pub=rospy.Publisher('rexf_u', Float32, queue_size=10)
#self._ref_v_pub=rospy.Publisher('ref_v', Float32, queue_size=10)
#self._ref_w_pub=rospy.Publisher('ref_w', Float32, queue_size=10)
#self._veh_u_pub=rospy.Publisher('veh_u', Float32, queue_size=10)
#self._veh_v_pub=rospy.Publisher('veh_v', Float32, queue_size=10)
#self._veh_w_pub=rospy.Publisher('veh_w', Float32, queue_size=10)
#self._error_up_pub=rospy.Publisher('error_up', Float32, queue_size=10)
#self._surface_up_pub=rospy.Publisher('surface_up', Float32, queue_size=10)
#self._force_up_pub=rospy.Publisher('force_up', Float32, queue_size=10)
self._MParameter_pub = rospy.Publisher('MParameter',
Matrix6,
queue_size=10)
self._CParameter_pub = rospy.Publisher('CParameter',
Matrix6,
queue_size=10)
self._DParameter_pub = rospy.Publisher('DParameter',
Matrix6,
queue_size=10)
self._vel_pub = rospy.Publisher('vel', Vector6, queue_size=10)
self._count = 0
self._imu_topic_sub = rospy.Subscriber('/rexrov2/imu', Imu,
self.imuCallback)
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6))
self._errors_refDelay = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6))
# Time step
self._dt = 0
self._prev_time = rospy.get_time()
self._services = dict()
self._services['reset'] = rospy.Service('reset_controller',
ResetController,
self.reset_controller_callback)
# Time stamp for the received trajectory
self._stamp_trajectory_received = rospy.get_time()
# Instance of the vehicle model
self._vehicle_model = None
# If list of callbacks is empty, set the default
if list_odometry_callbacks is not None and \
isinstance(list_odometry_callbacks, list):
self._odometry_callbacks = list_odometry_callbacks
else:
self._odometry_callbacks = [
self.update_errors, self.update_controller
]
# Initialize vehicle, if model based
self._create_vehicle_model()
# Flag to indicate that odometry topic is receiving data
self._init_odom = False
# Subscribe to odometry topic
self._odom_topic_sub = rospy.Subscriber('odom', numpy_msg(Odometry),
self._odometry_callback)
# Stores last simulation time
self._prev_t = -1.0
self._logger.info('DP controller successfully initialized')
self._boxOdom_topic_sub = rospy.Subscriber('/ground_truth/stateRexRov',
Odometry,
self.objectCallback)
#self.ref_boxPosition=np.array([0, 0, 0]);
self.ref_boxPosition = np.zeros(3)
self.ref_boxOrientation = np.array([0, 0, 0, 0])
self.ref_boxVelocityLinear = np.array([0, 0, 0])
#self.ref_boxVelocityLinear=np.zeros(3)
#self.ref_boxVelocityLinear1=np.array([0, 0, 0]);
#self.ref_boxPosition_prev=np.array([0, 0, 0]);
self.ref_boxVelocityLinear1 = np.zeros(3)
self.ref_boxPosition_prev = np.zeros(3)
self._prev_t = 0
self.ref_boxVelocityAngular = np.zeros(3)
#self.ref_boxVelocityAngular=np.array([0,0,0])
self.object_position = np.array([0, 0, 0])
self.object_orientation = np.array([0, 0, 0, 0])
self.q = Quaternion
self.object_velocityLinear = np.array([0, 0, 0])
self.object_velocityAngular = np.zeros(3)
self.pose_xytheta = Pose
self.x_wg = Float32()
self.y_wg = Float32()
self.theta_wg = Float32()
self.pose_vehicle = np.array([0, 0, 0])
self.imuAccLinear = np.array([0, 0, 0])
self._linear_acceleration = np.zeros(3)
self.pos_ref_prev1 = np.array([0, 0, -15])
self.pos_ref_prev2 = np.array([0, 0, -15])
self.pos_ref_prev3 = np.array([0, 0, -15])
self.pos_ref_prev4 = np.array([0, 0, -15])
self.pos_ref_prev5 = np.array([0, 0, -15])
self.pos_ref_prev6 = np.array([0, 0, -15])
self.pos_ref_prev7 = np.array([0, 0, -15])
self.pos_ref_prev8 = np.array([0, 0, -15])
self.pos_ref_prev9 = np.array([0, 0, -15])
self.pos_ref_prev10 = np.array([0, 0, -15])
self.pos_ref_prev11 = np.array([0, 0, -15])
self.pos_ref_prev12 = np.array([0, 0, -15])
self.pos_ref_prev13 = np.array([0, 0, -15])
self.pos_ref_prev14 = np.array([0, 0, -15])
self.rot_ref_prev1 = np.zeros(4)
self.rot_ref_prev2 = np.zeros(4)
self.rot_ref_prev3 = np.zeros(4)
self.rot_ref_prev4 = np.zeros(4)
self.rot_ref_prev5 = np.zeros(4)
self.rot_ref_prev6 = np.zeros(4)
self.rot_ref_prev7 = np.zeros(4)
self.rot_ref_prev8 = np.zeros(4)
self.rot_ref_prev9 = np.zeros(4)
self.rot_ref_prev10 = np.zeros(4)
self.rot_ref_prev11 = np.zeros(4)
self.rot_ref_prev12 = np.zeros(4)
self.rot_ref_prev13 = np.zeros(4)
self.rot_ref_prev14 = np.zeros(4)
self.vel_ref_prev1 = np.zeros(6)
self.vel_ref_prev2 = np.zeros(6)
self.vel_ref_prev3 = np.zeros(6)
self.vel_ref_prev4 = np.zeros(6)
self.vel_ref_prev5 = np.zeros(6)
self.vel_ref_prev6 = np.zeros(6)
self.vel_ref_prev7 = np.zeros(6)
self.vel_ref_prev8 = np.zeros(6)
self.vel_ref_prev9 = np.zeros(6)
self.vel_ref_prev10 = np.zeros(6)
self.vel_ref_prev11 = np.zeros(6)
self.vel_ref_prev12 = np.zeros(6)
self.vel_ref_prev13 = np.zeros(6)
self.vel_ref_prev14 = np.zeros(6)
self.pos_veh_prev1 = np.array([-7, 0, -15])
self.pos_veh_prev2 = np.array([-7, 0, -15])
self.pos_veh_prev3 = np.array([-7, 0, -15])
self.pos_veh_prev4 = np.array([-7, 0, -15])
self.pos_veh_prev5 = np.array([-7, 0, -15])
self.pos_veh_prev6 = np.array([-7, 0, -15])
self.pos_veh_prev7 = np.array([-7, 0, -15])
self.vel_veh_prev1 = np.zeros(6)
self.vel_veh_prev2 = np.zeros(6)
self.vel_veh_prev3 = np.zeros(6)
self.vel_veh_prev4 = np.zeros(6)
self.vel_veh_prev5 = np.zeros(6)
self.vel_veh_prev6 = np.zeros(6)
self.vel_veh_prev7 = np.zeros(6)
self.quat_veh_prev1 = np.array([0, 0, 0, 0.994])
self.quat_veh_prev2 = np.array([0, 0, 0, 0.994])
self.quat_veh_prev3 = np.array([0, 0, 0, 0.994])
self.quat_veh_prev4 = np.array([0, 0, 0, 0.994])
self.quat_veh_prev5 = np.array([0, 0, 0, 0.994])
self.quat_veh_prev6 = np.array([0, 0, 0, 0.994])
self.quat_veh_prev7 = np.array([0, 0, 0, 0.994])
def __del__(self):
# Removing logging message handlers
while self._logger.handlers:
self._logger.handlers.pop()
@staticmethod
def get_controller(name, *args):
"""Create instance of a specific DP controller."""
for controller in DPControllerBase1.__subclasses__():
if name == controller.__name__:
self._logger.info('Creating controller={}'.format(name))
return controller(*args)
@staticmethod
def get_list_of_controllers():
"""Return list of DP controllers using this interface."""
return [
controller.__name__
for controller in DPControllerBase1.__subclasses__()
]
@property
def label(self):
"""`str`: Identifier name of the controller"""
return self._LABEL
@property
def odom_is_init(self):
"""`bool`: `True` if the first odometry message was received"""
return self._init_odom
@property
def error_pos_world(self):
"""`numpy.array`: Position error wrt world frame"""
return np.dot(self._vehicle_model.rotBtoI, self._errors['pos'])
@property
def error_orientation_quat(self):
"""`numpy.array`: Orientation error"""
return deepcopy(self._errors['rot'][0:3])
@property
def error_orientation_rpy(self):
"""`numpy.array`: Orientation error in Euler angles."""
e1 = self._errors['rot'][0]
e2 = self._errors['rot'][1]
e3 = self._errors['rot'][2]
eta = self._errors['rot'][3]
rot = np.array([[
1 - 2 * (e2**2 + e3**2), 2 * (e1 * e2 - e3 * eta),
2 * (e1 * e3 + e2 * eta)
],
[
2 * (e1 * e2 + e3 * eta), 1 - 2 * (e1**2 + e3**2),
2 * (e2 * e3 - e1 * eta)
],
[
2 * (e1 * e3 - e2 * eta), 2 * (e2 * e3 + e1 * eta),
1 - 2 * (e1**2 + e2**2)
]])
# Roll
roll = np.arctan2(rot[2, 1], rot[2, 2])
# Pitch, treating singularity cases
den = np.sqrt(1 - rot[2, 1]**2)
pitch = -np.arctan(rot[2, 1] / max(0.001, den))
# Yaw
yaw = np.arctan2(rot[1, 0], rot[0, 0])
return np.array([roll, pitch, yaw])
@property
def error_pose_euler(self):
"""`numpy.array`: Pose error with orientation represented in Euler angles."""
return np.hstack((self._errors['pos'], self.error_orientation_rpy))
@property
def error_vel_world(self):
"""`numpy.array`: Linear velocity error"""
return np.dot(self._vehicle_model.rotBtoI, self._errors['vel'])
def __str__(self):
msg = 'Dynamic positioning controller\n'
msg += 'Controller= ' + self._LABEL + '\n'
msg += 'Is model based? ' + str(self._is_model_based) + '\n'
msg += 'Vehicle namespace= ' + self._namespace
return msg
def _create_vehicle_model(self):
"""Create a new instance of a vehicle model. If controller is not model
based, this model will have its parameters set to 0 and will be used
to receive and transform the odometry data.
"""
if self._vehicle_model is not None:
del self._vehicle_model
self._vehicle_model = Vehicle(
inertial_frame_id=self._local_planner.inertial_frame_id)
def objectCallback(self, data):
msg = Pose()
msg.position.x = self._vehicle_model._pose['pos'][0]
msg.position.y = self._vehicle_model._pose['pos'][1]
msg.position.z = self._vehicle_model._pose['pos'][2]
msg.orientation.x = self._vehicle_model._pose['rot'][0]
msg.orientation.y = self._vehicle_model._pose['rot'][1]
msg.orientation.z = self._vehicle_model._pose['rot'][2]
msg.orientation.w = self._vehicle_model._pose['rot'][3]
self.pose_vehicle = geo_maths.pose_to_xytheta(msg)
self.pose = geo_maths.pose_to_xytheta(data.pose.pose)
T_wb = geo_maths.xytheta_to_T(self.pose[0], self.pose[1], self.pose[2])
T_bg = geo_maths.xytheta_to_T(13, 0, 0)
T_wg = np.dot(T_wb, T_bg)
self.x_wg.data, self.y_wg.data, self.theta_wg.data = geo_maths.T_to_xytheta(
T_wg)
self.q = geo_maths.theta_to_quaternion(self.theta_wg.data)
self.ref_boxPosition[0] = self.x_wg.data
self.ref_boxPosition[1] = self.y_wg.data
#self.ref_boxOrientation[0]=q.x
#self.ref_boxOrientation[1]=q.y
#self.ref_boxOrientation[2]=q.z
#self.ref_boxOrientation[3]=q.w
self.object_position = data.pose.pose.position
self.object_orientation = data.pose.pose.orientation
self.object_velocityLinear = data.twist.twist.linear
self.object_velocityAngular = data.twist.twist.angular
#self.ref_boxPosition[0]=self.object_position.x
#self.ref_boxPosition[1]=self.object_position.y
self.ref_boxPosition[2] = self.object_position.z
self.ref_boxOrientation[0] = self.object_orientation.x
self.ref_boxOrientation[1] = self.object_orientation.y
self.ref_boxOrientation[2] = self.object_orientation.z
self.ref_boxOrientation[3] = self.object_orientation.w
self.ref_boxVelocityLinear[0] = self.object_velocityLinear.x
self.ref_boxVelocityLinear[1] = self.object_velocityLinear.y
self.ref_boxVelocityLinear[2] = self.object_velocityLinear.z
t = rospy.Time.now().to_sec()
dt = t - self._prev_t
if self._prev_t < 0.0:
dt = 0.05
ref_position = np.hstack(
(self.x_wg.data, self.y_wg.data, self.object_position.z))
self.ref_boxVelocityLinear1 = (
ref_position - self.ref_boxPosition_prev) / max(0.01, dt)
self.ref_boxPosition_prev = ref_position
self.ref_boxVelocityAngular[0] = self.object_velocityAngular.x
self.ref_boxVelocityAngular[1] = self.object_velocityAngular.y
self.ref_boxVelocityAngular[2] = self.object_velocityAngular.z
def _update_reference(self):
"""Call the local planner interpolator to retrieve a trajectory
point and publish the reference message as `uuv_control_msgs/TrajectoryPoint`.
"""
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(self._vehicle_model.pos,
self._vehicle_model.quat)
t = rospy.get_time()
reference = self._local_planner.interpolate(t)
if reference is not None:
# self._reference['pos'] = reference.p
# self._reference['rot'] = reference.q
# self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['pos'] = self.ref_boxPosition
self._reference['rot'] = [self.q.x, self.q.y, self.q.z, self.q.w]
self._reference['vel'] = np.hstack(
(self.ref_boxVelocityLinear1, self.ref_boxVelocityAngular))
pos_ref_delay1 = self.pos_ref_prev1
pos_ref_delay2 = self.pos_ref_prev2
pos_ref_delay3 = self.pos_ref_prev3
pos_ref_delay4 = self.pos_ref_prev4
pos_ref_delay5 = self.pos_ref_prev5
pos_ref_delay6 = self.pos_ref_prev6
pos_ref_delay7 = self.pos_ref_prev7
pos_ref_delay8 = self.pos_ref_prev8
pos_ref_delay9 = self.pos_ref_prev9
pos_ref_delay10 = self.pos_ref_prev10
pos_ref_delay11 = self.pos_ref_prev11
pos_ref_delay12 = self.pos_ref_prev12
pos_ref_delay13 = self.pos_ref_prev13
pos_ref_delay14 = self.pos_ref_prev14
self.pos_ref_prev1 = self._reference['pos']
self.pos_ref_prev2 = pos_ref_delay1
self.pos_ref_prev3 = pos_ref_delay2
self.pos_ref_prev4 = pos_ref_delay3
self.pos_ref_prev5 = pos_ref_delay4
self.pos_ref_prev6 = pos_ref_delay5
self.pos_ref_prev7 = pos_ref_delay6
self.pos_ref_prev8 = pos_ref_delay7
self.pos_ref_prev9 = pos_ref_delay8
self.pos_ref_prev10 = pos_ref_delay9
self.pos_ref_prev11 = pos_ref_delay10
self.pos_ref_prev12 = pos_ref_delay11
self.pos_ref_prev13 = pos_ref_delay12
self.pos_ref_prev14 = pos_ref_delay13
rot_ref_delay1 = self.rot_ref_prev1
rot_ref_delay2 = self.rot_ref_prev2
rot_ref_delay3 = self.rot_ref_prev3
rot_ref_delay4 = self.rot_ref_prev4
rot_ref_delay5 = self.rot_ref_prev5
rot_ref_delay6 = self.rot_ref_prev6
rot_ref_delay7 = self.rot_ref_prev7
rot_ref_delay8 = self.rot_ref_prev8
rot_ref_delay9 = self.rot_ref_prev9
rot_ref_delay10 = self.rot_ref_prev10
rot_ref_delay11 = self.rot_ref_prev11
rot_ref_delay12 = self.rot_ref_prev12
rot_ref_delay13 = self.rot_ref_prev13
self.rot_ref_prev1 = self._reference['rot']
self.rot_ref_prev2 = rot_ref_delay1
self.rot_ref_prev3 = rot_ref_delay2
self.rot_ref_prev4 = rot_ref_delay3
self.rot_ref_prev5 = rot_ref_delay4
self.rot_ref_prev6 = rot_ref_delay5
self.rot_ref_prev7 = rot_ref_delay6
self.rot_ref_prev8 = rot_ref_delay7
self.rot_ref_prev9 = rot_ref_delay8
self.rot_ref_prev10 = rot_ref_delay9
self.rot_ref_prev11 = rot_ref_delay10
self.rot_ref_prev12 = rot_ref_delay11
self.rot_ref_prev13 = rot_ref_delay12
self.rot_ref_prev14 = rot_ref_delay13
vel_ref_delay1 = self.vel_ref_prev1
vel_ref_delay2 = self.vel_ref_prev2
vel_ref_delay3 = self.vel_ref_prev3
vel_ref_delay4 = self.vel_ref_prev4
vel_ref_delay5 = self.vel_ref_prev5
vel_ref_delay6 = self.vel_ref_prev6
vel_ref_delay7 = self.vel_ref_prev7
vel_ref_delay8 = self.vel_ref_prev8
vel_ref_delay9 = self.vel_ref_prev9
vel_ref_delay10 = self.vel_ref_prev10
vel_ref_delay11 = self.vel_ref_prev11
vel_ref_delay12 = self.vel_ref_prev12
vel_ref_delay13 = self.vel_ref_prev13
vel_ref_delay14 = self.vel_ref_prev14
self.vel_ref_prev1 = self._reference['vel']
self.vel_ref_prev2 = vel_ref_delay1
self.vel_ref_prev3 = vel_ref_delay2
self.vel_ref_prev4 = vel_ref_delay3
self.vel_ref_prev5 = vel_ref_delay4
self.vel_ref_prev6 = vel_ref_delay5
self.vel_ref_prev7 = vel_ref_delay6
self.vel_ref_prev8 = vel_ref_delay7
self.vel_ref_prev9 = vel_ref_delay8
self.vel_ref_prev10 = vel_ref_delay9
self.vel_ref_prev11 = vel_ref_delay10
self.vel_ref_prev12 = vel_ref_delay11
self.vel_ref_prev13 = vel_ref_delay12
self.vel_ref_prev14 = vel_ref_delay13
#self._reference['acc'] = np.hstack((reference.a, reference.alpha))
if reference is not None and self._reference_pub.get_num_connections(
) > 0:
# Publish current reference
msg = TrajectoryPointObjectFollow()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self._local_planner.inertial_frame_id
msg.pose.position = Vector3(*self._reference['pos'])
msg.pose.orientation = Quaternion(*self._reference['rot'])
msg.velocity.linear = Vector3(*self._reference['vel'][0:3])
msg.velocity.angular = Vector3(*self._reference['vel'][3:6])
#msg.acceleration.linear = Vector3(*self._reference['acc'][0:3])
#msg.acceleration.angular = Vector3(*self._reference['acc'][3:6])
self._reference_pub.publish(msg)
return True
def imuCallback(self, data):
self._linear_acceleration = data.linear_acceleration
#self.imuAccLinear[0]=self._linear_acceleration.x
#self.imuAccLinear[1]=self._linear_acceleration.y
#self.imuAccLinear[2]=self._linear_acceleration.z
def _update_time_step(self):
"""Update time step."""
t = rospy.get_time()
self._dt = t - self._prev_time
self._prev_time = t
if self._prev_time < 0.0:
self._dt = 0.05
def _reset_controller(self):
"""Reset reference and and error vectors."""
self._init_reference = False
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6))
def reset_controller_callback(self, request):
"""Service handler function."""
self._reset_controller()
return ResetControllerResponse(True)
def update_controller(self):
"""This function must be implemented by derived classes
with the implementation of the control algorithm.
"""
# Does nothing, must be overloaded
raise NotImplementedError()
def update_errors(self):
"""Update error vectors."""
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
self.count()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
pos_veh_delay1 = self.pos_veh_prev1
pos_veh_delay2 = self.pos_veh_prev2
pos_veh_delay3 = self.pos_veh_prev3
pos_veh_delay4 = self.pos_veh_prev4
pos_veh_delay5 = self.pos_veh_prev5
pos_veh_delay6 = self.pos_veh_prev6
self.pos_veh_prev1 = self._vehicle_model.pos
self.pos_veh_prev2 = pos_veh_delay1
self.pos_veh_prev3 = pos_veh_delay2
self.pos_veh_prev4 = pos_veh_delay3
self.pos_veh_prev5 = pos_veh_delay4
self.pos_veh_prev6 = pos_veh_delay5
self.pos_veh_prev7 = pos_veh_delay6
vel_veh_delay1 = self.vel_veh_prev1
vel_veh_delay2 = self.vel_veh_prev2
vel_veh_delay3 = self.vel_veh_prev3
vel_veh_delay4 = self.vel_veh_prev4
vel_veh_delay5 = self.vel_veh_prev5
vel_veh_delay6 = self.vel_veh_prev6
self.vel_veh_prev1 = self._vehicle_model.vel
self.vel_veh_prev2 = vel_veh_delay1
self.vel_veh_prev3 = vel_veh_delay2
self.vel_veh_prev4 = vel_veh_delay3
self.vel_veh_prev5 = vel_veh_delay4
self.vel_veh_prev6 = vel_veh_delay5
self.vel_veh_prev7 = vel_veh_delay6
quat_veh_delay1 = self.quat_veh_prev1
quat_veh_delay2 = self.quat_veh_prev2
quat_veh_delay3 = self.quat_veh_prev3
quat_veh_delay4 = self.quat_veh_prev4
quat_veh_delay5 = self.quat_veh_prev5
quat_veh_delay6 = self.quat_veh_prev6
self.quat_veh_prev1 = self._vehicle_model.quat
self.quat_veh_prev2 = quat_veh_delay1
self.quat_veh_prev3 = quat_veh_delay2
self.quat_veh_prev4 = quat_veh_delay3
self.quat_veh_prev5 = quat_veh_delay4
self.quat_veh_prev6 = quat_veh_delay5
self.quat_veh_prev7 = quat_veh_delay6
# with delay
# t = rospy.get_time()
# if t>3 and t<=85:
# self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - self.pos_veh_prev2)
# self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat), self._reference['rot'])
# #self._errors['vel'] = np.hstack((np.dot(rotItoB, self._reference['vel'][0:3]) - self.vel_veh_prev2[0:3],np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# self._errors['vel'] = np.hstack((self._reference['vel'][0:3]-self.vel_veh_prev2[0:3], self._reference['vel'][3:6]-vel[3:6]))
# else:
# self._errors['pos'] = np.dot(
# rotItoB, self._reference['pos'] - pos)
# self._errors['rot'] = quaternion_multiply(
# quaternion_inverse(quat), self._reference['rot'])
# self._errors['vel'] = np.hstack((
# np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
# np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# no delay
self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - pos)
self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat),
self._reference['rot'])
self._errors['vel'] = np.hstack(
(np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# don't touch. with delay
# t = rospy.get_time()
# if t>8 and t<=85:
# self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - self.pos_veh_prev3)
# self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat), self._reference['rot'])
# #self._errors['vel'] = np.hstack((np.dot(rotItoB, self._reference['vel'][0:3]) - self.vel_veh_prev2[0:3],np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# self._errors['vel'] = np.hstack((self._reference['vel'][0:3]-self.vel_veh_prev3[0:3], self._reference['vel'][3:6]-vel[3:6]))
# else:
# self._errors['pos'] = np.dot(
# rotItoB, self._reference['pos'] - pos)
# self._errors['rot'] = quaternion_multiply(
# quaternion_inverse(quat), self._reference['rot'])
# self._errors['vel'] = np.hstack((
# np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
# np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# no delay
if self._error_pub.get_num_connections() > 0:
stamp = rospy.Time.now()
msg = TrajectoryPointObjectFollow()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(
*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(
*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def count(self):
self._count += 1
msg = Int32()
msg.data = self._count
self._count_pub.publish(msg)
def pub_dt(self, data):
msg = Float32()
msg.data = data
self._dt_pub.publish(msg)
def pub_dt1(self, data):
msg = Float32()
msg.data = data
self._dt_pub1.publish(msg)
def publish_MPara(self, matrixPara):
msg = Matrix6()
msg.x11 = matrixPara[0, 0]
msg.x12 = matrixPara[0, 1]
msg.x13 = matrixPara[0, 2]
msg.x14 = matrixPara[0, 3]
msg.x15 = matrixPara[0, 4]
msg.x16 = matrixPara[0, 5]
msg.x21 = matrixPara[1, 0]
msg.x22 = matrixPara[1, 1]
msg.x23 = matrixPara[1, 2]
msg.x24 = matrixPara[1, 3]
msg.x25 = matrixPara[1, 4]
msg.x26 = matrixPara[1, 5]
msg.x31 = matrixPara[2, 0]
msg.x32 = matrixPara[2, 1]
msg.x33 = matrixPara[2, 2]
msg.x34 = matrixPara[2, 3]
msg.x35 = matrixPara[2, 4]
msg.x36 = matrixPara[2, 5]
msg.x41 = matrixPara[3, 0]
msg.x42 = matrixPara[3, 1]
msg.x43 = matrixPara[3, 2]
msg.x44 = matrixPara[3, 3]
msg.x45 = matrixPara[3, 4]
msg.x46 = matrixPara[3, 5]
msg.x51 = matrixPara[4, 0]
msg.x52 = matrixPara[4, 1]
msg.x53 = matrixPara[4, 2]
msg.x54 = matrixPara[4, 3]
msg.x55 = matrixPara[4, 4]
msg.x56 = matrixPara[4, 5]
msg.x61 = matrixPara[5, 0]
msg.x62 = matrixPara[5, 1]
msg.x63 = matrixPara[5, 2]
msg.x64 = matrixPara[5, 3]
msg.x65 = matrixPara[5, 4]
msg.x66 = matrixPara[5, 5]
self._MParameter_pub.publish(msg)
def publish_CPara(self, matrixPara):
msg = Matrix6()
msg.x11 = matrixPara[0, 0]
msg.x12 = matrixPara[0, 1]
msg.x13 = matrixPara[0, 2]
msg.x14 = matrixPara[0, 3]
msg.x15 = matrixPara[0, 4]
msg.x16 = matrixPara[0, 5]
msg.x21 = matrixPara[1, 0]
msg.x22 = matrixPara[1, 1]
msg.x23 = matrixPara[1, 2]
msg.x24 = matrixPara[1, 3]
msg.x25 = matrixPara[1, 4]
msg.x26 = matrixPara[1, 5]
msg.x31 = matrixPara[2, 0]
msg.x32 = matrixPara[2, 1]
msg.x33 = matrixPara[2, 2]
msg.x34 = matrixPara[2, 3]
msg.x35 = matrixPara[2, 4]
msg.x36 = matrixPara[2, 5]
msg.x41 = matrixPara[3, 0]
msg.x42 = matrixPara[3, 1]
msg.x43 = matrixPara[3, 2]
msg.x44 = matrixPara[3, 3]
msg.x45 = matrixPara[3, 4]
msg.x46 = matrixPara[3, 5]
msg.x51 = matrixPara[4, 0]
msg.x52 = matrixPara[4, 1]
msg.x53 = matrixPara[4, 2]
msg.x54 = matrixPara[4, 3]
msg.x55 = matrixPara[4, 4]
msg.x56 = matrixPara[4, 5]
msg.x61 = matrixPara[5, 0]
msg.x62 = matrixPara[5, 1]
msg.x63 = matrixPara[5, 2]
msg.x64 = matrixPara[5, 3]
msg.x65 = matrixPara[5, 4]
msg.x66 = matrixPara[5, 5]
self._CParameter_pub.publish(msg)
def publish_DPara(self, matrixPara):
msg = Matrix6()
msg.x11 = matrixPara[0, 0]
msg.x12 = matrixPara[0, 1]
msg.x13 = matrixPara[0, 2]
msg.x14 = matrixPara[0, 3]
msg.x15 = matrixPara[0, 4]
msg.x16 = matrixPara[0, 5]
msg.x21 = matrixPara[1, 0]
msg.x22 = matrixPara[1, 1]
msg.x23 = matrixPara[1, 2]
msg.x24 = matrixPara[1, 3]
msg.x25 = matrixPara[1, 4]
msg.x26 = matrixPara[1, 5]
msg.x31 = matrixPara[2, 0]
msg.x32 = matrixPara[2, 1]
msg.x33 = matrixPara[2, 2]
msg.x34 = matrixPara[2, 3]
msg.x35 = matrixPara[2, 4]
msg.x36 = matrixPara[2, 5]
msg.x41 = matrixPara[3, 0]
msg.x42 = matrixPara[3, 1]
msg.x43 = matrixPara[3, 2]
msg.x44 = matrixPara[3, 3]
msg.x45 = matrixPara[3, 4]
msg.x46 = matrixPara[3, 5]
msg.x51 = matrixPara[4, 0]
msg.x52 = matrixPara[4, 1]
msg.x53 = matrixPara[4, 2]
msg.x54 = matrixPara[4, 3]
msg.x55 = matrixPara[4, 4]
msg.x56 = matrixPara[4, 5]
msg.x61 = matrixPara[5, 0]
msg.x62 = matrixPara[5, 1]
msg.x63 = matrixPara[5, 2]
msg.x64 = matrixPara[5, 3]
msg.x65 = matrixPara[5, 4]
msg.x66 = matrixPara[5, 5]
self._DParameter_pub.publish(msg)
def publish_vel(self, vel):
msg = Vector6()
msg.x1 = vel[0]
msg.x2 = vel[1]
msg.x3 = vel[2]
msg.x4 = vel[3]
msg.x5 = vel[4]
msg.x6 = vel[5]
self._vel_pub.publish(msg)
def publish_restoring(self, sur):
msg = Vector6()
msg.x1 = sur[0]
msg.x2 = sur[1]
msg.x3 = sur[2]
msg.x4 = sur[3]
msg.x5 = sur[4]
msg.x6 = sur[5]
self._restoring_pub.publish(msg)
def publish_generalForce(self, sur):
msg = Vector6()
msg.x1 = sur[0]
msg.x2 = sur[1]
msg.x3 = sur[2]
msg.x4 = sur[3]
msg.x5 = sur[4]
msg.x6 = sur[5]
self._generalForce_pub.publish(msg)
def publish_equivalentControl(self, sur):
msg = Vector6()
msg.x1 = sur[0]
msg.x2 = sur[1]
msg.x3 = sur[2]
msg.x4 = sur[3]
msg.x5 = sur[4]
msg.x6 = sur[5]
self._equivalentControl_pub.publish(msg)
def publish_slidingSurface(self, slidingSurface):
force_msg = WrenchStamped()
force_msg.header.stamp = rospy.Time.now()
force_msg.header.frame_id = '%s/%s' % (
self._namespace, self._vehicle_model.body_frame_id)
force_msg.wrench.force.x = slidingSurface[0]
force_msg.wrench.force.y = slidingSurface[1]
force_msg.wrench.force.z = slidingSurface[2]
force_msg.wrench.torque.x = slidingSurface[3]
force_msg.wrench.torque.y = slidingSurface[4]
force_msg.wrench.torque.z = slidingSurface[5]
self._sliding_pub.publish(force_msg)
def publish_control_wrench(self, force):
"""Publish the thruster manager control set-point.
> *Input arguments*
* `force` (*type:* `numpy.array`): 6 DoF control
set-point wrench vector
"""
if not self.odom_is_init:
return
# Apply saturation
for i in range(6):
if force[i] < -self._control_saturation:
force[i] = -self._control_saturation
elif force[i] > self._control_saturation:
force[i] = self._control_saturation
if not self.thrusters_only:
surge_speed = self._vehicle_model.vel[0]
self.publish_auv_command(surge_speed, force)
return
force_msg = WrenchStamped()
force_msg.header.stamp = rospy.Time.now()
force_msg.header.frame_id = '%s/%s' % (
self._namespace, self._vehicle_model.body_frame_id)
force_msg.wrench.force.x = force[0]
force_msg.wrench.force.y = force[1]
force_msg.wrench.force.z = force[2]
force_msg.wrench.torque.x = force[3]
force_msg.wrench.torque.y = force[4]
force_msg.wrench.torque.z = force[5]
self._thrust_pub.publish(force_msg)
def publish_auv_command(self, surge_speed, wrench):
"""Publish the AUV control command message
> *Input arguments*
* `surge_speed` (*type:* `float`): Reference surge speed
* `wrench` (*type:* `numpy.array`): 6 DoF wrench vector
"""
if not self.odom_is_init:
return
surge_speed = max(0, surge_speed)
msg = AUVCommand()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/%s' % (self._namespace,
self._vehicle_model.body_frame_id)
msg.surge_speed = surge_speed
msg.command.force.x = max(self._min_thrust, wrench[0])
msg.command.force.y = wrench[1]
msg.command.force.z = wrench[2]
msg.command.torque.x = wrench[3]
msg.command.torque.y = wrench[4]
msg.command.torque.z = wrench[5]
self._auv_command_pub.publish(msg)
def _odometry_callback(self, msg):
"""Odometry topic subscriber callback function.
> *Input arguments*
* `msg` (*type:* `nav_msgs/Odometry`): Input odometry
message
"""
self._vehicle_model.update_odometry(msg)
if not self._init_odom:
self._init_odom = True
if len(self._odometry_callbacks):
for func in self._odometry_callbacks:
func()
class DPControllerBase(object):
"""General abstract class for DP controllers for underwater vehicles.
This is an abstract class, must be inherited by a controller module that
overrides the update_controller method. If the controller is set to be
model based (is_model_based=True), than the vehicle parameters are going
to be read from the ROS parameter server.
"""
_LABEL = ''
def __init__(self, is_model_based=False, list_odometry_callbacks=[],
planner_full_dof=False):
# Flag will be set to true when all parameters are initialized correctly
self._is_init = False
self._logger = logging.getLogger('dp_controller')
out_hdlr = logging.StreamHandler(sys.stdout)
out_hdlr.setFormatter(logging.Formatter(
rospy.get_namespace().replace('/', '').upper() + ' -- %(asctime)s | %(levelname)s | %(module)s | %(message)s'))
out_hdlr.setLevel(logging.INFO)
self._logger.addHandler(out_hdlr)
self._logger.setLevel(logging.INFO)
# Reading current namespace
self._namespace = rospy.get_namespace()
# Configuration for the vehicle dynamic model
self._is_model_based = is_model_based
if self._is_model_based:
self._logger.info('Setting controller as model-based')
else:
self._logger.info('Setting controller as non-model-based')
self._use_stamped_poses_only = False
if rospy.has_param('~use_stamped_poses_only'):
self._use_stamped_poses_only = rospy.get_param('~use_stamped_poses_only')
# Flag indicating if the vehicle has only thrusters, otherwise
# the AUV allocation node will be used
self.thrusters_only = rospy.get_param('~thrusters_only', True)
# Instance of the local planner for local trajectory generation
self._local_planner = LocalPlanner(
full_dof=planner_full_dof,
stamped_pose_only=self._use_stamped_poses_only,
thrusters_only=self.thrusters_only)
self._control_saturation = 5000
# TODO: Fix the saturation term and how it is applied
if rospy.has_param('~saturation'):
self._thrust_saturation = rospy.get_param('~saturation')
if self._control_saturation <= 0:
raise rospy.ROSException('Invalid control saturation forces')
# Flag indicating either use of the AUV control allocator or
# direct command of fins and thruster
self.use_auv_control_allocator = False
if not self.thrusters_only:
self.use_auv_control_allocator = rospy.get_param('~use_auv_control_allocator', False)
# Remap the following topics, if needed
# Publisher for thruster allocator
if self.thrusters_only:
self._thrust_pub = rospy.Publisher(
'thruster_output', WrenchStamped, queue_size=1)
else:
self._thrust_pub = None
if not self.thrusters_only:
self._auv_command_pub = rospy.Publisher(
'auv_command_output', AUVCommand, queue_size=1)
else:
self._auv_command_pub = None
self._min_thrust = rospy.get_param('~min_thrust', 40.0)
self._reference_pub = rospy.Publisher('reference',
TrajectoryPoint,
queue_size=1)
# Publish error (for debugging)
self._error_pub = rospy.Publisher('error',
TrajectoryPoint, queue_size=1)
self._init_reference = False
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6),
acc=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6))
# Time step
self._dt = 0
self._prev_time = rospy.get_time()
self._services = dict()
self._services['reset'] = rospy.Service('reset_controller',
ResetController,
self.reset_controller_callback)
# Time stamp for the received trajectory
self._stamp_trajectory_received = rospy.get_time()
# Instance of the vehicle model
self._vehicle_model = None
# If list of callbacks is empty, set the default
if len(list_odometry_callbacks):
self._odometry_callbacks = list_odometry_callbacks
else:
self._odometry_callbacks = [self.update_errors,
self.update_controller]
# Initialize vehicle, if model based
self._create_vehicle_model()
# Flag to indicate that odometry topic is receiving data
self._init_odom = False
# Subscribe to odometry topic
self._odom_topic_sub = rospy.Subscriber(
'odom', numpy_msg(Odometry), self._odometry_callback)
# Stores last simulation time
self._prev_t = -1.0
self._logger.info('DP controller successfully initialized')
def __del__(self):
# Removing logging message handlers
while self._logger.handlers:
self._logger.handlers.pop()
@staticmethod
def get_controller(name, *args):
"""Create instance of a specific DP controller."""
for controller in DPControllerBase.__subclasses__():
if name == controller.__name__:
print 'Creating controller=', name
return controller(*args)
@staticmethod
def get_list_of_controllers():
"""Return list of DP controllers using this interface."""
return [controller.__name__ for controller in
DPControllerBase.__subclasses__()]
@property
def label(self):
return self._LABEL
@property
def odom_is_init(self):
return self._init_odom
@property
def error_pos_world(self):
return np.dot(self._vehicle_model.rotBtoI, self._errors['pos'])
@property
def error_orientation_quat(self):
return deepcopy(self._errors['rot'][0:3])
@property
def error_orientation_rpy(self):
"""Return orientation error in Euler angles."""
e1 = self._errors['rot'][0]
e2 = self._errors['rot'][1]
e3 = self._errors['rot'][2]
eta = self._errors['rot'][3]
rot = np.array([[1 - 2 * (e2**2 + e3**2),
2 * (e1 * e2 - e3 * eta),
2 * (e1 * e3 + e2 * eta)],
[2 * (e1 * e2 + e3 * eta),
1 - 2 * (e1**2 + e3**2),
2 * (e2 * e3 - e1 * eta)],
[2 * (e1 * e3 - e2 * eta),
2 * (e2 * e3 + e1 * eta),
1 - 2 * (e1**2 + e2**2)]])
# Roll
roll = np.arctan2(rot[2, 1], rot[2, 2])
# Pitch, treating singularity cases
den = np.sqrt(1 - rot[2, 1]**2)
pitch = - np.arctan(rot[2, 1] / max(0.001, den))
# Yaw
yaw = np.arctan2(rot[1, 0], rot[0, 0])
return np.array([roll, pitch, yaw])
@property
def error_pose_euler(self):
"""Pose error with orientation represented in Euler angles."""
return np.hstack((self._errors['pos'], self.error_orientation_rpy))
@property
def error_vel_world(self):
return np.dot(self._vehicle_model.rotBtoI, self._errors['vel'])
def __str__(self):
msg = 'Dynamic positioning controller\n'
msg += 'Controller= ' + self._LABEL + '\n'
msg += 'Is model based? ' + str(self._is_model_based) + '\n'
msg += 'Vehicle namespace= ' + self._namespace
return msg
def _create_vehicle_model(self):
"""
Create a new instance of a vehicle model. If controller is not model
based, this model will have its parameters set to 0 and will be used
to receive and transform the odometry data.
"""
if self._vehicle_model is not None:
del self._vehicle_model
self._vehicle_model = Vehicle(
inertial_frame_id=self._local_planner.inertial_frame_id)
def _update_reference(self):
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(
self._vehicle_model.pos, self._vehicle_model.quat)
t = rospy.get_time()
reference = self._local_planner.interpolate(t)
if reference is not None:
self._reference['pos'] = reference.p
self._reference['rot'] = reference.q
self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['acc'] = np.hstack((reference.a, reference.alpha))
# print '------------------ REFERENCE\n'
# print reference
if reference is not None and self._reference_pub.get_num_connections() > 0:
# Publish current reference
msg = TrajectoryPoint()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self._local_planner.inertial_frame_id
msg.pose.position = Vector3(*self._reference['pos'])
msg.pose.orientation = Quaternion(*self._reference['rot'])
msg.velocity.linear = Vector3(*self._reference['vel'][0:3])
msg.velocity.angular = Vector3(*self._reference['vel'][3:6])
msg.acceleration.linear = Vector3(*self._reference['acc'][0:3])
msg.acceleration.angular = Vector3(*self._reference['acc'][3:6])
self._reference_pub.publish(msg)
return True
def _update_time_step(self):
t = rospy.get_time()
self._dt = t - self._prev_time
self._prev_time = t
def _reset_controller(self):
self._init_reference = False
# Reference with relation to the INERTIAL frame
self._reference = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6),
acc=np.zeros(6))
# Errors wih relation to the BODY frame
self._errors = dict(pos=np.zeros(3),
rot=np.zeros(4),
vel=np.zeros(6))
def reset_controller_callback(self, request):
self._reset_controller()
return ResetControllerResponse(True)
def update_controller(self):
# Does nothing, must be overloaded
raise NotImplementedError()
def update_errors(self):
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
self._errors['pos'] = np.dot(
rotItoB, self._reference['pos'] - pos)
# Update orientation error
self._errors['rot'] = quaternion_multiply(
quaternion_inverse(quat), self._reference['rot'])
# Velocity error with respect to the the BODY frame
self._errors['vel'] = np.hstack((
np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
if self._error_pub.get_num_connections() > 0:
stamp = rospy.Time.now()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def publish_control_wrench(self, force):
if not self.odom_is_init:
return
# Apply saturation
for i in range(6):
if force[i] < -self._control_saturation:
force[i] = -self._control_saturation
elif force[i] > self._control_saturation:
force[i] = self._control_saturation
if not self.thrusters_only:
surge_speed = self._vehicle_model.vel[0]
self.publish_auv_command(surge_speed, force)
return
force_msg = WrenchStamped()
force_msg.header.stamp = rospy.Time.now()
force_msg.header.frame_id = '%s/%s' % (self._namespace, self._vehicle_model.body_frame_id)
force_msg.wrench.force.x = force[0]
force_msg.wrench.force.y = force[1]
force_msg.wrench.force.z = force[2]
force_msg.wrench.torque.x = force[3]
force_msg.wrench.torque.y = force[4]
force_msg.wrench.torque.z = force[5]
self._thrust_pub.publish(force_msg)
def publish_auv_command(self, surge_speed, wrench):
if not self.odom_is_init:
return
surge_speed = max(0, surge_speed)
msg = AUVCommand()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/%s' % (self._namespace, self._vehicle_model.body_frame_id)
msg.surge_speed = surge_speed
msg.command.force.x = max(self._min_thrust, wrench[0])
msg.command.force.y = wrench[1]
msg.command.force.z = wrench[2]
msg.command.torque.x = wrench[3]
msg.command.torque.y = wrench[4]
msg.command.torque.z = wrench[5]
self._auv_command_pub.publish(msg)
def _odometry_callback(self, msg):
"""Odometry topic subscriber callback function."""
self._vehicle_model.update_odometry(msg)
if not self._init_odom:
self._init_odom = True
if len(self._odometry_callbacks):
for func in self._odometry_callbacks:
func()