def __init__(self, full_dof=False):
self._logger = get_logger()
# Waypoint set
self._waypoints = None
# True if the path is generated for all degrees of freedom, otherwise
# the path will be generated for (x, y, z, yaw) only
self._is_full_dof = full_dof
# The parametric variable to use as input for the interpolator
self._s = list()
self._segment_to_wp_map = list()
self._cur_s = 0
self._s_step = 0.0001
self._start_time = None
self._duration = None
self._termination_by_time = True
self._final_pos_tolerance = 0.1
self._init_rot = quaternion_about_axis(0.0, [0, 0, 1])
self._last_rot = quaternion_about_axis(0.0, [0, 0, 1])
self._markers_msg = MarkerArray()
self._marker_id = 0
def __init__(self,
full_dof=False,
use_finite_diff=True,
interpolation_method='cubic',
stamped_pose_only=False):
"""Class constructor."""
self._logger = logging.getLogger('wp_trajectory_generator')
out_hdlr = logging.StreamHandler(sys.stdout)
out_hdlr.setFormatter(
logging.Formatter(
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)
self._path_generators = dict()
self._logger.info('Waypoint interpolators available:')
for gen in PathGenerator.get_all_generators():
self._logger.info('\t - ' + gen.get_label())
self._path_generators[gen.get_label()] = gen
self._path_generators[gen.get_label()].set_full_dof(full_dof)
# Time step between interpolated samples
self._dt = None
# Last time stamp
self._last_t = None
# Last interpolated point
self._last_pnt = None
self._this_pnt = None
# Flag to generate only stamped pose, no velocity profiles
self._stamped_pose_only = stamped_pose_only
self._t_step = 0.001
# Interpolation method
self._interp_method = interpolation_method
# True if the path is generated for all degrees of freedom, otherwise
# the path will be generated for (x, y, z, yaw) only
self._is_full_dof = full_dof
# Use finite differentiation if true, otherwise use motion regression
# algorithm
self._use_finite_diff = use_finite_diff
# Time window used for the regression method
self._regression_window = 0.5
# If the regression method is used, adjust the time step
if not self._use_finite_diff:
self._t_step = self._regression_window / 30
# Flags to indicate that the interpolation process has started and
# ended
self._has_started = False
self._has_ended = False
# The parametric variable to use as input for the interpolator
self._cur_s = 0
self._init_rot = quaternion_about_axis(0.0, [0, 0, 1])
def _compute_rot_quat(self, dx, dy, dz):
if np.isclose(dx, 0) and np.isclose(dy, 0):
rotq = self._last_rot
else:
heading = np.arctan2(dy, dx)
rotq = quaternion_about_axis(heading, [0, 0, 1])
if self._is_full_dof:
rote = quaternion_about_axis(
-1 * np.arctan2(dz, np.sqrt(dx**2 + dy**2)), [0, 1, 0])
rotq = quaternion_multiply(rotq, rote)
# Certify that the next quaternion remains in the same half hemisphere
d_prod = np.dot(self._last_rot, rotq)
if d_prod < 0:
rotq *= -1
return rotq
def generate_quat(self, s):
"""Compute the quaternion of the path reference for a interpolated
point related to `s`, `s` being represented in the curve's parametric
space.
The quaternion is computed assuming the heading follows the direction
of the path towards the target. Roll and pitch can also be computed
in case the `full_dof` is set to `True`.
> *Input arguments*
* `s` (*type:* `float`): Curve's parametric input expressed in the
interval of [0, 1]
> *Returns*
Rotation quaternion as a `numpy.array` as `(x, y, z, w)`
"""
s = max(0, s)
s = min(s, 1)
last_s = s - self._s_step
if last_s == 0:
last_s = 0
if s == 0:
self._last_rot = deepcopy(self._init_rot)
return self._init_rot
this_pos = self.generate_pos(s)
last_pos = self.generate_pos(last_s)
dx = this_pos[0] - last_pos[0]
dy = this_pos[1] - last_pos[1]
dz = this_pos[2] - last_pos[2]
rotq = self._compute_rot_quat(dx, dy, dz)
self._last_rot = deepcopy(rotq)
# Calculating the step for the heading offset
q_step = quaternion_about_axis(
self._interp_fcns['heading'](s),
np.array([0, 0, 1]))
# Adding the heading offset to the rotation quaternion
rotq = quaternion_multiply(rotq, q_step)
return rotq