def go_to(self, request): if self._vehicle_pose is None: self._display_message('Current pose has not been initialized yet') return GoToResponse(False) self.set_station_keeping(True) wp_set = uuv_trajectory_generator.WaypointSet() init_wp = uuv_trajectory_generator.Waypoint( x=self._vehicle_pose.pos[0], y=self._vehicle_pose.pos[1], z=self._vehicle_pose.pos[2], max_forward_speed=request.waypoint.max_forward_speed, heading_offset=request.waypoint.heading_offset, use_fixed_heading=request.waypoint.use_fixed_heading) wp_set.add_waypoint(init_wp) wp_set.add_waypoint_from_msg(request.waypoint) if not self._traj_interpolator.set_waypoints(wp_set): self._display_message('Error while setting waypoints') return GoToResponse(False) self._traj_interpolator.set_start_time(rospy.Time.now().to_sec()) self._update_trajectory_info() self.set_station_keeping(False) self.set_automatic_mode(True) self.set_trajectory_running(True) self._smooth_approach_on = False print '===========================================================' print 'GO TO' print '===========================================================' print wp_set print '# waypoints =', wp_set.num_waypoints print '===========================================================' return GoToResponse(True)
def go_to_incremental(self, request): """ Service callback to set the command to the vehicle to move to a relative position in the world. """ if self._vehicle_pose is None: self._logger.error('Current pose has not been initialized yet') return GoToIncrementalResponse(False) if request.max_forward_speed <= 0: self._logger.error('Max. forward speed must be positive') return GoToIncrementalResponse(False) self.set_station_keeping(True) wp_set = uuv_trajectory_generator.WaypointSet() init_wp = uuv_trajectory_generator.Waypoint( x=self._vehicle_pose.pos[0], y=self._vehicle_pose.pos[1], z=self._vehicle_pose.pos[2], max_forward_speed=request.max_forward_speed) wp_set.add_waypoint(init_wp) wp = uuv_trajectory_generator.Waypoint( x=self._vehicle_pose.pos[0] + request.step.x, y=self._vehicle_pose.pos[1] + request.step.y, z=self._vehicle_pose.pos[2] + request.step.z, max_forward_speed=request.max_forward_speed) wp_set.add_waypoint(wp) self._traj_interpolator.set_interp_method('lipb_interpolator') if not self._traj_interpolator.set_waypoints(wp_set): self._logger.error('Error while setting waypoints') return GoToIncrementalResponse(False) self._traj_interpolator.set_start_time(rospy.Time.now().to_sec()) self._update_trajectory_info() self.set_station_keeping(False) self.set_automatic_mode(True) self.set_trajectory_running(True) self._smooth_approach_on = False print '===========================================================' print 'GO TO INCREMENTAL' print '===========================================================' print wp_set print '# waypoints =', wp_set.num_waypoints print '===========================================================' return GoToIncrementalResponse(True)
def _calc_smooth_approach(self): """ Add the current vehicle position as waypoint to allow a smooth approach to the given trajectory. """ if self._vehicle_pose is None: self._logger.error( 'Simulation not properly initialized yet, ignoring approach...' ) return if not self._traj_interpolator.is_using_waypoints(): self._logger.error('Not using the waypoint interpolation method') return init_wp = uuv_trajectory_generator.Waypoint( x=self._vehicle_pose.pos[0], y=self._vehicle_pose.pos[1], z=self._vehicle_pose.pos[2], max_forward_speed=self._traj_interpolator.get_waypoints( ).get_waypoint(0).max_forward_speed) first_wp = self._traj_interpolator.get_waypoints().get_waypoint(0) dx = first_wp.x - init_wp.x dy = first_wp.y - init_wp.y dz = first_wp.z - init_wp.z # One new waypoint at each meter self._logger.info( 'Adding waypoints to approach the first position in the given waypoint set' ) steps = int(np.floor(first_wp.dist(init_wp.pos)) / 10) if steps > 0: for i in range(1, steps): wp = uuv_trajectory_generator.Waypoint( x=first_wp.x - i * dx / steps, y=first_wp.y - i * dy / steps, z=first_wp.z - i * dz / steps, max_forward_speed=self._traj_interpolator.get_waypoints( ).get_waypoint(0).max_forward_speed) self._traj_interpolator.add_waypoint(wp, add_to_beginning=True) self._traj_interpolator.add_waypoint(init_wp, add_to_beginning=True) self._update_trajectory_info()
def go_to(self, request): if self._vehicle_pose is None: self._logger.error('Current pose has not been initialized yet') return GoToResponse(False) if request.waypoint.max_forward_speed <= 0.0: self._logger.error('Max. forward speed must be greater than zero') return GoToResponse(False) self.set_station_keeping(True) wp_set = uuv_trajectory_generator.WaypointSet() init_wp = uuv_trajectory_generator.Waypoint( x=self._vehicle_pose.pos[0], y=self._vehicle_pose.pos[1], z=self._vehicle_pose.pos[2], max_forward_speed=request.waypoint.max_forward_speed, heading_offset=request.waypoint.heading_offset, use_fixed_heading=request.waypoint.use_fixed_heading) wp_set.add_waypoint(init_wp) wp_set.add_waypoint_from_msg(request.waypoint) self._traj_interpolator.set_interp_method('lipb_interpolator') if not self._traj_interpolator.set_waypoints(wp_set): self._logger.error('Error while setting waypoints') return GoToResponse(False) t = rospy.Time.now().to_sec() self._traj_interpolator.set_start_time(t) self._update_trajectory_info() self.set_station_keeping(False) self.set_automatic_mode(True) self.set_trajectory_running(True) self._smooth_approach_on = False print '===========================================================' print 'GO TO' print '===========================================================' print wp_set print 'Heading offset =', request.waypoint.heading_offset print '# waypoints =', self._traj_interpolator.get_waypoints( ).num_waypoints print 'Starting from =', self._traj_interpolator.get_waypoints( ).get_waypoint(0).pos print 'Start time [s]: ', t print 'Estimated max. time [s] = ', self._traj_interpolator.get_max_time( ) print '===========================================================' return GoToResponse(True)
def main(): # For a helical trajectory wp_set = uuv_trajectory_generator.WaypointSet() # Add some waypoints at the beginning wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-10, -12, -36, 0.5), add_to_beginning=True) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-13, -15, -44, 0.5), add_to_beginning=True) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-20, -24, -48, 0.5), add_to_beginning=True) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-10, 10, -5, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-20, 20, -5, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-30, 60, -50, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-40, 70, -55, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-40, 80, -30, 0.5)) run_generator(wp_set, 'cubic_interpolator') run_generator(wp_set, 'lipb_interpolator') plt.show()
ax = fig.add_subplot(212) for i in range(3): ax.plot([p.t for p in pnts], [p.acc[i + 3] for p in pnts], label='%d' % i) ax.legend() ax.grid(True) ax.set_title('Angular accelerations - ' + interp_method) ax.set_xlim(pnts[0].t, pnts[-1].t) if __name__ == '__main__': # For a helical trajectory wp_set = uuv_trajectory_generator.WaypointSet() # Add some waypoints at the beginning wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-10, -12, -36, 0.5), add_to_beginning=True) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-13, -15, -44, 0.5), add_to_beginning=True) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-20, -24, -48, 0.5), add_to_beginning=True) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-10, 10, -5, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-20, 20, -5, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-30, 60, -50, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-40, 70, -55, 0.5)) wp_set.add_waypoint(uuv_trajectory_generator.Waypoint(-40, 80, -30, 0.5)) run_generator(wp_set, 'cubic_interpolator') run_generator(wp_set, 'lipb_interpolator') plt.show()