def angle_divide(self,angle_start,angle_stop):
angle_list = []
vel_mag_list = []
theta_diff = CircleFunctions.circle_dist(angle_start,angle_stop)
# if abs(theta_diff) < self.on_setpoint_theta_dist:
# return angle_list,vel_mag_list
if 0 < theta_diff:
if angle_stop < angle_start:
angle_start = angle_start - 2*math.pi
else:
if angle_start < angle_stop:
angle_stop = angle_stop - 2*math.pi
# rospy.logwarn("angle_start = \n%s" % (str(angle_start)))
# rospy.logwarn("angle_stop = \n%s" % (str(angle_stop)))
# rospy.logwarn("diff = \n%s" % (str(diff)))
point_count = 0
r = self.setpoint.radius
theta_diff_positive = 0 < theta_diff
finished = False
if 0 < r:
vel_mag_list = [1] # Use dummy first value...
angle = angle_start
angle_list.append(angle)
while (not finished) and (point_count <= self.point_count_max):
vel_mag = self.find_theta_vel_mag(theta_diff,r)
point_count += 1
chord_length = vel_mag/self.ltm.freq
# rospy.logwarn("chord_length = %s" % (str(chord_length)))
angle_inc = chord_length/r
if not theta_diff_positive:
angle_inc = - angle_inc
angle += angle_inc
theta_diff = CircleFunctions.circle_dist(angle,angle_stop)
if (abs(theta_diff) < self.on_setpoint_theta_mag) or \
((theta_diff < 0) and theta_diff_positive) or \
((0 < theta_diff) and (not theta_diff_positive)):
finished = True
else:
vel_mag_list.append(vel_mag)
angle_list.append(angle)
# point_count = math.ceil(abs(diff)/angle_inc)
# if self.point_count_max < point_count:
# point_count = self.point_count_max
# angle_list = list(numpy.linspace(angle_start,angle_stop,num=point_count,endpoint=True))
# rospy.logwarn("angle_list = \n%s" % (str(angle_list)))
return angle_list,vel_mag_list,theta_diff_positive
def GetNextFlipValue(self):
# Get the prior flip value.
flipValuePrev = self.lpFlip.GetValue()
if flipValuePrev is None:
flipValuePrev = 0.0
# angleContour is ambiguous mod pi radians
eccmetric = (self.contour.ecc + 1/self.contour.ecc) - 1
#rospy.loginfo('%s: eccmetric=%0.2f' % (self.name, eccmetric))
if (eccmetric > 1.5):
angleContour = self.contour.angle #self.YawFromQuaternion(self.state.pose.orientation)
angleContour_flipped = angleContour + N.pi
# Most recent angle of travel.
if self.angleOfTravelRecent is not None:
angleOfTravel = self.angleOfTravelRecent
else:
angleOfTravel = angleContour
# Compare distances between angles of (travel - orientation) and (travel - flippedorientation)
dist = N.abs(CircleFunctions.circle_dist(angleContour, angleOfTravel))
dist_flipped = N.abs(CircleFunctions.circle_dist(angleContour_flipped, angleOfTravel))
# Vote for flipped or non-flipped.
if dist < dist_flipped:
flipValueNew = -1.0 # Not flipped
else:
flipValueNew = 1.0 # Flipped
else:
flipValueNew = flipValuePrev
# Weight the prev/new values based on speed.
speed = N.linalg.norm([self.state.velocity.linear.x, self.state.velocity.linear.y])
alpha = 10.0/(10.0+speed)
flipValueWeighted = (alpha * flipValuePrev) + ((1.0-alpha) * flipValueNew)
#if 'Fly1' in self.name:
# rospy.logwarn('%s: flipValue %0.2f, %0.2f, %0.2f' % (self.name, flipValuePrev,flipValueWeighted,flipValueNew))
return flipValueWeighted
def angle_divide(self, angle_start, angle_stop):
diff = CircleFunctions.circle_dist(angle_start, angle_stop)
if 0 < diff:
if angle_stop < angle_start:
angle_start = angle_start - 2 * math.pi
else:
if angle_start < angle_stop:
angle_stop = angle_stop - 2 * math.pi
# rospy.logwarn("angle_start = \n%s" % (str(angle_start)))
# rospy.logwarn("angle_stop = \n%s" % (str(angle_stop)))
# rospy.logwarn("diff = \n%s" % (str(diff)))
r = self.setpoint.radius
if 0 < r:
angle_inc = self.chord_length / r
point_count = math.ceil(abs(diff) / angle_inc)
if self.point_count_max < point_count:
point_count = self.point_count_max
angle_list = list(numpy.linspace(angle_start, angle_stop, num=point_count, endpoint=True))
else:
angle_list = []
# rospy.logwarn("angle_list = \n%s" % (str(angle_list)))
return angle_list
def SetFlipState(self):
# Update the flip filter, and convert it to a flip state.
flipValuePre = self.GetNextFlipValue()
flipValuePost = self.lpFlip.Update(flipValuePre, self.contour.header.stamp.to_sec())
if flipValuePost > 0.0:
flipNew = True
else:
flipNew = False
speed = N.linalg.norm([self.state.velocity.linear.x, self.state.velocity.linear.y])
if speed > 3.0: # Deadband.
self.flip = flipNew
# contour angle only ranges on [-pi,-0]. If wrapped, then change the flip state.
#if 'Fly1' in self.name:
# rospy.logwarn('self.flip=%s'%self.flip)
if (self.contourPrev is not None) and (isinstance(self.contourPrev.angle,float)) and (self.contour is not None) and (isinstance(self.contour.angle,float)):
#rospy.logwarn('contour.angle=%s, contourPrev.angle=%s'%(self.contour.angle, self.contourPrev.angle))
d = N.abs(CircleFunctions.circle_dist(self.contour.angle, self.contourPrev.angle))
if (d > (N.pi/2.0)):
#if 'Fly1' in self.name:
# rospy.logwarn('%s: wrap %0.2f, %0.2f, d=%0.2f'%(self.name, self.contour.angle, self.contourPrev.angle,d))
self.lpFlip.SetValue(-self.lpFlip.GetValue())
self.flip = not self.flip
def find_robot_setpoint_error(self):
self.robot_arena = self.convert_to_arena(self.robot_origin)
self.robot_control_frame = self.convert_to_control_frame(self.robot_origin)
dx = self.robot_control_frame.point.x
dy = self.robot_control_frame.point.y
self.robot_control_frame_radius = math.sqrt(dx**2 + dy**2)
self.robot_control_frame_theta = math.atan2(dy,dx)
self.radius_error = self.setpoint.radius - self.robot_control_frame_radius
self.theta_error = CircleFunctions.circle_dist(self.robot_control_frame_theta,self.setpoint.theta)
# rospy.logwarn("radius_error = %s" % (str(radius_error)))
# rospy.logwarn("theta_error = %s" % (str(theta_error)))
# self.on_setpoint_radius = abs(radius_error) < self.on_setpoint_radius_dist
# self.on_setpoint_theta = abs(theta_error) < self.on_setpoint_theta_dist
# if (not self.on_setpoint_radius) and (not self.on_setpoint_theta):
# if abs(radius_error) < self.on_setpoint_radius_dist:
# self.on_setpoint_radius = True
# # rospy.logwarn("on setpoint radius, not on setpoint theta")
# elif self.on_setpoint_radius and (not self.on_setpoint_theta):
# if abs(theta_error) < self.on_setpoint_theta_dist:
# self.on_setpoint_theta = True
# # rospy.logwarn("on setpoint radius, on setpoint theta")
# elif self.on_setpoint_theta:
# self.on_setpoint_radius = abs(self.radius_error) < self.on_setpoint_radius_dist
if not self.ltm.in_progress:
self.on_setpoint_radius = abs(self.radius_error) < self.on_setpoint_dist
if self.on_setpoint_radius:
self.near_setpoint_radius = False
else:
self.near_setpoint_radius = abs(self.radius_error) < self.near_setpoint_dist
else:
self.on_setpoint_radius = abs(self.radius_error) < self.lookup_table_move_setpoint_dist_multiplier*self.on_setpoint_dist
if self.on_setpoint_radius:
self.near_setpoint_radius = False
else:
self.near_setpoint_radius = abs(self.radius_error) < self.lookup_table_move_setpoint_dist_multiplier*self.near_setpoint_dist
if self.setpoint_radius_thresh < self.setpoint.radius:
self.on_setpoint_theta_mag = 2*math.atan(self.on_setpoint_dist/(2*self.setpoint.radius))
self.near_setpoint_theta_mag = 2*math.atan(self.near_setpoint_dist/(2*self.setpoint.radius))
self.on_setpoint_theta = abs(self.theta_error) < self.on_setpoint_theta_mag
if self.on_setpoint_theta:
self.near_setpoint_theta = False
else:
self.near_setpoint_theta = abs(self.theta_error) < self.near_setpoint_theta_mag
if self.ltm.in_progress and \
((not self.on_setpoint_theta) and (not self.near_setpoint_theta)) and \
(((self.theta_error < 0) and self.ltm.direction_positive) or ((0 < self.theta_error) and (not self.ltm.direction_positive))):
rospy.logwarn("using theta_error and ltm.direction_positive...")
rospy.logwarn("theta_error = %s" % (str(self.theta_error)))
rospy.logwarn("ltm.direction_positive = %s" % (str(self.ltm.direction_positive)))
# self.ltm.stop_move()
# self.on_setpoint_theta = False
# self.near_setpoint_theta = True
else:
self.on_setpoint_theta_mag = 0
self.near_setpoint_theta_mag = 0
self.on_setpoint_theta = True
self.near_setpoint_theta = False