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._logger = logging.getLogger('dp_local_planner')
out_hdlr = logging.StreamHandler(sys.stdout)
out_hdlr.setFormatter(logging.Formatter('%(asctime)s | %(levelname)s | %(module)s | %(message)s'))
out_hdlr.setLevel(logging.INFO)
self._logger.addHandler(out_hdlr)
self._logger.setLevel(logging.INFO)
self.thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
self.thread.start()
# Load disturbances and check for missing information
specs = dict(current=['starting_time', 'velocity', 'horizontal_angle',
'vertical_angle'],
wrench=['starting_time', 'duration', 'force', 'torque'],
thruster_state=['starting_time', 'thruster_id', 'is_on',
'duration'],
propeller_efficiency=['starting_time', 'thruster_id', 'duration',
'efficiency'],
thrust_efficiency=['starting_time', 'thruster_id', 'duration',
'efficiency'])
thruster_ids = list()
#if self.has_parameter('disturbances'):
#self._logger.info(str(self.get_parameters(['/'])))
self._disturbances = self.get_parameters_by_prefix('disturbances')
self._disturbances = parse_nested_params_to_dict(self._disturbances, unpack_value=True)
if self._disturbances != {}:
#if type(self._disturbances) != list:
# raise RuntimeError('Current specifications must be '
# 'given as a list of dict')
#for i in range(len(self._disturbances)):
for key in self._disturbances.keys():
item = self._disturbances[key]
if type(item) != dict:
raise RuntimeError('Disturbance description must be'
' given as a dict')
if 'type' not in item:
raise RuntimeError('Type of disturbance not '
'specified')
if item['type'] not in specs:
raise RuntimeError(
'Invalid type of disturbance, value=%s' % item['type'])
for spec in specs[item['type']]:
if spec not in item:
raise RuntimeError(
'Invalid current model specification, '
'missing tag=%s' % spec)
if item['type'] == 'thruster_state':
thruster_ids.append(item['thruster_id'])
# Create flag to indicate that perturbation has been applied
self._disturbances[key]['is_applied'] = False
self._disturbances[key]['ended'] = False
else:
raise RuntimeError('No disturbance specifications given')
# List all disturbances to be applied
#for i in range(len(self._disturbances)):
cpt = 0
for key in self._disturbances.keys():
self._logger.info('Disturbance #%d: %s' % (cpt, self._disturbances[key]))
cpt = cpt + 1
self._body_force = np.zeros(3)
self._body_torque = np.zeros(3)
self._body_wrench_msg = WrenchStamped()
# For body wrench disturbances, publish a topic
self._wrench_topic = self.create_publisher(
WrenchStamped, 'wrench_perturbation', 1)
vehicle_name = self.get_namespace().replace('/', '')
# Obtain service proxy
self._service_cb = dict()
service_list = list()
try:
service_list.append(self.create_client(
SetCurrentVelocity, '/hydrodynamics/set_current_velocity'))
self._service_cb['current_velocity'] = service_list[-1]
service_list.append(self.create_client(
ApplyBodyWrench, '/gazebo/apply_body_wrench'))
self._service_cb['wrench'] = service_list[-1]
self._service_cb['thrusters'] = dict()
for item in self._disturbances.values():
if item['type'] == 'thruster_state':
thruster_id = item['thruster_id']
if 'state' not in self._service_cb['thrusters']:
self._service_cb['thrusters']['state'] = dict()
service_list.append(self.create_client(
SetThrusterState,
create_service_name(vehicle_name, thruster_id, 'set_thruster_state')))
#'/%s/thrusters/%d/set_thruster_state' % (vehicle_name, item['thruster_id'])))
self._service_cb['thrusters']['state'][thruster_id] = service_list[-1]
elif item['type'] == 'propeller_efficiency':
if 'propeller_efficiency' not in self._service_cb['thrusters']:
self._service_cb['thrusters']['propeller_efficiency'] = dict()
service_list.append(self.create_client(
SetThrusterEfficiency,
create_service_name(vehicle_name, thruster_id, 'set_dynamic_state_efficiency')))
#'/%s/thrusters/%d/set_dynamic_state_efficiency' % (vehicle_name, item['thruster_id'])))
self._service_cb['thrusters']['propeller_efficiency'][thruster_id] = service_list[-1]
elif item['type'] == 'thrust_efficiency':
if 'thrust_efficiency' not in self._service_cb['thrusters']:
self._service_cb['thrusters']['thrust_efficiency'] = dict()
service_list.append(self.create_client(
SetThrusterEfficiency,
create_service_name(vehicle_name, thruster_id, 'set_thrust_force_efficiency')))
#'/%s/thrusters/%d/set_thrust_force_efficiency' % (vehicle_name, item['thruster_id'])))
self._service_cb['thrusters']['thrust_efficiency'][thruster_id] = service_list[-1]
except Exception as e:
self._logger.info('Service call failed, error=%s' % str(e))
sys.exit(-1)
# Test if services are reachable
try:
#self._service_cb.values()
# services = ['/hydrodynamics/set_current_velocity',
# '/gazebo/apply_body_wrench']
# for item in self._disturbances:
# if item['type'] == 'thruster_state':
# services.append('/%s/thrusters/%d/set_thruster_state' % (vehicle_name, item['thruster_id']))
# elif item['type'] == 'propeller_efficiency':
# services.append('/%s/thrusters/%d/set_dynamic_state_efficiency' % (vehicle_name, item['thruster_id']))
# elif item['type'] == 'thrust_efficiency':
# services.append('/%s/thrusters/%d/set_thrust_force_efficiency' % (vehicle_name, item['thruster_id']))
for s in service_list:
ready = s.wait_for_service(timeout_sec=10)
if not ready:
raise RuntimeError('Service %s not ready' % (s.srv_name))
except Exception as e:
self._logger.error('Some services are not available! message=' + str(e))
self._logger.error('Closing node...')
sys.exit(-1)
self._wrench_timer = self.create_timer(0.1, self._publish_wrench_disturbance)
#rate = self.create_rate(100)
#FREQ = 100
rate = node.create_rate(100)
while rclpy.ok():
t = time_in_float_seconds(self.get_clock().now())
#for i in range(len(self._disturbances)):
for d in self._disturbances.values():
#d = self._disturbances[i]
if t > d['starting_time'] and not d['is_applied']:
###########################################################
if d['type'] == 'current':
self.set_current(d['velocity'], d['horizontal_angle'],
d['vertical_angle'])
###########################################################
elif d['type'] == 'wrench':
self.set_link_wrench(d['force'],
d['torque'],
-1,
d['starting_time'])
###########################################################
elif d['type'] == 'thruster_state':
self.set_thruster_state(d['thruster_id'], bool(d['is_on']))
###########################################################
elif d['type'] == 'propeller_efficiency':
self.set_propeller_efficiency(d['thruster_id'], d['efficiency'])
###########################################################
elif d['type'] == 'thrust_efficiency':
self.set_thrust_efficiency(d['thruster_id'], d['efficiency'])
# Set applied flag to true
#self._disturbances[i]['is_applied'] = True
d['is_applied'] = True
if 'duration' in d:
if d['duration'] == -1:
#self._disturbances[i]['ended'] = True
d['ended'] = True
else:
#self._disturbances[i]['ended'] = True
d['ended'] = True
elif d['is_applied'] and 'duration' in d and not d['ended']:
if d['duration'] > 0:
if self.get_clock().now().nanoseconds > int((d['starting_time'] + d['duration']) * 1e9):
###########################################################
if d['type'] == 'current':
# Set current to zero
self.set_current(0, d['horizontal_angle'],
d['vertical_angle'])
###########################################################
elif d['type'] == 'wrench':
# Cancel out force and torque
self.set_link_wrench([-1 * d['force'][n] for n in range(3)],
[-1 * d['torque'][n] for n in range(3)],
-1,
time_in_float_sec(self.get_clock().now()))
###########################################################
elif d['type'] == 'thruster_state':
self.set_thruster_state(d['thruster_id'], not bool(d['is_on']))
###########################################################
elif d['type'] == 'propeller_efficiency':
self.set_propeller_efficiency(d['thruster_id'], 1.0)
###########################################################
elif d['type'] == 'thrust_efficiency':
self.set_thrust_efficiency(d['thruster_id'], 1.0)
#self._disturbances[i]['ended'] = True
d['ended'] = True
rate.sleep()
def _update_time_step(self):
"""Update time step."""
t = time_in_float_sec(self.get_clock().now())
self._dt = t - self._prev_time
self._prev_time = t
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'start_circular_trajectory',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Starting the circular trajectory creator')
# Important...ensure the clock has been updated when using sim time
while node.get_clock().now() == rclpy.time.Time(
clock_type=node.get_clock().clock_type):
rclpy.spin_once(node)
# If no start time is provided: start *now*.
start_time = time_in_float_sec(node.get_clock().now())
start_now = False
if node.has_parameter('start_time'):
start_time = node.get_parameter('start_time').value
if start_time < 0.0:
node.get_logger().warn('Negative start time, setting it to 0.0')
start_time = 0.0
start_now = True
else:
start_now = True
param_labels = [
'radius', 'center', 'n_points', 'heading_offset', 'duration',
'max_forward_speed'
]
params = dict()
for label in param_labels:
if not node.has_parameter(label):
node.get_logger().error(
'{} must be provided for the trajectory generation!'.format(
label))
sys.exit(-1)
params[label] = node.get_parameter(label).value
if len(params['center']) != 3:
node.get_logger().error(
'Center of circle must have 3 components (x, y, z): ' +
str(params['center']))
sys.exit(-1)
if params['n_points'] <= 2:
node.get_logger().error('Number of points must be at least 2: ' +
str(params['n_points']))
sys.exit(-1)
if params['max_forward_speed'] <= 0:
node.get_logger().error('Velocity limit must be positive: ' +
str(params['max_forward_speed']))
sys.exit(-1)
srv_name = 'start_circular_trajectory'
traj_gen = node.create_client(InitCircularTrajectory, srv_name)
if not traj_gen.wait_for_service(timeout_sec=20):
node.get_logger().error('Service %s not available! Closing node...' %
(traj_gen.srv_name))
sys.exit(-1)
node.get_logger().info(
'Generating trajectory that starts at t={} s'.format(start_time))
# Convert the time value
(sec, nsec) = float_sec_to_int_sec_nano(start_time)
req = InitCircularTrajectory.Request()
req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
req.start_now = start_now
req.radius = params['radius']
req.center = Point(params['center'][0], params['center'][1],
params['center'][2])
req.is_clockwise = False
req.angle_offset = 0.0
req.n_points = params['n_points']
req.heading_offset = params['heading_offset'] * pi / 180
req.max_forward_speed = params['max_forward_speed']
req.duration = params['duration']
future = traj_gen.call_async(req)
rclpy.spin_until_future_complete(node, future)
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + str(e))
else:
node.get_logger().info('Trajectory successfully generated!')
node.destroy_node()
def update_controller(self):
if not self._is_init:
return False
t = time_in_float_sec(self.get_clock().now())
dt = t - self._prev_t
if self._prev_t < 0.0:
dt = 0.0
# Get trajectory errors (reference - actual)
e_p_linear_b = self._errors['pos']
e_v_linear_b = self._errors['vel'][0:3]
e_p_angular_b = self.error_orientation_rpy # check this
e_v_angular_b = self._errors['vel'][3:6]
# Compute sliding surface s wrt body frame
s_linear_b = -e_v_linear_b - np.multiply(self._slope[0:3],
e_p_linear_b)
s_angular_b = -e_v_angular_b - np.multiply(self._slope[3:6],
e_p_angular_b)
# Compute exponential decay for transient improvement
if self._first_pass == True:
self._t_init, self._s_linear_b_init, self._s_angular_b_init = t, s_linear_b, s_angular_b
self._first_pass = False
sd_linear_b = np.multiply(self._s_linear_b_init,
np.exp(-self._K[0:3] * (t - self._t_init)))
sd_angular_b = np.multiply(self._s_angular_b_init,
np.exp(-self._K[3:6] * (t - self._t_init)))
# Compute sliding surface sn wrt body frame
sn_linear_b = s_linear_b - sd_linear_b
sn_angular_b = s_angular_b - sd_angular_b
# Compute summation sign(sn) wrt body frame
if self._prev_t > 0.0 and dt > 0.0:
self._summ_sign_sn_linear_b = self._summ_sign_sn_linear_b + 0.5 * (
self.sat(sn_linear_b, self._sat_epsilon) +
self._prev_sign_sn_linear_b) * dt
self._summ_sign_sn_angular_b = self._summ_sign_sn_angular_b + 0.5 * (
self.sat(sn_angular_b, self._sat_epsilon) +
self._prev_sign_sn_angular_b) * dt
# Compute extended error wrt body frame
sr_linear_b = sn_linear_b + np.multiply(self._Ki[0:3],
self._summ_sign_sn_linear_b)
sr_angular_b = sn_angular_b + np.multiply(self._Ki[3:6],
self._summ_sign_sn_angular_b)
# Compute required forces and torques wrt body frame
force_b = -np.multiply(self._Kd[0:3], sr_linear_b)
torque_b = -np.multiply(self._Kd[3:6], sr_angular_b)
self._tau = np.hstack((force_b, torque_b))
self.publish_control_wrench(self._tau)
self._prev_sign_sn_linear_b = self.sat(sn_linear_b, self._sat_epsilon)
self._prev_sign_sn_angular_b = self.sat(sn_angular_b,
self._sat_epsilon)
self._prev_t = t
def __init__(self, name, is_model_based=False, list_odometry_callbacks=None,
planner_full_dof=False, world_ned_to_enu=None, **kwargs):
super().__init__(name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
# 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 = self.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 self.has_parameter('use_stamped_poses_only'):
self._use_stamped_poses_only = self.get_parameter('use_stamped_poses_only').get_parameter_value().bool_value
# Flag indicating if the vehicle has only thrusters, otherwise
# the AUV allocation node will be used
self.thrusters_only = self.get_parameter_or_helper('thrusters_only', True).get_parameter_value().bool_value
self._callback_group = rclpy.callback_groups.ReentrantCallbackGroup()
# Instance of the local planner for local trajectory generation
self._local_planner = LocalPlanner(
self,
full_dof=planner_full_dof,
stamped_pose_only=self._use_stamped_poses_only,
thrusters_only=self.thrusters_only,
callback_group = self._callback_group,
tf_trans_world_ned_to_enu=world_ned_to_enu)
self._control_saturation = 5000
# TODO: Fix the saturation term and how it is applied
if self.has_parameter('saturation'):
self._thrust_saturation = self.get_parameter('saturation').value
if self._control_saturation <= 0:
raise RuntimeError('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 = self.get_parameter_or_helper(
'use_auv_control_allocator', False).get_parameter_value().bool_value
# Remap the following topics, if needed
# Publisher for thruster allocator
if self.thrusters_only:
self._thrust_pub = self.create_publisher(
WrenchStamped, 'thruster_output', 1)
else:
self._thrust_pub = None
if not self.thrusters_only:
self._auv_command_pub = self.create_publisher(
AUVCommand, 'auv_command_output', 1)
else:
self._auv_command_pub = None
self._min_thrust = self.get_parameter_or_helper('min_thrust', 40.0).get_parameter_value().double_value
self._reference_pub = self.create_publisher(TrajectoryPoint, 'reference', 1)
# Publish error (for debugging)
self._error_pub = self.create_publisher(TrajectoryPoint, 'error', 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 = time_in_float_sec(self.get_clock().now())
self._services = dict()
self._services['reset'] = self.create_service(ResetController,
'reset_controller',
self.reset_controller_callback,
callback_group=self._callback_group)
# Time stamp for the received trajectory
self._stamp_trajectory_received = time_in_float_sec(self.get_clock().now())
# 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(world_ned_to_enu)
# Flag to indicate that odometry topic is receiving data
self._init_odom = False
# Subscribe to odometry topic
self._odom_topic_sub = self.create_subscription(
Odometry, 'odom', self._odometry_callback, 10,
callback_group=self._callback_group)
# Stores last simulation time
self._prev_t = -1.0
self._logger.info('DP controller successfully initialized')
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_thrusters_output_efficiency',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Set the thruster output efficiency for vehicle, namespace=' + self.get_namespace())
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = float(node.get_parameter('starting_time').value)
node.get_logger().info('Starting time= {} s'.format(starting_time))
duration = 0.0
if node.has_parameter('duration'):
duration = float(node.get_parameter('duration').value)
if duration <= 0.0:
raise RuntimeError('Duration not set, leaving node...')
node.get_logger().info('Duration [s]=', ('Inf.' if duration < 0 else str(duration)))
if node.has_parameter('efficiency'):
efficiency = float(node.get_parameter('efficiency').value)
if efficiency < 0 or efficiency > 1:
raise RuntimeError('Invalid thruster output efficiency, leaving node...')
else:
raise RuntimeError('Thruster output efficiency not set, leaving node...')
thruster_id = -1
if node.has_parameter('thruster_id'):
if node.get_parameter('thruster_id').type_ == rclpy.Parameter.Type.INTEGER:
thruster_id = node.get_parameter('thruster_id').get_parameter_value().integer_value
else:
raise RuntimeError('Thruster ID not given')
if thruster_id < 0:
raise RuntimeError('Invalid thruster ID')
node.get_logger().info('Setting thruster output efficiency #{} to {}'.format(thruster_id, 100 * efficiency))
vehicle_name = node.get_namespace().replace('/', '')
srv_name = build_service_name(vehicle_name, thruster_id, 'set_thrust_force_efficiency')
set_eff = node.create_client(SetThrusterEfficiency, srv_name)
if not set_eff.wait_for_service(timeout_sec=2):
raise RuntimeError('Service %s not available! Closing node...' %(srv_name))
FREQ = 100
rate = node.create_rate(FREQ)
thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < starting_time:
#Just a guard for really short timeouts
if 1.0 / FREQ < starting_time:
rate.sleep()
# rate = node.create_rate(100)
# while time_in_float_sec(node.get_clock().now()) < starting_time:
# rate.sleep()
req = SetThrusterEfficiency.Request()
req.efficiency = efficiency
#success = set_eff.call(efficiency)
future = set_eff.call_async(efficiency)
#NB : spining is done from another thread
while not future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)):
else:
node.get_logger().info('Time={} s'.format(rtime_in_float_sec(node.get_clock().now())))
node.get_logger().info('Current thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
if duration > 0:
while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
if 1.0 / FREQ < starting_time + duration:
rate.sleep()
# rate = node.create_rate.Rate(100)
# while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
# rate.sleep()
req.efficiency = 1.0
success = set_eff.call(efficiency)
while not future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)):
else:
node.get_logger().info('Time={} s'.format(time_in_float_sec(node.get_clock().now())))
node.get_logger().infoint(
'Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
# if success:
# print('Time={} s'.format(time_in_float_sec(node.get_clock().now())))
# print('Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
node.get_logger().info('Leaving node...')
node.destroy_node()
rclpy.shutdown()
thread.join()
def update_controller(self):
if not self._is_init:
return False
t = time_in_float_sec(self.get_clock().now())
dt = t - self._prev_t
if self._prev_t < 0.0:
dt = 0.0
# Update integrator
self._int += 0.5 * (self.error_pose_euler -
self._error_pose) * self._dt
# Store current pose error
self._error_pose = self.error_pose_euler
# Get trajectory errors (reference - actual)
e_p_linear_b = self._errors['pos']
e_v_linear_b = self._errors['vel'][0:3]
e_p_angular_b = self.error_orientation_rpy
e_v_angular_b = self._errors['vel'][3:6]
e_p_b = np.hstack((e_p_linear_b, e_p_angular_b))
e_v_b = np.hstack((e_v_linear_b, e_v_angular_b))
# Compute sliding surface s wrt body frame
self._s_b = -e_v_b - np.multiply(self._lambda, e_p_b) \
- self._sliding_int * np.multiply(np.square(self._lambda)/4, self._int)
# Acceleration estimate
self._rotBtoI_dot = np.dot(
cross_product_operator(self._vehicle_model._vel[3:6]),
self._vehicle_model.rotBtoI)
self._accel_linear_estimate_b = np.dot(
self._vehicle_model.rotItoB, (self._reference['acc'][0:3] - \
np.dot(self._rotBtoI_dot, self._vehicle_model._vel[0:3]))) + \
np.multiply(self._lambda[0:3], e_v_linear_b) + \
self._sliding_int * np.multiply(np.square(self._lambda[0:3]) / 4, e_p_linear_b)
self._accel_angular_estimate_b = np.dot(self._vehicle_model.rotItoB, (self._reference['acc'][3:6] -
np.dot(self._rotBtoI_dot, self._vehicle_model._vel[3:6]))) + \
np.multiply(self._lambda[3:6], e_v_angular_b) + \
self._sliding_int * np.multiply(np.square(self._lambda[3:6]) / 4,
e_p_angular_b)
self._accel_estimate_b = np.hstack(
(self._accel_linear_estimate_b, self._accel_angular_estimate_b))
# Equivalent control
acc = self._vehicle_model.to_SNAME(self._accel_estimate_b)
self._f_eq = self._vehicle_model.compute_force(acc, use_sname=False)
# Linear control
self._f_lin = -np.multiply(self._k, self._s_b)
# Uncertainties / disturbances upper boundaries for robust control
self._rho_total = self._adaptive_bounds * self._rho_adapt + self._constant_bound * self._rho_constant
# Adaptation law
self._rho_adapt = self._rho_adapt + \
(self._adapt_slope[0] * np.abs(self._s_b) +
(self._adapt_slope[1] * np.abs(self._s_b) * np.abs(e_p_b) * np.abs(e_p_b)) +
(self._adapt_slope[2] * np.abs(self._s_b) * np.abs(e_v_b) * np.abs(e_v_b)) +
self._drift_prevent * (self._rho_0 - self._rho_adapt)) * dt
# Robust control
self._f_robust = -np.multiply(
self._rho_total,
(2 / np.pi) * np.arctan(np.multiply(self._c, self._s_b)))
# Compute required forces and torques wrt body frame
self._tau = self._ctrl_eq * self._f_eq + self._ctrl_lin * self._f_lin + self._ctrl_robust * self._f_robust
self.publish_control_wrench(self._tau)
self._prev_t = t
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_timed_current_perturbation',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
node.get_logger().info('Starting current perturbation node')
node.get_logger().info('Programming the generation of a current perturbation')
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = node.get_parameter('starting_time').value
if starting_time < 0.0:
node.get_logger().warn('Negative starting time, setting it to 0.0')
starting_time = 0.0
node.get_logger().info('Starting time=', starting_time)
end_time = -1
if node.has_parameter('end_time'):
end_time = node.get_parameter('end_time').value
if end_time > 0 and end_time <= starting_time:
raise RuntimeError('End time is smaller than the starting time')
node.get_logger().info('End time=', (end_time if end_time > 0 else 'Inf.'))
vel = 0.0
if node.has_parameter('current_velocity'):
vel = node.get_parameter('current_velocity').value
node.get_logger().info('Current velocity [m/s]=', vel)
horz_angle = 0.0
if node.has_parameter('horizontal_angle'):
horz_angle = node.get_parameter('horizontal_angle').value
horz_angle *= pi / 180
node.get_logger().info('Current horizontal angle [deg]=', horz_angle * 180 / pi)
vert_angle = 0.0
if node.has_parameter('vertical_angle'):
vert_angle = node.get_parameter('vertical_angle').value
vert_angle *= pi / 180
node.get_logger().info('Current vertical angle [deg]=', horz_angle * 180 / pi)
#Create client service
srv_name = '/hydrodynamics/set_current_velocity'
set_current = node.create_client(SetCurrentVelocity, srv_name)
if not set_current.wait_for_service(timeout_sec=20)
node.get_logger().error('%s service not available! Closing node...' %(srv_name))
sys.exit(-1)
# Wait to set the current model
FREQ = 100
rate = node.create_rate(FREQ)
thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < starting_time:
if 1.0 / FREQ < starting_time:
rate.sleep()
# Wait to set the current model
# rate = node.create_rate(100)
# if rospy.get_time() < starting_time:
# while rospy.get_time() < starting_time:
# rate.sleep()
node.get_logger().info('Applying current model...')
req = SetCurrentVelocity.Request()
req.vel = vel
req.horz_angle = horz_angle
req.vert_angle = vert_angle
future = set_current.call_async(req)
#NB : spining is done from another thread
while not future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name + ' failed, error=' + str(e)):
else:
t = time_in_float_sec(node.get_clock().now().get_time())
node.get_logger().info('Current velocity changed successfully at %f s! vel= %f m/s' % (t, vel))
# if set_current(vel, horz_angle, vert_angle):
# print('Current velocity changed successfully at %f s! vel= %f m/s' % (rospy.get_time(), vel))
# else:
# print('Failed to change current velocity')
if end_time > 0:
while time_in_float_sec(node.get_clock().now()) < end_time:
if 1.0 / FREQ < end_time:
rate.sleep()
# while time_in_float_sec(node.get_clock().now()) < end_time:
# rate.sleep()
req.vel = 0
req.horz_angle = horz_angle
req.vert_angle = vert_angle
node.get_logger().info('TIMEOUT, setting current velocity to zero...')
future = set_current.call_async(req)
#NB : spining is done from another thread
while not future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name + ' failed, error=' + str(e)):
#set_current.call(req)
node.get_logger().info('Leaving node...')
node.destroy_node()
rclpy.shutdown()
thread.join()
#==============================================================================
if __name__ == '__main__':
try:
main()
except Exception as e:
print('Something went wrong: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'send_waypoint_file',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Send a waypoint file, namespace=%s' %
node.get_namespace())
if node.has_parameter('filename'):
filename = node.get_parameter(
'filename').get_parameter_value().string_value
else:
raise RuntimeError('No filename found')
# Important...ensure the clock has been updated when using sim time
while node.get_clock().now() == rclpy.time.Time(
clock_type=node.get_clock().clock_type):
rclpy.spin_once(node)
# If no start time is provided: start *now*.
start_time = time_in_float_sec(node.get_clock().now())
start_now = True
if node.has_parameter('start_time'):
start_time = node.get_parameter('start_time').value
if start_time < 0.0:
node.get_logger().warn('Negative start time, setting it to 0.0')
start_time = 0.0
start_now = True
else:
start_now = False
node.get_logger().info('Start time=%.2f s' % start_time)
interpolator = node.get_parameter_or(
'interpolator', 'lipb').get_parameter_value().string_value
srv_name = 'init_waypoints_from_file'
init_wp = node.create_client(InitWaypointsFromFile, srv_name)
ready = init_wp.wait_for_service(timeout_sec=30)
if not ready:
raise RuntimeError('Service not available! Closing node...')
(sec, nsec) = float_sec_to_int_sec_nano(start_time)
req = InitWaypointsFromFile.Request()
req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
req.start_now = start_now
req.filename = String(data=filename)
req.interpolator = String(data=interpolator)
future = init_wp.call_async(req)
rclpy.spin_until_future_complete(node, future)
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + str(e))
else:
node.get_logger().info(
'Waypoints file successfully received, '
'filename=%s', filename)
node.destroy_node()
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_link_wrench',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Apply programmed perturbation to vehicle ' +
node.get_namespace())
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = float(node.get_parameter('starting_time').value)
node.get_logger().info('Starting time in = {} s'.format(starting_time))
duration = 0.0
if node.has_parameter('duration'):
duration = float(node.get_parameter('duration').value)
# Compare to eps ?
if duration == 0.0:
node.get_logger().info('Duration not set, leaving node...')
sys.exit(-1)
node.get_logger().info('Duration [s]=' +
('Inf.' if duration < 0 else str(duration)))
force = [0.0, 0.0, 0.0]
if node.has_parameter('force'):
force = node.get_parameter('force').value
if len(force) != 3:
raise RuntimeError('Invalid force vector')
# Ensure type is float
force = [float(elem) for elem in force]
node.get_logger().info('Force [N]=' + str(force))
torque = [0.0, 0.0, 0.0]
if node.has_parameter('torque'):
torque = node.get_parameter('torque').value
if len(torque) != 3:
raise RuntimeError('Invalid torque vector')
# Ensure type is float
torque = [float(elem) for elem in torque]
node.get_logger().info('Torque [N]=' + str(torque))
gazebo_ns = 'gazebo'
if node.has_parameter('gazebo_ns'):
gazebo_ns = node.get_parameter('gazebo_ns').value
if not gazebo_ns.startswith('/'):
gazebo_ns = '/' + gazebo_ns
service_name = '{}/apply_link_wrench'.format(gazebo_ns)
try:
apply_wrench = node.create_client(ApplyLinkWrench, service_name)
except Exception as e:
node.get_logger().error('Creation of service ' + service_name +
' failed, error=' + str(e))
sys.exit(-1)
try:
ready = apply_wrench.wait_for_service(timeout_sec=10)
if not ready:
raise RuntimeError('')
except:
node.get_logger().error('Service ' + service_name +
' not available! Closing node...')
sys.exit(-1)
ns = node.get_namespace().replace('/', '')
body_name = '%s/base_link' % ns
FREQ = 100
rate = node.create_rate(FREQ)
thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < starting_time:
# Just a guard for really short timeouts
if 1.0 / FREQ < starting_time:
rate.sleep()
(d_secs, d_nsecs) = float_sec_to_int_sec_nano(duration)
apw = ApplyLinkWrench.Request()
apw.link_name = body_name
apw.reference_frame = 'world'
apw.reference_point = Point(x=0.0, y=0.0, z=0.0)
apw.wrench = Wrench()
apw.wrench.force = Vector3(x=force[0], y=force[1], z=force[2])
apw.wrench.torque = Vector3(x=torque[0], y=torque[1], z=torque[2])
apw.start_time = node.get_clock().now().to_msg()
apw.duration = rclpy.time.Duration(seconds=d_secs,
nanoseconds=d_nsecs).to_msg()
future = apply_wrench.call_async(apw)
# NB : spining is done from another thread
while rclpy.ok():
if future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().error(
'Could not apply link wrench, exception thrown: %r' %
(e, ))
else:
if response.success:
message = '\nLink wrench perturbation applied!'
message += '\n\tFrame: ' + body_name
message += '\n\tDuration [s]: ' + str(duration)
message += '\n\tForce [N]: ' + str(force)
message += '\n\tTorque [Nm]: ' + str(torque)
node.get_logger().info(message)
else:
message = 'Could not apply link wrench'
message += '\n\tError is: %s' % (response.status_message)
node.get_logger().error(message)
finally:
break
node.destroy_node()
