def ArenaState_callback(self, arenastate):
if self.initialized:
robot_x = arenastate.robot.pose.position.x
robot_y = arenastate.robot.pose.position.y
try:
fly_x = arenastate.flies[0].pose.position.x
fly_y = arenastate.flies[0].pose.position.y
except:
return
# Get and publish InBounds.
robot_dist = N.sqrt((robot_x - self.start_position_x) ** 2 + (robot_y - self.start_position_y) ** 2)
fly_dist = N.sqrt((fly_x - self.start_position_x) ** 2 + (fly_y - self.start_position_y) ** 2)
self.in_bounds.bounds_radius = self.radiusArena
if robot_dist < self.radiusArena:
self.in_bounds.robot_in_bounds = True
else:
self.in_bounds.robot_in_bounds = False
if fly_dist < self.radiusArena:
self.in_bounds.fly_in_bounds = True
else:
self.in_bounds.fly_in_bounds = False
self.pubInBounds.publish(self.in_bounds)
# Get & publish the FlyView.
try:
self.tfrx.waitForTransform("Fly1", self.robot_origin.header.frame_id, rospy.Time(), rospy.Duration(5.0))
self.robot_origin_fly_frame = self.tfrx.transformPoint("Fly1", self.robot_origin)
self.fly_view.robot_position_x = rx = self.robot_origin_fly_frame.point.x
self.fly_view.robot_position_y = ry = self.robot_origin_fly_frame.point.y
self.fly_view.robot_angle = CircleFunctions.mod_angle(N.arctan2(ry, rx))
self.fly_view.robot_distance = N.sqrt(rx ** 2 + ry ** 2)
self.pubFlyView.publish(self.fly_view)
except (tf.Exception):
pass
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 GetNextFlipValue(self):
# Get the prior flip value.
flipvalue = self.lpFlip.GetValue()
if (flipvalue is not None):
magFlipvalue = np.abs(flipvalue)
signFlipvalue = np.sign(flipvalue)
else:
magFlipvalue = 0.0
signFlipvalue = 1.0
if signFlipvalue == 0.0:
signFlipvalue = 1.0
angle = self.GetResolvedAngleFiltered()
# Compare distances between angles of (travel - prevorientation) and (travel - flippedprevorientation)
dist_current = np.abs(
CircleFunctions.DistanceCircle(angle, self.angleOfTravelRecent))
dist_flipped = np.abs(
CircleFunctions.DistanceCircle(angle + np.pi,
self.angleOfTravelRecent))
if (self.speed > self.params['tracking']['speedThresholdForTravel']):
if (self.contourinfo.ecc > 0.6):
magFlipvalueNew = 1.0
# Choose the better orientation.
if (dist_flipped < dist_current):
signFlipvalue = -signFlipvalue # Flipped
else:
magFlipvalueNew = magFlipvalue
else:
magFlipvalueNew = magFlipvalue
flipvalueNew = signFlipvalue * magFlipvalueNew
#if 'Fly1' in self.name:
# rospy.logwarn('flipvalue Old,New: % 3.2f, % 3.2f' % (flipvalue, flipvalueNew))
# rospy.logwarn('angle,travel: % 3.2f, % 3.2f dist,flipped: % 3.2f, % 3.2f' % (angle, self.angleOfTravelRecent, dist_current, dist_flipped))
return flipvalueNew
def ArenaState_callback(self, arenastate):
if self.initialized:
robot_x = arenastate.robot.pose.position.x
robot_y = arenastate.robot.pose.position.y
try:
fly_x = arenastate.flies[0].pose.position.x
fly_y = arenastate.flies[0].pose.position.y
except:
return
# Get and publish InBounds.
robot_dist = np.sqrt((robot_x - self.start_position_x)**2 +
(robot_y - self.start_position_y)**2)
fly_dist = np.sqrt((fly_x - self.start_position_x)**2 +
(fly_y - self.start_position_y)**2)
self.in_bounds.bounds_radius = self.radiusArena
if robot_dist < self.radiusArena:
self.in_bounds.robot_in_bounds = True
else:
self.in_bounds.robot_in_bounds = False
if fly_dist < self.radiusArena:
self.in_bounds.fly_in_bounds = True
else:
self.in_bounds.fly_in_bounds = False
self.pubInBounds.publish(self.in_bounds)
# Get & publish the FlyView.
try:
self.tfrx.waitForTransform("Fly1",
self.robot_origin.header.frame_id,
rospy.Time(), rospy.Duration(5.0))
self.robot_origin_fly_frame = self.tfrx.transformPoint(
"Fly1", self.robot_origin)
self.fly_view.robot_position_x = rx = self.robot_origin_fly_frame.point.x
self.fly_view.robot_position_y = ry = self.robot_origin_fly_frame.point.y
self.fly_view.robot_angle = CircleFunctions.mod_angle(
np.arctan2(ry, rx))
self.fly_view.robot_distance = np.sqrt(rx**2 + ry**2)
self.pubFlyView.publish(self.fly_view)
except (tf.Exception):
pass
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 Update(self, z, t):
if not isinstance(z, float):
z = None
if (self.zf is not None) and (self.t is not None):
if (z is not None) and (t is not None):
zUnwrapped = CircleFunctions.UnwrapHalfCircle(z, self.zf)
dt = t - self.t
tt = (self.RC + dt)
if (tt > 0.0):
alpha = dt / (self.RC + dt)
else:
alpha = 0.0
self.zf = (1.0 - alpha) * self.zf + alpha * zUnwrapped
else: # Initialized, but no measurement.
pass
else: # Not initialized, but have a measurement.
self.zf = z
self.t = t
return self.zf
def joystick_commands_callback(self,data):
if self.initialized:
self.control_frame = data.header.frame_id
self.setpoint.header.frame_id = self.control_frame
self.setpoint_center_origin.header.frame_id = self.control_frame
self.setpoint_origin.header.frame_id = self.control_frame
self.setpoint_int_origin.header.frame_id = self.control_frame
self.setpoint.radius += self.inc_radius*data.radius_inc
if self.setpoint.radius < self.setpoint_radius_min:
self.setpoint.radius = self.setpoint_radius_min
elif self.setpoint_radius_max < self.setpoint.radius:
self.setpoint.radius = self.setpoint_radius_max
self.setpoint.theta += self.inc_theta*data.theta_inc
self.setpoint.theta = CircleFunctions.mod_angle(self.setpoint.theta)
# self.setpoint.theta = math.fmod(self.setpoint.theta,2*math.pi)
# if self.setpoint.theta < 0:
# self.setpoint.theta = 2*math.pi - self.setpoint.theta
if data.tracking and (not self.tracking):
self.tracking = True
if data.goto_start and (not self.goto_start):
self.goto_start = True
self.tracking = False
if data.stop:
self.goto_start = False
self.tracking = False
self.setpoint_pub.publish(self.setpoint)
self.setpoint_origin.point.x = self.setpoint.radius*math.cos(self.setpoint.theta)
self.setpoint_origin.point.y = self.setpoint.radius*math.sin(self.setpoint.theta)
self.setpoint_arena = self.convert_to_arena(self.setpoint_origin)
# rospy.logwarn("setpoint_origin.point.x = %s" % (str(self.setpoint_origin.point.x)))
# rospy.logwarn("setpoint_origin.point.y = %s" % (str(self.setpoint_origin.point.y)))
if not self.tracking:
if self.goto_start:
if not self.moving_to_start:
self.moving_to_start = True
self.stage_commands.position_control = True
self.stage_commands.velocity_control = False
self.stage_commands.lookup_table_correct = False
self.set_path_to_start(self.robot_velocity_max)
self.sc_ok_to_publish = True
self.goto_start = False
else:
self.sc_ok_to_publish = False
else:
self.sc_ok_to_publish = True
if self.moving_to_start:
self.moving_to_start = False
self.stage_commands.position_control = False
self.stage_commands.velocity_control = True
self.stage_commands.lookup_table_correct = False
self.radius_velocity = data.radius_velocity*self.robot_velocity_max
self.tangent_velocity = data.tangent_velocity*self.robot_velocity_max
self.vel_vector_arena[0,0] = data.x_velocity*self.robot_velocity_max
self.vel_vector_arena[1,0] = data.y_velocity*self.robot_velocity_max
try:
(self.stage_commands.x_velocity,self.stage_commands.y_velocity) = self.vel_vector_convert(self.vel_vector_arena,"Arena")
except (tf.LookupException, tf.ConnectivityException):
self.stage_commands.x_velocity = [self.vel_vector_arena[0,0]]
self.stage_commands.y_velocity = [-self.vel_vector_arena[1,0]]
if self.sc_ok_to_publish:
self.sc_pub.publish(self.stage_commands)
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
