def react(self, this_robot, sensor_dump, visualize=False): twist = Twist() image = sensor_dump.lower_image # Relying on only a single forward-pointing ray causes distant pucks # to be easily missed (they may be detected, but too sporadically to # attract the robot). We accept as forward-pointing any sensor ray # within 'front_angle_threshold' of zero. Correspondingly set the # two predicates 'react_to_puck' and 'react_to_robot'. react_to_puck = False react_to_robot = False for i in range(image.n_rows): angle = image.calib_array[i, 4] dist = image.calib_array[i, 5] if fabs(angle) < self.front_angle_threshold: if image.masks[i] & self.puck_mask != 0: react_to_puck = True if image.masks[i] == ROBOT_MASK: react_to_robot = True if react_to_robot: react_to_puck = False # Now react... if react_to_puck: # Turn left twist.linear = self.linear_speed twist.angular = self.angular_speed if visualize: draw_ray(this_robot, 0, (255, 0, 255)) elif react_to_robot: # Turn left and slow twist.linear = self.linear_speed * self.slow_factor twist.angular = self.angular_speed if visualize: # Reacting to robot draw_ray(this_robot, 0, (255, 0, 0)) else: # Turn right twist.linear = self.linear_speed twist.angular = -self.angular_speed if visualize: draw_ray(this_robot, 0, (0, 255, 0)) return twist
def react(self, lscan): (cl_dist, cl_angle, cl_mask) = self.get_lmark_dist_angle_mask(lscan) draw_circle(self.body.position, 2, (255, 255, 255)) if cl_angle != None and cl_dist > 0: if cl_mask == ARC_LANDMARK_MASK: draw_ray(self, cl_angle, (0, 255, 0), 20, 1) elif cl_mask == BLAST_LANDMARK_MASK: draw_ray(self, cl_angle + pi, (255, 0, 0), 20, 1) elif (cl_mask == POLE_LANDMARK_MASK): # We'll average all pole landmark responses. vx = 0 vy = 0 for i in range(len(lscan.ranges)): if (lscan.masks[i] & POLE_LANDMARK_MASK != 0): vx += cos(lscan.angles[i]) vy += sin(lscan.angles[i]) combined_angle = atan2(vy, vx) draw_ray(self, combined_angle, (0, 0, 255), 20, 1)
def act(self, robot, sensor_dump, visualize=False): """ Set wheel speeds according to processed sensor data. """ image = sensor_dump.lower_image # Check if there are no landmarks in view, but there are wall pixels we # can use for guidance. react_to_wall = False react_to_wall_angle = None if self.guidance_vec == None: closest_wall_angle = None closest_wall_range = float('inf') for i in range(image.n_rows): if image.masks[i] == WALL_MASK: wall_angle = image.calib_array[i, 4] wall_range = image.calib_array[i, 5] if wall_range < closest_wall_range: closest_wall_range = wall_range react_to_wall_angle = wall_angle react_to_wall = True # Check for the presence of another robot in the centre of the image. react_to_robot = False react_to_robot_angle = None for i in range(image.n_rows): if image.masks[i] == ROBOT_MASK: (xr, yr) = image.calib_array[i, 2], image.calib_array[i, 3] bot_a = image.calib_array[i, 4] bot_r = image.calib_array[i, 5] if fabs(bot_a) < pi / 4 and bot_r < self.robot_react_range: react_to_robot = True react_to_robot_angle = bot_a twist = Twist() if react_to_robot: # Turn slow in the open direction twist.linear = self.slow_factor * self.linear_speed twist.angular = self.angular_speed if visualize: draw_ray(robot, react_to_robot_angle, color=(255, 0, 0)) elif react_to_wall: if visualize: draw_ray(robot, react_to_wall_angle, color=(255, 255, 0)) twist = self.curve_to_angle(pi / 2, react_to_wall_angle) elif self.guidance_vec == None: # No landmarks in view AND no walls in view. Just go straight. twist.linear = self.linear_speed elif (self.state == "ODO_HOME" or (self.odo_condition == "NO_ODO" and self.target_range_bearing != None)): (x, y, theta) = sensor_dump.odo_pose c = cos(theta) s = sin(theta) goal_Rx = c * (self.odo_goal_x - x) + s * (self.odo_goal_y - y) goal_Ry = -s * (self.odo_goal_x - x) + c * (self.odo_goal_y - y) #twist.linear = 0.1 * goal_Rx #twist.angular = 0.2 * goal_Ry twist.linear = self.linear_speed * sign(goal_Rx) twist.angular = self.angular_speed * sign(goal_Ry) if visualize: draw_segment_wrt_robot(robot, (0, 0), (goal_Rx, goal_Ry), color=(255, 0, 255), width=3) else: twist = self.curve_to_angle(-pi / 2, self.guidance_angle) return twist
def act(self, robot, sensor_dump, visualize=False): """ Set wheel speeds according to processed sensor data. """ image = sensor_dump.lower_image # Check if there are no landmarks in view, but there are wall pixels we # can use for guidance. react_to_wall = False react_to_wall_angle = None # if self.guidance_vec == None: closest_wall_angle = None closest_wall_range = self.wall_react_range for i in range(image.n_rows): if image.masks[i] == WALL_MASK: wall_angle = image.calib_array[i,4] wall_range = image.calib_array[i,5] if wall_range < closest_wall_range: closest_wall_range = wall_range react_to_wall_angle = wall_angle react_to_wall = True # Check for the presence of another robot in the centre of the image. react_to_robot = False react_to_robot_angle = None for i in range(image.n_rows): if image.masks[i] == ROBOT_MASK: (xr, yr) = image.calib_array[i,2], image.calib_array[i,3] bot_a = image.calib_array[i,4] bot_r = image.calib_array[i,5] if fabs(bot_a) < pi/4 and bot_r < self.robot_react_range: react_to_robot = True react_to_robot_angle = bot_a # Quantity referenced in a couple of places below: if self.guidance_vec != None: dist_to_lmarks = sqrt(self.guidance_vec[0]**2 + self.guidance_vec[1]**2) twist = Twist() action_print = None if react_to_robot: # Turn slow in the open direction twist.linear = self.slow_factor * self.linear_speed twist.angular = self.angular_speed action_print = "ROBOT" if visualize: draw_ray(robot, react_to_robot_angle, color=(255,0,0)) elif react_to_wall: action_print = "WALL" if visualize: draw_ray(robot, react_to_wall_angle, color=(255,0,255)) #twist = self.curve_to_angle(pi/2, react_to_wall_angle, self.wall_turn_on_spot) twist.linear = uniform(-0.1, 0.1) * self.linear_speed twist.angular = -self.angular_speed elif self.guidance_vec == None: # No landmarks in view AND no walls in view. Just go straight. twist.linear = self.linear_speed action_print = "STRAIGHT (NO LANDMARKS OR WALLS IN VIEW)" elif self.target_pos != None and dist_to_lmarks > self.puck_dist_threshold: ##twist.linear = self.linear_speed * sign(self.target_pos[0]) ##twist.angular = self.angular_speed * sign(self.target_pos[1]) #twist.linear = self.linear_speed * 1.0 * self.target_pos[0] #twist.angular = self.angular_speed * 20.0 * self.target_pos[1] #twist = self.cap_twist(twist) angle = atan2(self.target_pos[1], self.target_pos[0]) twist.linear = self.linear_speed * cos(angle) twist.angular = self.angular_speed * sin(angle) action_print = "TARGETING" if visualize: draw_segment_wrt_robot(robot, (0, 0), (self.target_pos[0], self.target_pos[1]), color=(255, 255, 255), width=3) else: # FIRST METHOD: NO CONTROL OVER DISTANCE #twist = self.curve_to_angle(-pi/2, self.guidance_angle, self.no_target_turn_on_spot) # SECOND METHOD: BANG-BANG CONTROL OVER DISTANCE BY CURVING IN OR OUT # BY A FIXED ANGLE """ dist = sqrt(self.guidance_vec[0]**2 + self.guidance_vec[1]**2) gamma = self.curve_gamma twist = self.curve_to_angle(-pi/2 + gamma, self.guidance_angle) if dist < self.lmark_ideal_range: twist = self.curve_to_angle(-pi/2 - gamma, self.guidance_angle) action_print = "CURVING OUT" if visualize: draw_ray(robot, gamma, color=(255,0,0)) else: twist = self.curve_to_angle(-pi/2 + gamma, self.guidance_angle) action_print = "CURVING IN" if visualize: draw_ray(robot, -gamma, color=(0,255,0)) """ dist = dist_to_lmarks ideal_dist = self.lmark_ideal_range # We want to use proportional control to keep the robot at # self.lmark_ideal_range away from the landmarks, while facing # orthogonal to them. So we need an error signal for distance. # The most simpleminded such signal is this one: # dist_error = ideal_dist - dist # But its unknown scale presents a problem. So instead we use the # following which lies in the range [-1, 1]: dist_error = (ideal_dist - min(dist, 2*ideal_dist)) / (ideal_dist) # One consequence of using this error signal is that the response # at a distance greater than 2*ideal_dist is the same as at # 2*ideal_dist. # But we also have to consider the current angle w.r.t. the # guidance angle. Ideally, the guidance angle should be at -pi/2 # w.r.t. the robot's forward axis when dist_error == 0. If # dist_error is positive (meaning we are too close to the # landmarks) then the ideal guidance angle should be in the range # [-pi, -pi/2]. Otherwise, the ideal guidance angle should be in # the range [0, -pi/2]. We will use dist_error to scale linearly # within these ranges as we compute the ideal guidance angle. pi_2 = pi / 2.0 ideal_guidance_angle = -pi_2 - pi_2 * dist_error action_print = "PROPORTIONAL CONTROL" # Now define the angle_error angle_error = get_smallest_signed_angular_difference( self.guidance_angle, ideal_guidance_angle) twist = self.diff_to_twist_bang_bang(angle_error) landmark_print = None if self.closest_pair != None: landmark_print = "PAIR" if visualize: draw_segment_wrt_robot(robot, self.closest_pair[0], self.closest_pair[1], color=(255,255,255), width=3) draw_segment_wrt_robot(robot, (0,0), self.guidance_vec, color=(0,255,255), width=3) elif self.guidance_vec != None: landmark_print = "SINGLE" if visualize: draw_segment_wrt_robot(robot, (0,0), self.guidance_vec, color=(255,255,0), width=3) else: landmark_print = "NO LANDMARK" if visualize: print("{}: {}".format(landmark_print, action_print)) return twist
def act(self, robot, sensor_dump, visualize=False): """ Set wheel speeds according to processed sensor data. """ image = sensor_dump.lower_image # Check if there are no landmarks in view, but there are wall pixels we # can use for guidance. react_to_wall = False react_to_wall_angle = None # if self.guidance_vec == None: closest_wall_angle = None closest_wall_range = self.wall_react_range for i in range(image.n_rows): if image.masks[i] == WALL_MASK: wall_angle = image.calib_array[i, 4] wall_range = image.calib_array[i, 5] if wall_range < closest_wall_range: closest_wall_range = wall_range react_to_wall_angle = wall_angle react_to_wall = True # Check for the presence of another robot in the centre of the image. react_to_robot = False react_to_robot_angle = None for i in range(image.n_rows): if image.masks[i] == ROBOT_MASK: (xr, yr) = image.calib_array[i, 2], image.calib_array[i, 3] bot_a = image.calib_array[i, 4] bot_r = image.calib_array[i, 5] if fabs(bot_a) < pi / 4 and bot_r < self.robot_react_range: react_to_robot = True react_to_robot_angle = bot_a twist = Twist() if react_to_robot: # Turn slow in the open direction twist.linear = self.slow_factor * self.linear_speed twist.angular = self.angular_speed if visualize: print("ROBOT") draw_ray(robot, react_to_robot_angle, color=(255, 0, 0)) elif react_to_wall: if visualize: print("WALL") draw_ray(robot, react_to_wall_angle, color=(255, 255, 0)) #twist = self.curve_to_angle(pi/2, react_to_wall_angle, self.wall_turn_on_spot) twist.linear = uniform(-0.1, 0.1) * self.linear_speed twist.angular = -self.angular_speed elif self.guidance_vec == None: # No landmarks in view AND no walls in view. Just go straight. twist.linear = self.linear_speed if visualize: print("STRAIGHT (NO LANDMARKS OR WALLS IN VIEW)") elif self.target_pos != None: twist.linear = self.linear_speed * sign(self.target_pos[0]) twist.angular = self.angular_speed * sign(self.target_pos[1]) if visualize: print("TARGETING") draw_segment_wrt_robot( robot, (0, 0), (self.target_pos[0], self.target_pos[1]), color=(255, 255, 255), width=3) else: #twist = self.curve_to_angle(-pi/2, self.guidance_angle, self.no_target_turn_on_spot) dist = sqrt(self.guidance_vec[0]**2 + self.guidance_vec[1]**2) if dist < self.lmark_ideal_range: twist = self.curve_to_angle(-pi / 2 - 0.25, self.guidance_angle, self.no_target_turn_on_spot) if visualize: print("CURVING OUT") else: twist = self.curve_to_angle(-pi / 2 + 0.25, self.guidance_angle, self.no_target_turn_on_spot) if visualize: print("CURVING IN") return twist
def react(self, this_robot, sensor_suite, visualize=False): twist = Twist() scan = sensor_suite.range_scan n = len(scan.ranges) centre_angle = 0 # EXPERIMENT TO ACHIEVE A MINIMUM SIZE AGGREGATE. """ lscan = sensor_suite.landmark_scan closest_lmark_distance = float('inf') for i in range(len(lscan.ranges)): if (lscan.masks[i] & ARC_LANDMARK_MASK != 0 and lscan.ranges[i] < closest_lmark_distance): closest_lmark_distance = lscan.ranges[i] if closest_lmark_distance < 200: centre_angle = 0.3 """ # Relying on only a single forward-pointing ray causes distant pucks # to be easily missed (they may be detected, but too sporadically to # attract the robot). We accept as forward-pointing any sensor ray # within 'front_angle_threshold' of zero. Correspondingly set the # two predicates 'react_to_puck' and 'react_to_robot'. react_to_puck = False react_to_robot = False for i in range(n): if fabs(scan.angles[i] + centre_angle) < self.front_angle_threshold: if (scan.masks[i] & self.puck_mask) != 0: react_to_puck = True if scan.masks[i] == ROBOT_MASK: react_to_robot = True if react_to_robot: react_to_puck = False # Now react... if react_to_puck: print("PUCK") # Turn left twist.linear = self.linear_speed twist.angular = self.angular_speed if visualize: draw_ray(this_robot, 0, (255, 0, 255)) elif react_to_robot: print("ROBOT") # Turn left and slow twist.linear = self.linear_speed * self.slow_factor twist.angular = self.angular_speed if visualize: # Reacting to robot draw_ray(this_robot, 0, (255, 0, 0)) else: print("NO PUCK/ROBOT -RIGHT") # Turn right twist.linear = self.linear_speed twist.angular = -self.angular_speed if visualize: draw_ray(this_robot, 0, (0, 255, 0)) return twist