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)