diff --git a/src/simulation_pkg/simulation_pkg/decision_making.py b/src/simulation_pkg/simulation_pkg/decision_making.py index aaf9743..53c2a99 100644 --- a/src/simulation_pkg/simulation_pkg/decision_making.py +++ b/src/simulation_pkg/simulation_pkg/decision_making.py @@ -472,6 +472,10 @@ def calculate_pass_probability(self, bot: Robot, teammate: Robot) -> float: # Prefer passes forward forward_factor = 1.0 if bot.position.x - teammate.position.x < 0 else 0.0 + + # If lane is totally clear, ignore distance penalty + if line_obstruction < 0.1: + distance_factor = 1.0 - (distance / 3) # Combine factors (you can adjust weights) pass_probability = ( @@ -573,6 +577,9 @@ def draw_pass_line(self, line: List[Position], image): self.visualizer.draw_dotted_line(image, (sx, sy), (ex, ey), (0, 255, red)) sx, sy = ex, ey + + + # --- Modified "We Have Ball" logic --- def _handle_we_have_ball(self): """Handle decision making when we have the ball—with shot/pass logic.""" @@ -611,10 +618,27 @@ def _handle_we_have_ball(self): target_y = ball_handler.possession_position.y + dy * scale ball_handler.target_position = Position(target_x, target_y, ball_handler.position.z) - ball_handler.target_theta = math.atan2(goal_pos.y - ball_handler.position.y, - goal_pos.x - ball_handler.position.x) - self.get_logger().info(f"Ball handler {ball_handler.id} advancing toward goal at ({target_x:.2f}, {target_y:.2f}) (goal_prob: {goal_prob:.2f})") + # ball_handler.target_theta = math.atan2(goal_pos.y - ball_handler.position.y, + # goal_pos.x - ball_handler.position.x) + # self.get_logger().info(f"Ball handler {ball_handler.id} advancing toward goal at ({target_x:.2f}, {target_y:.2f}) (goal_prob: {goal_prob:.2f})") + + + + + + + + # self.get_logger().info( + # f"Ball handler {ball_handler.id} advancing toward goal at ({target_x:.2f}, {target_y:.2f}) " + # f"(goal_prob: {goal_prob:.2f}) facing theta: {math.degrees(theta):.2f}°" + # ) + + + + + + # Decision branches: GOAL_PROB_THRESHOLD = 0.45 PASS_PROB_THRESHOLD = 0.7 @@ -625,9 +649,43 @@ def _handle_we_have_ball(self): if pos_error < 0.5: command_msg = String() kick_speed = 700 # For a shot, use a relatively high speed. + ball_handler.target_theta = math.atan2(goal_pos.y - ball_handler.position.y, goal_pos.x - ball_handler.position.x) command_msg.data = f"KICK {int(kick_speed)} 45" self.command_pub.publish(command_msg) self.get_logger().info(f"Ball handler {ball_handler.id} shooting with command: {command_msg.data}") + + # if goal_prob > GOAL_PROB_THRESHOLD: + # # If near the target, attempt a shot. + # pos_error = math.sqrt((ball_handler.position.x - target_x)**2 + (ball_handler.position.y - target_y)**2) + # if pos_error < 0.5: + # # Step 1: Align towards goal + # dx = target_x - ball_handler.position.x + # dy = target_y - ball_handler.position.y + # desired_orientation = math.atan2(dy, dx) + # orientation_error = desired_orientation - ball_handler.orientation + + # if abs(orientation_error) > 0.1: # Small threshold to decide when to align + # command_msg = String() + # angular_speed = 0.5 if orientation_error > 0 else -0.5 + # command_msg.data = f"TURN {angular_speed}" + # self.command_pub.publish(command_msg) + # self.get_logger().info( + # f"Ball handler {ball_handler.id} aligning to goal with command: {command_msg.data}" + # ) + # else: + # # Step 2: Shoot once aligned + # command_msg = String() + # kick_speed = 700 # For a shot, use a relatively high speed. + # command_msg.data = f"KICK {int(kick_speed)} 45" + # self.command_pub.publish(command_msg) + # self.get_logger().info( + # f"Ball handler {ball_handler.id} shooting with command: {command_msg.data}" + # ) + + + + + elif best_pass_id is not None and best_pass_prob > PASS_PROB_THRESHOLD: # Attempt a pass if one is not already in progress. @@ -702,6 +760,51 @@ def _handle_we_have_ball(self): self.our_robots[robot_id].target_position = target_pos # self.get_logger().info(f"Robot {robot_id} moving to support at position ({target_pos.x:.2f}, {target_pos.y:.2f})") + + + else: + + ## ORIENTATION LOGIC ## + orientation_vec = np.zeros(2) + epsilon = 1e-3 + opponent_threat_dist = 2.0 # opponent changing orientation + + # Face toward the teammate with the highest pass probability (if any) + if best_pass_id is not None and best_pass_prob > 0: + teammate = self.our_robots[best_pass_id] + vec = np.array([ + teammate.position.x - ball_handler.position.x, + teammate.position.y - ball_handler.position.y + ]) + dist = np.linalg.norm(vec) + if dist > epsilon: + orientation_vec += vec / dist # normalized vector toward best pass target + + # Penalize facing nearby opponents + for opponent in self.opponent_robots: + vec = np.array([ + opponent.position.x - ball_handler.position.x, + opponent.position.y - ball_handler.position.y + ]) + dist = np.linalg.norm(vec) + if dist > epsilon and dist <= opponent_threat_dist: + weight = 1.0 / (dist + epsilon) # stronger penalty for closer opponents + orientation_vec -= 0.5 * weight * (vec / dist) # penalty factor + + # Fallback: face goal if orientation vector too small + if np.linalg.norm(orientation_vec) < 1e-3: + orientation_vec = np.array([dx, dy]) # face toward goal + + theta = math.atan2(orientation_vec[1], orientation_vec[0]) + ball_handler.target_theta = theta + + self.get_logger().info( + f"Ball handler {ball_handler.id} aiming toward best pass target or goal " + f"(goal_prob: {goal_prob:.2f}, best_pass_prob: {best_pass_prob:.2f}) " + f"facing theta: {math.degrees(theta):.2f}°" + ) + + def _handle_loose_ball(self): """Handle decision making when the ball is loose (only invoked when no pass is underway).""" if self.pass_in_progress: