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()