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