Skip to content

improved we have ball and improved pass probability #5

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
architagarwal2103 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

improved we have ball and improved pass probability #5

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
109 changes: 106 additions & 3 deletions src/simulation_pkg/simulation_pkg/decision_making.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 = (
Expand Down Expand Up @@ -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."""
Expand Down Expand Up @@ -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
Expand All @@ -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.
Expand Down Expand Up @@ -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:
Expand Down