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_waypoints.WaypointSet()
init_wp = uuv_waypoints.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_waypoints.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_waypoints.WaypointSet()
init_wp = uuv_waypoints.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 _update_waypoints(self, msg):
self._waypoints = uuv_waypoints.WaypointSet()
self._waypoints.from_message(msg)
def start_helix(self, request):
if request.radius <= 0 or request.n_points <= 0 or \
request.n_turns <= 0:
self._logger.error(
'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._logger.error(
'The trajectory starts in the past, correct the starting time!'
)
return InitHelicalTrajectoryResponse(False)
else:
self._logger.info('Start helical trajectory now!')
wp_set = uuv_waypoints.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_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()
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._logger.error(
'Error generating helical trajectory from waypoint set')
return InitHelicalTrajectoryResponse(False)