Skip to content
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.

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

Check out the wall chucker #2325

Open
wants to merge 13 commits into
base: ros2
Choose a base branch
from
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

please uncommit this. your local settings.json are not a part of the shared code. (you can later make a case for inserting this into our .gitignore file, if you please, or you can locally tell get to ignore this file using various methods )

Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
{
"cmake.sourceDirectory": "/home/applhau/robocup-software/soccer/src/soccer/strategy/agent/position/framework/src/tests"
}
94 changes: 70 additions & 24 deletions soccer/src/soccer/strategy/agent/position/defense.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,15 +36,15 @@ Defense::State Defense::update_state() {
}

switch (current_state_) {
case IDLING:
break;
case JOINING_WALL:
case IDLING:{
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this seems like it breaks code style (no space before '{' and no newline before '}'). did you run a formatter? it's also possible our formatter doesn't catch this case (blocks in a case statement is a bit nonstandard but I do love them)

break;}
case JOINING_WALL:{
send_join_wall_request();
// SPDLOG_INFO("join wall {}", robot_id_);
next_state = WALLING;
walling_robots_ = {(u_int8_t)robot_id_};
break;
case WALLING:
break;}
case WALLING:{
// If a wall is already full,
// Remove the robot with the highest ID from a wall
// and make them a marker instead.
Expand All @@ -54,43 +54,69 @@ Defense::State Defense::update_state() {
// SPDLOG_INFO("leave wall {}", robot_id_);
next_state = ENTERING_MARKING;
}
break;
case SEARCHING:
break;
case RECEIVING:
//wall stealing
if(can_steal_ball()) {
send_leave_wall_request();
next_state = STEALING;
}
break;}
case SEARCHING:{
break;}
case RECEIVING:{
// transition to idling if we are close enough to the ball
if (distance_to_ball < ball_receive_distance_) {
next_state = IDLING;
}
break;
case PASSING:
break;}
case PASSING:{
// transition to idling if we no longer have the ball (i.e. it was passed or it was
// stolen)
if (check_is_done()) {
next_state = IDLING;
if (check_is_done() || distance_to_ball > ball_lost_distance_) {
next_state = JOINING_WALL;
}

if (distance_to_ball > ball_lost_distance_) {
next_state = IDLING;
}
break;
case FACING:
break;}
case FACING:{
if (check_is_done()) {
next_state = IDLING;
}
case MARKING:
}}
case MARKING:{
if (marker_.get_target() == -1 || marker_.target_out_of_bounds(world_state)) {
next_state = ENTERING_MARKING;
}
break;
case ENTERING_MARKING:
break;}
case ENTERING_MARKING:{
marker_.choose_target(world_state);
int target_id = marker_.get_target();
if (target_id == -1) {
next_state = ENTERING_MARKING;
} else {
next_state = MARKING;
}
break;}
case STEALING: // wall steal
{
// Go to passing if successful
if (check_is_done()) {
send_leave_wall_request();
next_state = CLEARING;
}

if (!can_steal_ball()) {
next_state = JOINING_WALL;
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we probably want break here and in the below case


/*if (timed_out()) {
// If we timed out and the ball is close, assume we have it
// (because is_done for settle/collect are not great)
next_state = IDLING;
}*/
}
case CLEARING:
{
if(check_is_done()) {
next_state = JOINING_WALL;
}
}
}

return next_state;
Expand Down Expand Up @@ -145,7 +171,7 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
if (!walling_robots_.empty() && waller_id_ != -1) {
Waller waller{waller_id_, walling_robots_};
return waller.get_task(intent, last_world_state_, this->field_dimensions_);
}
}
} else if (current_state_ == FACING) {
rj_geometry::Point robot_position =
last_world_state_->get_robot(true, robot_id_).pose.position();
Expand All @@ -164,6 +190,26 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
} else if (current_state_ == MARKING) {
// Marker marker = Marker((u_int8_t) robot_id_);
return marker_.get_task(intent, last_world_state_, this->field_dimensions_);
} else if(current_state_ == STEALING) { //wall stealer
auto collect_cmd = planning::MotionCommand{"collect"};
intent.motion_command = collect_cmd;
intent.dribbler_speed = 255.0;
return intent;
} else if(current_state_ == CLEARING) {
planning::LinearMotionInstant target{clear_point_};
auto line_kick_cmd = planning::MotionCommand{"line_kick", target};
intent.motion_command = line_kick_cmd;

// note: the way this is set up makes it impossible to
// shoot on time without breakbeam
// TODO(Kevin): make intent hold a manip msg instead? to be cleaner?
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i think it's about time to stop adding TODOs for @kfu02, lol.

i know you just copied this from somewhere but let's take this out, and maybe make a clickup card if we think this task is actually a good idea

intent.shoot_mode = RobotIntent::ShootMode::CHIP;
intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;
intent.kick_speed = 4.0;
intent.dribbler_speed = 255.0;
intent.is_active = true;

return intent;
}

return std::nullopt;
Expand Down
68 changes: 68 additions & 0 deletions soccer/src/soccer/strategy/agent/position/defense.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ class Defense : public Position {
void revive() override;

private:

const rj_geometry::Point clear_point_{0.0, 4.5};

// static constexpr int kMaxWallers{6};
static constexpr int kMaxWallers{
static_cast<int>(kNumShells)}; // This effectively turns off marking
Expand All @@ -69,6 +72,8 @@ class Defense : public Position {
FACING, // turning to face the passing robot
MARKING, // Following closely to an offense robot
ENTERING_MARKING, // Choosing/waiting for a robot to mark
STEALING, // wall stealing
CLEARING,
};

State update_state();
Expand All @@ -80,11 +85,74 @@ class Defense : public Position {
*/
void send_join_wall_request();



/**
* @brief Sends a LeaveWallRequest to each of the robots in walling_robots_.
*/
void send_leave_wall_request();

/**
* @return if the current state has timed out
* wall steal
*/
/*bool timed_out() const {
// Defined here so it can be inlined
using namespace std::chrono_literals;


return (max_time > 0s) && (last_time_ + max_time < RJ::now());
};*/

/**
* @return distance from this agent to ball
* wall steal
*/
double distance_to_ball() const {
return last_world_state_->ball.position.dist_to(
last_world_state_->get_robot(true, robot_id_).pose.position());
};

/**
* @brief This FSM has timeouts for certain states.
* Ideally, these would not be necessary; as planners get more sophisticated
* they should not get "stuck". However, empirically, the offense FSM in particular
* has been observed to deadlock often.
*
* One common case is when waiting for a receiver to accept a pass; if no receiver responds,
* the timeout is necessary. In the future receivers may be able to respond in the negative
* instead of ignoring the request.
*
* The timeouts are a safety mechanism, and should not be the primary reason for a
* state transition. They are set relatively high for this reason.
*
* @return the maximum duration to stay in a given state, or -1 if there is no maximum.
*
*/
static constexpr RJ::Seconds timeout(State s) {
switch (s) {
case PASSING:
return RJ::Seconds{5};
case STEALING:
return RJ::Seconds{10};
}
}

/**
* @return if the current state has timed out
*/
bool timed_out() const {
// Defined here so it can be inlined
using namespace std::chrono_literals;

const auto max_time = timeout(current_state_);

return (max_time > 0s) && (last_time_ + max_time < RJ::now());
};

// The time at which the last state started.
RJ::Time last_time_;

/**
* @brief Adds the new waller to this robot's list of wallers and updates this robot's position
* in the wall.
Expand Down
1 change: 1 addition & 0 deletions soccer/src/soccer/strategy/agent/position/framework
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

also uncommit this, please. you seem to have cloned ER-Force's framework git repo inside of this one. git repos don't nest well unfortunately, because now you've checked their entire repo into ours.

Submodule framework added at a6c523
98 changes: 49 additions & 49 deletions soccer/src/soccer/strategy/agent/position/offense.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -268,7 +268,7 @@ std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {
auto collect_cmd = planning::MotionCommand{"collect"};
intent.motion_command = collect_cmd;
intent.dribbler_speed = 255.0;
// }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

crazy find. looks correct, but how was the previous code compiling?



return intent;
}
Expand Down Expand Up @@ -521,47 +521,47 @@ double Offense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry:
return min_angle;
}

bool Offense::can_steal_ball() const {
// Ball in red zone or not
if (ball_in_red()) {
return false;
}
// Ball location
rj_geometry::Point ball_position = this->last_world_state_->ball.position;

// Our robot is closest robot to ball
bool closest = true;

auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position();

auto our_dist = (current_pos - ball_position).mag();
for (auto enemy : this->last_world_state_->their_robots) {
auto dist = (enemy.pose.position() - ball_position).mag();
if (dist < our_dist) {
closest = false;
break;
}
}

if (!closest) {
return closest;
}

for (auto pal : this->last_world_state_->our_robots) {
// if (pal.robot_id_ == robot_id_) {
// continue;
// }
auto dist = (pal.pose.position() - ball_position).mag();
if (dist < our_dist) {
closest = false;
break;
}
}

return closest;

// return distance_to_ball() < kStealBallRadius;
}
// bool Offense::can_steal_ball() const {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

not a fan of commenting-out methods. either we need them or we delete them, IMO. if we temporarily don't need them, then at least leave a comment explaining. but honestly no harm in leaving them there, in that case.

// // Ball in red zone or not
// if (ball_in_red()) {
// return false;
// }
// // Ball location
// rj_geometry::Point ball_position = this->last_world_state_->ball.position;

// // Our robot is closest robot to ball
// bool closest = true;

// auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position();

// auto our_dist = (current_pos - ball_position).mag();
// for (auto enemy : this->last_world_state_->their_robots) {
// auto dist = (enemy.pose.position() - ball_position).mag();
// if (dist < our_dist) {
// closest = false;
// break;
// }
// }

// if (!closest) {
// return closest;
// }

// for (auto pal : this->last_world_state_->our_robots) {
// // if (pal.robot_id_ == robot_id_) {
// // continue;
// // }
// auto dist = (pal.pose.position() - ball_position).mag();
// if (dist < our_dist) {
// closest = false;
// break;
// }
// }

// return closest;

// // return distance_to_ball() < kStealBallRadius;
// }

rj_geometry::Point Offense::calculate_best_shot() const {
// Goal location
Expand All @@ -587,13 +587,13 @@ rj_geometry::Point Offense::calculate_best_shot() const {
return best_shot;
}

// Checks whether ball is out of range for stealing/receiving
bool Offense::ball_in_red() const {
auto& ball_pos = last_world_state_->ball.position;
return (field_dimensions_.our_defense_area().contains_point(ball_pos) ||
field_dimensions_.their_defense_area().contains_point(ball_pos) ||
!field_dimensions_.field_rect().contains_point(ball_pos));
}
// // Checks whether ball is out of range for stealing/receiving
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same comment as above.

// bool Offense::ball_in_red() const {
// auto& ball_pos = last_world_state_->ball.position;
// return (field_dimensions_.our_defense_area().contains_point(ball_pos) ||
// field_dimensions_.their_defense_area().contains_point(ball_pos) ||
// !field_dimensions_.field_rect().contains_point(ball_pos));
// }
void Offense::broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding) {
communication::SeekerRequest seeker_request{};
communication::generate_uid(seeker_request);
Expand Down
17 changes: 9 additions & 8 deletions soccer/src/soccer/strategy/agent/position/offense.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -210,10 +210,10 @@ class Offense : public Position {
*/
double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const;

/**
* @brief Check if this agent could easily steal the ball
*/
bool can_steal_ball() const;
// /**
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's get rid of these entirely if we're elevating to Position (rather than commenting out)

// * @brief Check if this agent could easily steal the ball
// */
// bool can_steal_ball() const;

/**
* @return distance from this agent to ball
Expand All @@ -233,14 +233,15 @@ class Offense : public Position {
*/
rj_geometry::Point calculate_best_shot() const;

/**
* @return whether the ball is in an area that non-goalies cannot reach.
*/
bool ball_in_red() const;
// /**
// * @return whether the ball is in an area that non-goalies cannot reach.
// */
// bool ball_in_red() const;

void broadcast_seeker_request(rj_geometry::Point seeking_point, bool adding);

std::unordered_map<int, rj_geometry::Point> seeker_points_;
};


} // namespace strategy
Loading
Loading