def test_add_repeated_waypoint(self): wp = Waypoint(x=1, y=2, z=3, max_forward_speed=1) wp_set = WaypointSet() self.assertTrue(wp_set.add_waypoint(wp), 'Error occured while adding waypoint to empty set') self.assertFalse(wp_set.add_waypoint(wp), 'Repeated waypoint wrongfully added')
class PathGenerator(object): """Base class to be inherited by custom path generator to generate paths from interpolated waypoints. > *Attributes* * `LABEL` (*type:* `str`): Name of the path generator > *Input arguments* * `full_dof` (*type:* `bool`, *default:* `False`): If `True`, generate 6 DoF paths, if `False`, roll and pitch are set to zero. """ LABEL = '' 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 # ========================================================================= @staticmethod def get_generator(name, *args, **kwargs): """Factory method for all derived path generators. > *Input arguments* * `name` (*type:* `str`): Name identifier of the path generator * `args` (*type:* `list`): List of arguments for the path generator constructor * `kwards` (*type:* `dict`): Keyword arguments for the path generator constructor > *Returns* An instance of the desired path generator. If the `name` input does not describe any of the derived path generator classes, an `ValueError` will be raised. """ for gen in PathGenerator.__subclasses__(): if name == gen.LABEL: return gen(*args, **kwargs) msg = 'Invalid path generator method' self._logger.error(msg) raise ValueError(msg) # ========================================================================= @staticmethod def get_all_generators(): """Get the name identifiers of all path generator classes. > *Returns* List of `str` """ generators = list() for gen in PathGenerator.__subclasses__(): generators.append(gen()) return generators # ========================================================================= @property def waypoints(self): """`uuv_waypoints.WaypointSet`: Set of waypoints""" return self._waypoints # ========================================================================= @property def max_time(self): """`float`: Absolute final timestamp assigned to the path in seconds""" return self._duration + self._start_time # ========================================================================= @property def duration(self): """`float`: Duration in seconds for the whole path""" return self._duration # ========================================================================= @duration.setter def duration(self, t): assert t > 0, 'Duration must be a positive value' self._duration = t # ========================================================================= @property def start_time(self): """`float`: Start timestamp assigned to the first waypoint""" return self._start_time # ========================================================================= @start_time.setter def start_time(self, time): assert time >= 0, 'Invalid negative time' self._start_time = time # ========================================================================= @property def closest_waypoint(self): """`uuv_waypoints.Waypoint`: Return the closest waypoint to the current position on the path. """ return self._waypoints.get_waypoint(self.closest_waypoint_idx) # ========================================================================= @property def closest_waypoint_idx(self): """Return the index of the closest waypoint to the current position on the path. """ if self._cur_s == 0: return 0 if self._cur_s == 1: return len(self._s) - 1 v = np.array(self._s - self._cur_s) idx = np.argmin(v) return idx # ========================================================================= @property def s_step(self): """`float`: Value of the step size for the path's parametric variable """ return self._s_step # ========================================================================= @s_step.setter def s_step(self, step): assert 0 < step < 1 self._s_step = step # ========================================================================= @property def termination_by_time(self): """`data_type`: Property description""" return self._termination_by_time # ========================================================================= def reset(self): 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 # ========================================================================= def get_segment_idx(self, s): if len(self._s) == 0: return 0 # Ensure the parameter s is 0 <= s <= 1 s = max(0, s) s = min(s, 1) if s == 1: idx = self._s.size - 1 else: idx = (self._s - s >= 0).nonzero()[0][0] return idx # ========================================================================= def get_remaining_waypoints_idx(self, s): idx = self.get_segment_idx(s) try: wps = self._segment_to_wp_map[idx::] return np.unique(wps) except: self._logger.error('Invalid segment index') return None # ========================================================================= def is_full_dof(self): return self._is_full_dof # ========================================================================= def set_full_dof(self, flag): self._is_full_dof = flag # ========================================================================= def get_label(self): return self.LABEL # ========================================================================= def init_interpolator(self): raise NotImplementedError() # ========================================================================= def get_samples(self, max_time, step=0.005): raise NotImplementedError() # ========================================================================= def get_visual_markers(self): return self._markers_msg # ========================================================================= def add_waypoint(self, waypoint, add_to_beginning=False): """Add waypoint to the existing waypoint set. If no waypoint set has been initialized, create new waypoint set structure and add the given waypoint.""" if self._waypoints is None: self._waypoints = WaypointSet() self._waypoints.add_waypoint(waypoint, add_to_beginning) return self.init_interpolator() # ========================================================================= def init_waypoints(self, waypoints=None, init_rot=np.array([0, 0, 0, 1])): if waypoints is not None: self._waypoints = deepcopy(waypoints) if self._waypoints is None: self._logger.error('Waypoint list has not been initialized') return False self._init_rot = init_rot self._logger.info('Setting initial rotation as={}'.format(init_rot)) return True # ========================================================================= def interpolate(self, tag, s): return self._interp_fcns[tag](s) # ========================================================================= def is_finished(self, t): if self._termination_by_time: return t > self.max_time else: return True # ========================================================================= def has_started(self, t): if self._termination_by_time: return t - self.start_time > 0 else: return True # ========================================================================= def generate_pnt(self, s): raise NotImplementedError() # ========================================================================= def generate_pos(self, s): raise NotImplementedError() # ========================================================================= def generate_quat(self, s): raise NotImplementedError() # ========================================================================= def set_parameters(self, params): raise NotImplementedError() # ========================================================================= 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
class PathGenerator(object): """ Abstract class to be inherited by custom path generator to interpolate waypoints """ LABEL = '' def __init__(self, full_dof=False): # 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 @staticmethod def get_generator(name, *args, **kwargs): for gen in PathGenerator.__subclasses__(): if name == gen.LABEL: return gen(*args, **kwargs) rospy.ROSException('Invalid path generator method') @staticmethod def get_all_generators(): generators = list() for gen in PathGenerator.__subclasses__(): generators.append(gen()) return generators @property def waypoints(self): return self._waypoints @property def max_time(self): return self._duration + self._start_time @property def duration(self): return self._duration @duration.setter def duration(self, t): assert t > 0, 'Duration must be a positive value' self._duration = t @property def start_time(self): return self._start_time @start_time.setter def start_time(self, time): assert time >= 0, 'Invalid negative time' self._start_time = time @property def closest_waypoint(self): """Return the closest waypoint to the current position on the path.""" return self._waypoints.get_waypoint(self.closest_waypoint_idx) @property def closest_waypoint_idx(self): """ Return the index of the closest waypoint to the current position on the path. """ if self._cur_s == 0: return 0 if self._cur_s == 1: return len(self._s) - 1 v = np.array(self._s - self._cur_s) idx = np.argmin(v) return idx @property def s_step(self): return self._s_step @s_step.setter def s_step(self, step): assert 0 < step < 1 self._s_step = step @property def termination_by_time(self): return self._termination_by_time def reset(self): 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 def get_segment_idx(self, s): if len(self._s) == 0: return 0 # Ensure the parameter s is 0 <= s <= 1 s = max(0, s) s = min(s, 1) if s == 1: idx = self._s.size - 1 else: idx = (self._s - s >= 0).nonzero()[0][0] return idx def get_remaining_waypoints_idx(self, s): idx = self.get_segment_idx(s) try: wps = self._segment_to_wp_map[idx::] return np.unique(wps) except: rospy.logerr('Invalid index') return None def is_full_dof(self): return self._is_full_dof def set_full_dof(self, flag): self._is_full_dof = flag def get_label(self): return self.LABEL def init_interpolator(self): raise NotImplementedError() def get_samples(self, max_time, step=0.005): raise NotImplementedError() def get_visual_markers(self): return self._markers_msg def add_waypoint(self, waypoint, add_to_beginning=False): """Add waypoint to the existing waypoint set. If no waypoint set has been initialized, create new waypoint set structure and add the given waypoint.""" if self._waypoints is None: self._waypoints = WaypointSet() self._waypoints.add_waypoint(waypoint, add_to_beginning) return self.init_interpolator() def init_waypoints(self, waypoints=None, init_rot=np.array([0, 0, 0, 1])): if waypoints is not None: self._waypoints = deepcopy(waypoints) if self._waypoints is None: rospy.logerr('Waypoint list has not been initialized') return False self._init_rot = init_rot rospy.loginfo('PathGenerator::Setting initial rotation as=%s', str(init_rot)) return True def interpolate(self, tag, s): return self._interp_fcns[tag](s) def is_finished(self, t): if self._termination_by_time: return t > self.max_time else: return True def has_started(self, t): if self._termination_by_time: return t - self.start_time > 0 else: return True def generate_pnt(self, s): raise NotImplementedError() def generate_pos(self, s): raise NotImplementedError() def generate_quat(self, s): raise NotImplementedError() def set_parameters(self, params): raise NotImplementedError() 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