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 start_helix(self, request): if request.radius <= 0 or request.n_points <= 0 or \ request.n_turns <= 0: self._display_message('Invalid parameters to generate a helical trajectory') return InitHelicalTrajectoryResponse(False) t = rospy.Time(request.start_time.data.secs, request.start_time.data.nsecs) if t.to_sec() < rospy.get_time() and not request.start_now: self._display_message('The trajectory starts in the past, correct the starting time!') return InitHelicalTrajectoryResponse(False) else: self._display_message('Start helical trajectory now!') wp_set = uuv_trajectory_generator.WaypointSet() success = wp_set.generate_helix(radius=request.radius, center=request.center, num_points=request.n_points, max_forward_speed=request.max_forward_speed, delta_z=request.delta_z, num_turns=request.n_turns, theta_offset=request.angle_offset, heading_offset=request.heading_offset) if success: self.set_station_keeping(True) self._traj_interpolator.set_waypoints(wp_set) self._traj_interpolator.set_start_time((t.to_sec() if not request.start_now else rospy.get_time())) if request.duration > 0: if self._traj_interpolator.set_max_time(request.duration): self._display_message('Setting a maximum duration') else: self._display_message('Setting maximum duration failed') self._update_trajectory_info() self.set_station_keeping(False) self.set_automatic_mode(True) self.set_trajectory_running(True) self._smooth_approach_on = True print '===========================================================' print 'HELICAL TRAJECTORY GENERATED FROM WAYPOINT INTERPOLATION' print '===========================================================' print 'Radius [m] =', request.radius print 'Center [m] = (%.2f, %.2f, %.2f)' % (request.center.x, request.center.y, request.center.z) print '# of points =', request.n_points print 'Max. forward speed =', request.max_forward_speed print 'Delta Z =', request.delta_z print '# of turns =', request.n_turns print 'Helix angle offset =', request.angle_offset print 'Heading offset =', request.heading_offset print '# waypoints =', self._traj_interpolator.get_waypoints().num_waypoints print 'Starting from =', self._traj_interpolator.get_waypoints().get_waypoint(0).pos print 'Starting time [s] =', (t.to_sec() if not request.start_now else rospy.get_time()) print 'Estimated max. time [s] = ', self._traj_interpolator.get_max_time() print '===========================================================' return InitHelicalTrajectoryResponse(True) else: self._display_message('Error generating helical trajectory from waypoint set') return InitHelicalTrajectoryResponse(False)
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 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()
def start_circle(self, request): if request.max_forward_speed <= 0 or request.radius <= 0 or \ request.n_points <= 0: self._logger.error( 'Invalid parameters to generate a circular trajectory') return InitCircularTrajectoryResponse(False) t = rospy.Time(request.start_time.data.secs, request.start_time.data.nsecs) if t.to_sec() < rospy.get_time() and not request.start_now: self._logger.error( 'The trajectory starts in the past, correct the starting time!' ) return InitCircularTrajectoryResponse(False) wp_set = uuv_trajectory_generator.WaypointSet() success = wp_set.generate_circle( radius=request.radius, center=request.center, num_points=request.n_points, max_forward_speed=request.max_forward_speed, theta_offset=request.angle_offset, heading_offset=request.heading_offset) if success: # Activates station keeping self.set_station_keeping(True) self._traj_interpolator.set_interp_method('cubic_interpolator') self._traj_interpolator.set_waypoints(wp_set) self._traj_interpolator.set_start_time( (t.to_sec() if not request.start_now else rospy.get_time())) if request.duration > 0: if self._traj_interpolator.set_duration(request.duration): self._logger.info( 'Setting a maximum duration, duration=%.2f s' % request.duration) else: self._logger.error('Setting maximum duration failed') self._update_trajectory_info() # Disables station keeping to start trajectory self.set_station_keeping(False) self.set_automatic_mode(True) self.set_trajectory_running(True) self._smooth_approach_on = True print '===========================================================' print 'CIRCULAR TRAJECTORY GENERATED FROM WAYPOINT INTERPOLATION' print '===========================================================' print 'Radius [m] =', request.radius print 'Center [m] = (%.2f, %.2f, %.2f)' % ( request.center.x, request.center.y, request.center.z) print '# of points =', request.n_points print 'Max. forward speed =', request.max_forward_speed print 'Circle angle offset =', request.angle_offset print 'Heading offset =', request.heading_offset print '# waypoints =', self._traj_interpolator.get_waypoints( ).num_waypoints print 'Starting from =', self._traj_interpolator.get_waypoints( ).get_waypoint(0).pos print 'Starting time [s] =', (t.to_sec() if not request.start_now else rospy.get_time()) print 'Estimated max. time [s] = ', self._traj_interpolator.get_max_time( ) print '===========================================================' return InitCircularTrajectoryResponse(True) else: self._logger.error( 'Error generating circular trajectory from waypoint set') return InitCircularTrajectoryResponse(False)
ax.set_title('Linear accelerations - ' + interp_method) ax.set_xlim(pnts[0].t, pnts[-1].t) 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')
def _update_waypoints(self, msg): self._waypoints = uuv_trajectory_generator.WaypointSet() self._waypoints.from_message(msg)