def __init__(self, name, **kwargs):
"""Class constructor."""
super().__init__(name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
# This flag will be set to true once the thruster allocation matrix is
# available
self._ready = False
# Acquiring the namespace of the vehicle
self.namespace = self.get_namespace()
if self.namespace != '':
if self.namespace[-1] != '/':
self.namespace += '/'
if self.namespace[0] != '/':
self.namespace = '/' + self.namespace
# Load all parameters
self.config = self.get_parameters_by_prefix('thruster_manager')
if len(self.config) == 0:
raise RuntimeError('Thruster manager parameters not '
'initialized for uuv_name=' + self.namespace)
self.config = parse_nested_params_to_dict(self.config, '.')
# Indicates whether we could get the robot description for the thrust axes
self.use_robot_descr = False
self.axes = {}
self.robot_description_subscription = None
if self.config['update_rate'].value < 0:
self.config['update_rate'].value = 50.0
self.base_link_ned_to_enu = None
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer, self)
# Initialize some variables
self.output_dir = None
self.n_thrusters = 0
# Thruster objects used to calculate the right angular velocity command
self.thrusters = list()
# Thrust forces vector
self.thrust = None
# Thruster allocation matrix: transform thruster inputs to force/torque
self.configuration_matrix = None
self.inverse_configuration_matrix = None
self.init_future = rclpy.Future()
self.init_thread = threading.Thread(target=self._init_async,
daemon=True)
self.init_thread.start()
def __init__(self, node_name, **kwargs):
super().__init__(node_name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
self.get_logger().info(node_name +
': start publishing vehicle footprints to RViz')
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# self.set_parameters([sim_time])
self._model_paths = dict()
try:
# Handle for retrieving model properties
service_name = '/gazebo/get_entity_state'
self.get_entity_state = self.create_client(GetEntityState,
service_name)
ready = self.get_entity_state.wait_for_service(timeout_sec=100)
if not ready:
raise RuntimeError('service is unavailable')
except RuntimeError:
self.get_logger().error('%s service is unavailable' % service_name)
sys.exit()
meshes = self.get_parameters_by_prefix('meshes')
if meshes != None:
#if self.has_parameter('meshes'):
#meshes = self.get_parameter('meshes').value
if type(meshes) != dict:
raise RuntimeError('A list of mesh filenames is required')
meshes = parse_nested_params_to_dict(meshes, '.')
self.add_meshes(meshes)
self._mesh_topic = self.create_publisher(MarkerArray, '/world_models',
1)
self.timer = self.create_timer(10, self.publish_meshes)
def __init__(self):
super().__init__('finned_uuv_teleop',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
#Default sim_time to True
sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
self.set_parameters([sim_time])
self.get_logger().info('FinnedUUVControllerNode: initializing node')
self._ready = False
# Test if any of the needed parameters are missing
param_labels = ['n_fins', 'gain_roll', 'gain_pitch', 'gain_yaw',
'thruster_model.max_thrust', 'thruster_model.name',
'fin_topic_prefix',
'fin_topic_suffix', 'thruster_topic',
'axis_thruster', 'axis_roll', 'axis_pitch', 'axis_yaw']
for label in param_labels:
if not self.has_parameter('%s' % label):
raise Exception('Parameter missing, label=%s' % label)
# Number of fins
self._n_fins = self.get_parameter('n_fins').get_parameter_value().integer_value
# Thruster joy axis gain
self._thruster_joy_gain = 1
if self.has_parameter('thruster_joy_gain'):
self._thruster_joy_gain = self.get_parameter('thruster_joy_gain').value
# Read the vector for contribution of each fin on the change on
# orientation
gain_roll = self.get_parameter('gain_roll').value
gain_pitch = self.get_parameter('gain_pitch').value
gain_yaw = self.get_parameter('gain_yaw').value
if len(gain_roll) != self._n_fins or len(gain_pitch) != self._n_fins \
or len(gain_yaw) != self._n_fins:
raise Exception('Input gain vectors must have length '
'equal to the number of fins')
# Create the command angle to fin angle mapping
self._rpy_to_fins = numpy.vstack((gain_roll, gain_pitch, gain_yaw)).T
# Read the joystick mapping
self._joy_axis = dict(axis_thruster=self.get_parameter('axis_thruster').get_parameter_value().integer_value,
axis_roll=self.get_parameter('axis_roll').get_parameter_value().integer_value,
axis_pitch=self.get_parameter('axis_pitch').get_parameter_value().integer_value,
axis_yaw=self.get_parameter('axis_yaw').get_parameter_value().integer_value)
# Subscribe to the fin angle topics
self._pub_cmd = list()
self._fin_topic_prefix = self.get_parameter('fin_topic_prefix').get_parameter_value().string_value
self._fin_topic_suffix = self.get_parameter('fin_topic_suffix').get_parameter_value().string_value
for i in range(self._n_fins):
topic = self.build_topic_name(self._fin_topic_prefix, i, self._fin_topic_suffix)
#topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
self._pub_cmd.append(self.create_publisher(FloatStamped, topic, 10))
# Create the thruster model object
try:
self._thruster_topic = self.get_parameter('thruster_topic').get_parameter_value().string_value
self._thruster_params = self.get_parameters_by_prefix('thruster_model')
self._thruster_params = parse_nested_params_to_dict(self._thruster_params, '.')
if 'params' not in self._thruster_params:
raise Exception('No thruster params given')
#Should not happen
if 'max_thrust' not in self._thruster_params:
raise Exception('No limit to thruster output was given')
self._thruster_model = Thruster.create_thruster(
self, self._thruster_params['name'].value, 0,
self._thruster_topic, None, None,
**{key: val.value for key, val in self._thruster_params['params'].items()})
except Exception as e:
raise RuntimeError('Thruster model could not be initialized: ' + str(e))
# Subscribe to the joystick topic
self.sub_joy = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
self._ready = True
def __init__(self, name, **kwargs):
super().__init__(name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
# 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.get_logger().info('Starting disturbance manager')
self.thread = None
# 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()
# 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 != {}:
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
cpt = 0
for key in self._disturbances.keys():
self.get_logger().info('\nDisturbance #%d: \n\t%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 link 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(
ApplyLinkWrench, '/gazebo/apply_link_wrench'))
self._service_cb['wrench'] = service_list[-1]
self._service_cb['thrusters'] = dict()
for item in self._disturbances.values():
thruster_id = -1
if 'thruster_id' in item: thruster_id = item['thruster_id']
if item['type'] == 'thruster_state':
if 'state' not in self._service_cb['thrusters']:
self._service_cb['thrusters']['state'] = dict()
service_list.append(self.create_client(
SetThrusterState,
self.build_service_name(vehicle_name, thruster_id, 'set_thruster_state')))
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,
self.build_service_name(vehicle_name, thruster_id, 'set_dynamic_state_efficiency')))
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,
self.build_service_name(vehicle_name, thruster_id, 'set_thrust_force_efficiency')))
self._service_cb['thrusters']['thrust_efficiency'][thruster_id] = service_list[-1]
except Exception as e:
self.get_logger().info('Service creation failed, error=%s' % str(e))
sys.exit(-1)
# Test if services are reachable
try:
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.get_logger().error('Some services are not available! message=' + str(e))
self.get_logger().error('Closing node...')
sys.exit(-1)
self._wrench_timer = self.create_timer(0.1, self._publish_wrench_disturbance)
def __init__(self, node_name, **kwargs):
super().__init__(node_name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# self.set_parameters([sim_time])
self.namespace = self.get_namespace().replace('/', '')
self.get_logger().info('Initialize control for vehicle <%s>' %
self.namespace)
self.local_planner = DPControllerLocalPlanner(full_dof=True,
thrusters_only=False,
stamped_pose_only=False)
self.base_link = self.get_parameter_or_helper(
'base_link', 'base_link').get_parameter_value().string_value
# Reading the minimum thrust generated
self.min_thrust = self.get_parameter_or_helper('min_thrust', 0.0).value
assert self.min_thrust >= 0
self.get_logger().info('Min. thrust [N]=%.2f' % self.min_thrust)
# Reading the maximum thrust generated
self.max_thrust = self.get_parameter_or_helper('max_thrust', 0.0).value
assert self.max_thrust > 0 and self.max_thrust > self.min_thrust
self.get_logger().info('Max. thrust [N]=%.2f' % self.max_thrust)
# Reading the thruster topic
self.thruster_topic = self.get_parameter_or_helper(
'thruster_topic',
'thrusters/id_0/input').get_parameter_value().string_value
assert len(self.thruster_topic) > 0
# Reading the thruster gain
self.p_gain_thrust = self.get_parameter_or_helper(
'thrust_p_gain', 0.0).value
assert self.p_gain_thrust > 0
self.d_gain_thrust = self.get_parameter_or_helper(
'thrust_d_gain', 0.0).value
assert self.d_gain_thrust >= 0
# Reading the roll gain
self.p_roll = self.get_parameter_or_helper('p_roll', 0.0).value
assert self.p_roll > 0
# Reading the pitch P gain
self.p_pitch = self.get_parameter_or_helper('p_pitch', 0.0).value
assert self.p_pitch > 0
# Reading the pitch D gain
self.d_pitch = self.get_parameter_or_helper('d_pitch', 0.0).value
assert self.d_pitch >= 0
# Reading the yaw P gain
self.p_yaw = self.get_parameter_or_helper('p_yaw', 0.0).value
assert self.p_yaw > 0
# Reading the yaw D gain
self.d_yaw = self.get_parameter_or_helper('d_yaw', 0.0).value
assert self.d_yaw >= 0
# Reading the saturation for the desired pitch
self.desired_pitch_limit = self.get_parameter_or_helper(
'desired_pitch_limit', 15 * np.pi / 180).value
assert self.desired_pitch_limit > 0
# Reading the saturation for yaw error
self.yaw_error_limit = self.get_parameter_or_helper(
'yaw_error_limit', 1.0).value
assert self.yaw_error_limit > 0
# Reading the number of fins
self.n_fins = self.get_parameter_or_helper(
'n_fins', 0).get_parameter_value().integer_value
assert self.n_fins > 0
# Reading the mapping for roll commands
self.map_roll = self.get_parameter_or_helper('map_roll',
[0, 0, 0, 0]).value
assert isinstance(self.map_roll, list)
assert len(self.map_roll) == self.n_fins
# Reading the mapping for the pitch commands
self.map_pitch = self.get_parameter_or_helper('map_pitch',
[0, 0, 0, 0]).value
assert isinstance(self.map_pitch, list)
assert len(self.map_pitch) == self.n_fins
# Reading the mapping for the yaw commands
self.map_yaw = self.get_parameter_or_helper('map_yaw',
[0, 0, 0, 0]).value
assert isinstance(self.map_yaw, list)
assert len(self.map_yaw) == self.n_fins
# Retrieve the thruster configuration parameters
self.thruster_config = self.get_parameter_by_prefix('thruster_config')
#Parse parameters to dictionary and unpack params to values
self.thruster_config = parse_nested_params_to_dict(
thruster_config, '.', True)
# Check if all necessary thruster model parameter are available
thruster_params = [
'conversion_fcn_params', 'conversion_fcn', 'topic_prefix',
'topic_suffix', 'frame_base', 'max_thrust'
]
for p in thruster_params:
if p not in self.thruster_config:
raise RuntimeError(
'Parameter <%s> for thruster conversion function is '
'missing' % p)
# Setting up the thruster topic name
self.thruster_topic = build_thruster_topic_name(
self.namespace, self.thruster_config['topic_prefix'], 0,
self.thruster_config['topic_suffix'])
# self.thruster_topic = '/%s/%s/id_%d/%s' % (self.namespace,
# self.thruster_config['topic_prefix'], 0,
# self.thruster_config['topic_suffix'])
base = '%s/%s' % (self.namespace, self.base_link)
frame = '%s/%s%d' % (self.namespace,
self.thruster_config['frame_base'], 0)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer, self)
self.get_logger().info('Lookup: Thruster transform found %s -> %s' %
(base, frame))
trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(),
rclpy.time.Duration(5))
pos = np.array([
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
])
quat = np.array([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])
self.get_logger().info('Thruster transform found %s -> %s' %
(base, frame))
self.get_logger().info('pos=' + str(pos))
self.get_logger().info('rot=' + str(quat))
# Read transformation from thruster
#params = {key: val.value for key, val in params.items()}
self.thruster = Thruster.create_thruster(
self.thruster_config['conversion_fcn'], 0, self.thruster_topic,
pos, quat, **self.thruster_config['conversion_fcn_params'])
self.get_logger().info('Thruster configuration=\n' +
str(self.thruster_config))
self.get_logger().info('Thruster input topic=' + self.thruster_topic)
self.max_fin_angle = self.get_parameter_or_helper(
'max_fin_angle', 0.0).value
assert self.max_fin_angle > 0
# Reading the fin input topic prefix
self.fin_topic_prefix = self.get_parameter_or_helper(
'fin_topic_prefix', 'fins').get_parameter_value().string_value
self.fin_topic_suffix = self.get_parameter_or_helper(
'fin_topic_suffix', 'input').get_parameter_value().string_value
self.rpy_to_fins = np.vstack(
(self.map_roll, self.map_pitch, self.map_yaw)).T
self.pub_cmd = list()
for i in range(self.n_fins):
topic = self.build_fin_topic_name(self.fin_topic_prefix, i,
self.fin_topic_suffix)
#topic = '%s/id_%d/%s' % (self.fin_topic_prefix, i, self.fin_topic_suffix)
self.pub_cmd.append(self.create_publisher(FloatStamped, topic, 10))
self.odometry_sub = self.create_subscription(Odometry, 'odom',
self.odometry_callback,
10)
self.reference_pub = self.create_publisher(TrajectoryPoint,
'reference', 1)
# Publish error (for debugging)
self.error_pub = self.create_publisher(TrajectoryPoint, 'error', 1)
def __init__(self, node_name):
super().__init__(node_name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
#Default use_sim_time to true
sim_time = rclpy.parameter.Parameter('use_sim_time',
rclpy.Parameter.Type.BOOL, True)
self.set_parameters([sim_time])
# Load the mapping for each input
self._axes = dict(x=4,
y=3,
z=1,
roll=2,
pitch=5,
yaw=0,
xfast=-1,
yfast=-1,
zfast=-1,
rollfast=-1,
pitchfast=-1,
yawfast=-1)
# Load the gain for each joystick axis input
# (default values for the XBox 360 controller)
self._axes_gain = dict(x=3,
y=3,
z=0.5,
roll=0.5,
pitch=0.5,
yaw=0.5,
xfast=6,
yfast=6,
zfast=1,
rollfast=2,
pitchfast=2,
yawfast=2)
mapping = self.get_parameters_by_prefix('mapping')
if len(mapping) != 0:
#if self.has_parameter('mapping'):
mapping = parse_nested_params_to_dict(mapping, '.')
for tag in self._axes:
if tag not in mapping:
self.get_logger().debug(
'Tag not found in axes mapping, tag=%s' % tag)
else:
if 'axis' in mapping[tag]:
self._axes[tag] = mapping[tag][
'axis'].get_parameter_value().integer_value
if 'gain' in mapping[tag]:
self._axes_gain[tag] = mapping[tag][
'gain'].get_parameter_value().double_value
# Dead zone: Force values close to 0 to 0
# (Recommended for imprecise controllers)
self._deadzone = 0.5
if self.has_parameter('deadzone'):
self._deadzone = self.get_parameter(
'deadzone').get_parameter_value().double_value
# Default for the RB button of the XBox 360 controller
self._deadman_button = -1
if self.has_parameter('deadman_button'):
self._deadman_button = self.get_parameter(
'deadman_button').get_parameter_value().integer_value
# If these buttons are pressed, the arm will not move
if self.has_parameter('exclusion_buttons'):
self._exclusion_buttons = self.get_parameter(
'exclusion_buttons').value
if type(self._exclusion_buttons) == str:
# Eloquent type inference problem...if a list is passed in the launch file from
# arg element to param with a value-sep attribute, it is not correctly parsed
# (you get a list with 1 str value). Let's provide a custom parser:
self._exclusion_buttons = str(
self._exclusion_buttons).split(',')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) is str:
try:
int(n)
except:
raise Exception(
'Exclusion buttons must be an integer index or an equivalent str value'
)
elif type(n) not in [float, int]:
raise Exception(
'Exclusion buttons must be an integer index to '
'the joystick button')
#Ensure we only get int value
self._exclusion_buttons = [
int(n) for n in self._exclusion_buttons
]
else:
self._exclusion_buttons = list()
# Default for the start button of the XBox 360 controller
self._home_button = 7
if self.has_parameter('home_button'):
self._home_button = self.get_parameter(
'home_button').get_parameter_value().integer_value
self._msg_type = 'twist'
if self.has_parameter('type'):
self._msg_type = self.get_parameter(
'type').get_parameter_value().string_value
if self._msg_type not in ['twist', 'accel']:
raise Exception('Teleoperation output must be either '
'twist or accel')
if self._msg_type == 'twist':
self._output_pub = self.create_publisher(Twist, 'output', 1)
else:
self._output_pub = self.create_publisher(Accel, 'output', 1)
self._home_pressed_pub = self.create_publisher(Bool, 'home_pressed', 1)
# Joystick topic subscriber
self._joy_sub = self.create_subscription(Joy, 'joy',
self._joy_callback, 10)
def __init__(self, node_name, **kwargs):
"""Class constructor."""
super().__init__(node_name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
#Default sim_time to True
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# self.set_parameters([sim_time])
# This flag will be set to true once the thruster allocation matrix is
# available
self._ready = False
# Acquiring the namespace of the vehicle
self.namespace = self.get_namespace()
if self.namespace != '':
if self.namespace[-1] != '/':
self.namespace += '/'
if self.namespace[0] != '/':
self.namespace = '/' + self.namespace
self.config = self.get_parameters_by_prefix('thruster_manager')
if len(self.config) == 0:
#if not self.has_parameter('thruster_manager'):
raise RuntimeError('Thruster manager parameters not '
'initialized for uuv_name=' + self.namespace)
self.config = parse_nested_params_to_dict(self.config, '.')
# Load all parameters
#self.config = self.get_parameter('thruster_manager').value
#TODO To change in foxy
#robot_description_param = self.namespace + 'robot_description'
robot_description_param = 'urdf_file'
self.use_robot_descr = False
self.axes = {}
if self.has_parameter(robot_description_param):
urdf_file = self.get_parameter(robot_description_param).value
if urdf_file != "":
self.use_robot_descr = True
self.parse_urdf(urdf_file)
if self.config['update_rate'].value < 0:
self.config['update_rate'].value = 50.0
self.base_link_ned_to_enu = None
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer, self)
# Initialize some variables
self.output_dir = None
self.n_thrusters = 0
# Thruster objects used to calculate the right angular velocity command
self.thrusters = list()
# Thrust forces vector
self.thrust = None
# Thruster allocation matrix: transform thruster inputs to force/torque
self.configuration_matrix = None
self.inverse_configuration_matrix = None
self.init_future = rclpy.Future()
self.init_thread = threading.Thread(target=self._init_async,
daemon=True)
self.init_thread.start()
def __init__(self, name, world_ned_to_enu=None, **kwargs):
super().__init__(name,
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
**kwargs)
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# self.set_parameters([sim_time])
self.namespace = self.get_namespace().replace('/', '')
self.get_logger().info('Initialize control for vehicle <%s>' %
self.namespace)
self.local_planner = DPControllerLocalPlanner(
self,
full_dof=True,
thrusters_only=False,
stamped_pose_only=False,
tf_trans_world_ned_to_enu=world_ned_to_enu)
self.base_link = self.get_parameter_or_helper(
'base_link', 'base_link').get_parameter_value().string_value
# Reading the minimum thrust generated
self.min_thrust = self.get_parameter_or_helper('min_thrust', 0.0).value
assert self.min_thrust >= 0
self.get_logger().info('Min. thrust [N]=%.2f' % self.min_thrust)
# Reading the maximum thrust generated
self.max_thrust = self.get_parameter_or_helper('max_thrust', 0.0).value
assert self.max_thrust > 0 and self.max_thrust > self.min_thrust
self.get_logger().info('Max. thrust [N]=%.2f' % self.max_thrust)
# Reading the thruster topic
self.thruster_topic = self.get_parameter_or_helper(
'thruster_topic',
'thrusters/id_0/input').get_parameter_value().string_value
assert len(self.thruster_topic) > 0
# Reading the thruster gain
self.p_gain_thrust = self.get_parameter_or_helper(
'thrust_p_gain', 0.0).value
assert self.p_gain_thrust > 0
self.d_gain_thrust = self.get_parameter_or_helper(
'thrust_d_gain', 0.0).value
assert self.d_gain_thrust >= 0
# Reading the roll gain
self.p_roll = self.get_parameter_or_helper('p_roll', 0.0).value
assert self.p_roll > 0
# Reading the pitch P gain
self.p_pitch = self.get_parameter_or_helper('p_pitch', 0.0).value
assert self.p_pitch > 0
# Reading the pitch D gain
self.d_pitch = self.get_parameter_or_helper('d_pitch', 0.0).value
assert self.d_pitch >= 0
# Reading the yaw P gain
self.p_yaw = self.get_parameter_or_helper('p_yaw', 0.0).value
assert self.p_yaw > 0
# Reading the yaw D gain
self.d_yaw = self.get_parameter_or_helper('d_yaw', 0.0).value
assert self.d_yaw >= 0
# Reading the saturation for the desired pitch
self.desired_pitch_limit = self.get_parameter_or_helper(
'desired_pitch_limit', 15 * np.pi / 180).value
assert self.desired_pitch_limit > 0
# Reading the saturation for yaw error
self.yaw_error_limit = self.get_parameter_or_helper(
'yaw_error_limit', 1.0).value
assert self.yaw_error_limit > 0
# Reading the number of fins
self.n_fins = self.get_parameter_or_helper(
'n_fins', 0).get_parameter_value().integer_value
assert self.n_fins > 0
# Reading the mapping for roll commands
self.map_roll = self.get_parameter_or_helper('map_roll',
[0, 0, 0, 0]).value
assert isinstance(self.map_roll, list)
assert len(self.map_roll) == self.n_fins
# Reading the mapping for the pitch commands
self.map_pitch = self.get_parameter_or_helper('map_pitch',
[0, 0, 0, 0]).value
assert isinstance(self.map_pitch, list)
assert len(self.map_pitch) == self.n_fins
# Reading the mapping for the yaw commands
self.map_yaw = self.get_parameter_or_helper('map_yaw',
[0, 0, 0, 0]).value
assert isinstance(self.map_yaw, list)
assert len(self.map_yaw) == self.n_fins
# Retrieve the thruster configuration parameters
self.thruster_config = self.get_parameters_by_prefix('thruster_config')
#Parse parameters to dictionary and unpack params to values
self.thruster_config = parse_nested_params_to_dict(
self.thruster_config, '.', True)
# Check if all necessary thruster model parameter are available
thruster_params = [
'conversion_fcn_params', 'conversion_fcn', 'topic_prefix',
'topic_suffix', 'frame_base', 'max_thrust'
]
for p in thruster_params:
if p not in self.thruster_config:
raise RuntimeError(
'Parameter <%s> for thruster conversion function is '
'missing' % p)
# Setting up the thruster topic name
self.thruster_topic = self.build_thruster_topic_name(
self.namespace, self.thruster_config['topic_prefix'], 0,
self.thruster_config['topic_suffix'])
# self.thruster_topic = '/%s/%s/id_%d/%s' % (self.namespace,
# self.thruster_config['topic_prefix'], 0,
# self.thruster_config['topic_suffix'])
self.max_fin_angle = self.get_parameter_or_helper(
'max_fin_angle', 0.0).value
assert self.max_fin_angle > 0
# Reading the fin input topic prefix
self.fin_topic_prefix = self.get_parameter_or_helper(
'fin_topic_prefix', 'fins').get_parameter_value().string_value
self.fin_topic_suffix = self.get_parameter_or_helper(
'fin_topic_suffix', 'input').get_parameter_value().string_value
self.rpy_to_fins = np.vstack(
(self.map_roll, self.map_pitch, self.map_yaw)).T
self.pub_cmd = list()
self.odometry_sub = None
self.reference_pub = None
self.error_pub = None
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer, self)
self._ready = False
self.init_future = rclpy.Future()
self.init_thread = threading.Thread(target=self._init_async,
daemon=True)
self.init_thread.start()
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()
