def iterate(self):
""" Main loop operations """
# Compute current
current = self.compute_currents()
# Runge-Kutta, 4th order
k1_pos, k1_vel = self.step(self.p, self.v, self.old_u, self.old_f,
current)
k2_pos, k2_vel = self.step(self.p + self.period * 0.5 * k1_pos,
self.v + self.period * 0.5 * k1_vel,
0.5 * (self.old_u + self.u),
0.5 * (self.old_f + self.f), current)
k3_pos, k3_vel = self.step(self.p + self.period * 0.5 * k2_pos,
self.v + self.period * 0.5 * k2_vel,
0.5 * (self.old_u + self.u),
0.5 * (self.old_f + self.f), current)
k4_pos, k4_vel = self.step(self.p + self.period * k3_pos,
self.v + self.period * k3_vel, self.u,
self.f, current)
self.p = self.p + self.period / 6.0 * (k1_pos + 2.0 * k2_pos +
2.0 * k3_pos + k4_pos)
self.v = self.v + self.period / 6.0 * (k1_vel + 2.0 * k2_vel +
2.0 * k3_vel + k4_vel)
self.p[3] = cola2_lib.wrapAngle(self.p[3])
self.p[4] = cola2_lib.wrapAngle(self.p[4])
self.p[5] = cola2_lib.wrapAngle(self.p[5])
# Publish odometry
self.pub_odometry()
def compute_waypoints(self,
target_position,
initial_angle,
z,
track_length=5.0,
number_of_views=8):
"""Compute reacquisition pattern waypoints."""
angle_inc = math.radians(360.0 / number_of_views)
waypoints = list()
for i in range(self.number_of_views):
angle = cola2_lib.wrapAngle(angle_inc * i + initial_angle)
x = math.cos(angle) * track_length + target_position[0]
y = math.sin(angle) * track_length + target_position[1]
waypoints.append([x, y, z])
sorted_waypoints = list()
for i in range(0, self.number_of_views / 2, 2):
sorted_waypoints.append(waypoints[i])
sorted_waypoints.append(waypoints[i + self.number_of_views / 2])
sorted_waypoints.append(waypoints[i + self.number_of_views / 2 +
1])
sorted_waypoints.append(waypoints[i + 1])
print waypoints
return sorted_waypoints
def perform_star_maneuver(self, x, y, depth, radius, sides):
"""Perform star maneuver."""
self.transit_to(x, y, 0.0)
angle_error_sum = 0.0
angle_delta = 2.0 * math.pi * math.floor(sides / 2) / sides
angle = 0.0
x1 = x + radius
y1 = y
for i in range(sides):
angle_new = angle + angle_delta
x2 = x + math.cos(angle_new) * radius
y2 = y + math.sin(angle_new) * radius
self.star_segment(x1, y1, x2, y2, depth)
desired = math.atan2(y2 - y1, x2 - x1)
achieved = math.atan2(self.last_nav.position.east - y1,
self.last_nav.position.north - x1)
error = math.degrees(cola2_lib.wrapAngle(achieved - desired))
angle_error_sum = angle_error_sum + error
rospy.loginfo('%s: transect error = %s degrees', self.name,
str(error))
angle = angle_new
x1 = x2
y1 = y2
rospy.loginfo('%s: final mean error = %s degrees', self.name,
str(angle_error_sum / float(sides)))
self.transit_to(x, y, 0.0)
rospy.loginfo('%s: done', self.name)
def facing(self, angle, depth, max_angle_error):
"""Face specific orientation while reaching desired Z."""
ww_req = WorldWaypointReq()
ww_req.goal.requester = self.name + "_pose"
ww_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
ww_req.position.depth = depth
ww_req.orientation.yaw = cola2_lib.wrapAngle(angle)
ww_req.disable_axis.x = True
ww_req.disable_axis.y = True
ww_req.disable_axis.z = False
ww_req.disable_axis.roll = True
ww_req.disable_axis.pitch = True
ww_req.disable_axis.yaw = False
r = rospy.Rate(10)
while abs(self.vehicle_angle -
angle) > max_angle_error and not self.disable_trajectory:
ww_req.header.stamp = rospy.Time.now()
self.pub_world_waypoint_req.publish(ww_req)
r.sleep()
def rotate_wp(self, wp):
x = self.inspection_points[wp][0] * np.cos(self.angle_offset) - self.inspection_points[wp][1] * np.sin(self.angle_offset)
y = self.inspection_points[wp][1] * np.cos(self.angle_offset) + self.inspection_points[wp][0] * np.sin(self.angle_offset)
z = self.inspection_points[wp][2]
yaw = cola2_lib.wrapAngle(self.inspection_points[wp][3] + self.angle_offset)
return [x, y, z, yaw]
def __init__(self):
"""Constructor."""
# ======================================================================
# Config
# ======================================================================
self.config = cola2_ros_lib.Config()
self.config.mode = 'cont'
# ======================================================================
# Parameters
# ======================================================================
self.dr_server = DynamicReconfigureServer(MicronConfig,
self.reconfigure)
self.get_params()
# App data
self.scan_right_ = True
self.angle_ = self.config.llim
# Received data
self.sonar_range_ = 0.0
self.sonar_range_available_ = False
self.joint_state_ = JointState()
self.joint_state_available_ = False
self.tf_broadcaster_ = tf.TransformBroadcaster()
# ======================================================================
# Subscribers
# ======================================================================
# Navigation data (feedback)
self.nav_sts_sub_ = rospy.Subscriber("/cola2_navigation/nav_sts",
NavSts,
self.navStsSubCallback,
queue_size=1)
self.nav_sts_available_ = False
# Simulated sonar range and joint state
rospy.Subscriber('/seaking_range',
Range,
self.handleRange,
queue_size=1)
rospy.Subscriber('/uwsim/joint_state',
JointState,
self.handleJointState,
queue_size=1)
# ======================================================================
# Publishers
# ======================================================================
self.joint_sonar_command_pub_ = rospy.Publisher(
'/uwsim/joint_state_command', JointState, queue_size=1)
self.sonar_slice_pub_ = rospy.Publisher(
'/tritech_' + self.config.sonarType + '_slice',
LaserScan,
queue_size=1)
self.sonar_beam_pub_ = rospy.Publisher('/tritech_' +
self.config.sonarType + '_beam',
LaserScan,
queue_size=1)
# ======================================================================
# Waiting for nav sts
# ======================================================================
rospy.logwarn("%s: waiting for navigation data", rospy.get_name())
r = rospy.Rate(5)
while not rospy.is_shutdown() and not self.nav_sts_available_:
r.sleep()
rospy.logwarn("%s: navigation data received", rospy.get_name())
# ======================================================================
# Sonar to initial position
# ======================================================================
rospy.loginfo("%s: wait 5s to uwsim be completely loaded",
rospy.get_name())
rospy.sleep(5.0)
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'seaking_profiler'
msg.name = [self.jointname]
msg.position = [cola2_lib.wrapAngle(self.angle_)]
msg.velocity = []
msg.effort = []
self.joint_sonar_command_pub_.publish(msg)
self.accum_ranges_ = []
self.accum_angles_ = []
self.last_angle_ = self.angle_
# ======================================================================
# Setup periodic task to revolve the profiler
# ======================================================================
rospy.Timer(rospy.Duration(1 / self.config.angle_inc_rate),
self.keepRevolving)
return
def wrapAngle(self, angle):
"""Wrap angle bepween Pi and -Pi."""
angle = cola2_lib.wrapAngle(angle)
if angle < 0:
angle = angle + 2 * math.pi
return angle
def keepRevolving(self, event):
"""Keep the micron join Revolving."""
"""
Revolves the joint on AUV (G500 and SPARUS-II) where the range
sensor is attached to.
"""
# r = rospy.Rate(self.config.angle_inc_rate)
# while not rospy.is_shutdown():
if self.joint_state_available_:
# ==================================================================
# Get last range
# ==================================================================
if len(self.accum_ranges_) == 0:
self.first_beam_time_ = rospy.Time.now()
if self.sonar_range_ < self.config.max_range:
self.accum_ranges_.append(self.sonar_range_)
rospy.logdebug("%s: Grabbed new beam", rospy.get_name())
else:
self.accum_ranges_.append(self.config.max_range)
rospy.logdebug("%s: Discarding beam > max_range",
rospy.get_name())
self.accum_angles_.append(self.last_angle_)
# ==================================================================
# Revolve simulated sonar
# ==================================================================
rospy.logdebug("%s: moving joint", rospy.get_name())
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'seaking_profiler'
msg.name = [self.jointname]
msg.position = [cola2_lib.wrapAngle(self.angle_)]
msg.velocity = []
msg.effort = []
self.last_angle_ = self.angle_
self.joint_sonar_command_pub_.publish(msg)
rospy.logdebug("-------------------------------------------------")
if self.config.mode == 'sector':
rospy.logdebug("Sector")
else:
rospy.logdebug("Cont")
rospy.logdebug("Scan right = " + str(self.scan_right_))
rospy.logdebug("Angle: {0} ({1} - {2})".format(
math.degrees(self.angle_), math.degrees(self.config.llim),
math.degrees(self.config.rlim)))
# Update angle
# TODO what if sector goes through 0 and rlim>180 or llim<180?
change_direction = False
if self.config.mode == 'sector' and self.config.rlim > self.config.llim:
if self.scan_right_ and self.angle_ >= self.config.rlim:
self.scan_right_ = False
change_direction = True
elif not self.scan_right_ and self.angle_ <= self.config.llim:
self.scan_right_ = True
change_direction = True
elif self.config.mode == 'sector' and self.config.rlim < self.config.llim:
angle = cola2_lib.wrapAngle(self.angle_)
rlim = cola2_lib.wrapAngle(self.config.rlim)
llim = cola2_lib.wrapAngle(self.config.llim)
if self.scan_right_ and angle >= rlim:
self.scan_right_ = False
change_direction = True
elif not self.scan_right_ and angle <= llim:
self.scan_right_ = True
change_direction = True
elif self.config.mode != 'cont':
self.angle_ = self.config.rlim
if self.scan_right_:
ainc = self.config.angle_inc
else:
ainc = -self.config.angle_inc
self.angle_ = self.wrapAngle(self.angle_ + ainc)
# ==================================================================
# Checking if laser_scan is complete to be published
# ==================================================================
if len(self.accum_ranges_
) + 1 > self.config.publish_every or change_direction:
rospy.logdebug("len(self.accum_ranges_): " +
str(len(self.accum_ranges_)))
rospy.logdebug("self.accum_ranges_: " +
str(self.accum_ranges_))
rospy.logdebug("self.accum_angles_: " +
str(self.accum_angles_))
msg = LaserScan()
# timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
msg.header.stamp = self.first_beam_time_
msg.header.frame_id = self.config.frame_id
if self.scan_right_:
# angular distance between measurements [rad]
msg.angle_increment = self.config.angle_inc
# start angle of the scan [rad]
msg.angle_min = min(self.accum_angles_)
# end angle of the scan [rad]
msg.angle_max = max(self.accum_angles_)
else:
msg.angle_increment = -self.config.angle_inc
# start angle of the scan [rad]
msg.angle_min = max(self.accum_angles_)
# end angle of the scan [rad]
msg.angle_max = min(self.accum_angles_)
# time between measurements [seconds] - if your scanner
msg.time_increment = 1.0 / self.config.angle_inc_rate
# is moving, this will be used in interpolating position
# of 3d points
# time between scans [seconds]
msg.scan_time = msg.time_increment * (len(self.accum_ranges_) -
1)
# minimum range value [m]
msg.range_min = self.config.min_range
# max(self.accum_ranges) maximum range value [m]
msg.range_max = self.config.max_range
msg.ranges = self.accum_ranges_
msg.intensities = []
self.accum_ranges_ = []
self.accum_angles_ = []
self.tf_broadcaster_.sendTransform(
(self.translation[0], self.translation[1],
self.translation[2]),
tf.transformations.quaternion_from_euler(
self.rotation_rpy[0], self.rotation_rpy[1],
self.rotation_rpy[2]), msg.header.stamp,
self.config.frame_id, self.config.robot_frame_name)
self.sonar_slice_pub_.publish(msg)
else:
rospy.logwarn("%s: no joint state available", rospy.get_name())