-
Notifications
You must be signed in to change notification settings - Fork 186
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Added behavior for marking enemy robots (#2176)
Co-authored-by: Sid Parikh <[email protected]>
- Loading branch information
1 parent
66dd969
commit e52b026
Showing
5 changed files
with
142 additions
and
9 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| #include "marker.hpp" | ||
|
|
||
| namespace strategy { | ||
| Marker::Marker(FieldDimensions field_dimensions) { | ||
| this->y_bound = field_dimensions.length() / 2; | ||
| this->marker_follow_cutoff = field_dimensions.width() / 2; | ||
| } | ||
|
|
||
| std::optional<RobotIntent> Marker::get_task(RobotIntent intent, const WorldState* world_state, | ||
| [[maybe_unused]] FieldDimensions field_dimensions) { | ||
| this->y_bound = field_dimensions.length() / 2; | ||
| this->marker_follow_cutoff = field_dimensions.width() / 2; | ||
|
|
||
| rj_geometry::Point targetPoint = world_state->get_robot(false, target_).pose.position(); | ||
| rj_geometry::Point ballPoint = world_state->ball.position; | ||
| rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); | ||
| planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; | ||
| intent.motion_command = | ||
| planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; | ||
| return intent; | ||
| } | ||
|
|
||
| void Marker::choose_target(const WorldState* ws) { | ||
| // TODO: (James Vogt, github: jvogt23) | ||
| // If we ever use multiple Markers, they should choose different | ||
| // robots to track from each other. Logic for this operation must be | ||
| // added because multiple markers currently mark the same robot. | ||
| for (int i = 0; i < kNumShells; i++) { | ||
| if (std::fabs(ws->get_robot(false, i).pose.position().x()) < marker_follow_cutoff && | ||
| ws->get_robot(false, i).pose.position().y() < y_bound && | ||
| (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) { | ||
| target_ = i; | ||
| return; | ||
| } | ||
| } | ||
| target_ = -1; | ||
| } | ||
|
|
||
| bool Marker::target_out_of_bounds(const WorldState* ws) { | ||
| if (target_ == -1) return true; | ||
| if (ws->get_robot(false, target_).pose.position().y() > y_bound) { | ||
| target_ = -1; | ||
| return true; | ||
| } | ||
| return false; | ||
| } | ||
|
|
||
| int Marker::get_target() { return target_; } | ||
| } // namespace strategy |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,43 @@ | ||
| #pragma once | ||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <rj_geometry/segment.hpp> | ||
| #include <rj_msgs/msg/robot_state.hpp> | ||
| #include <rj_msgs/msg/world_state.hpp> | ||
|
|
||
| #include "position.hpp" | ||
| #include "rj_geometry/point.hpp" | ||
| #include "role_interface.hpp" | ||
|
|
||
| namespace strategy { | ||
|
|
||
| /** | ||
| * Represents a Marker, a defensive role that targets an enemy robot | ||
| * and follows it around the field while it is on our side, blocking passes. | ||
| */ | ||
| class Marker : public RoleInterface { | ||
| private: | ||
| int target_{-1}; | ||
|
|
||
| // Calculated from field dimensions - Prevent the marker from | ||
| // marking any enemies that are out of range; e. g. on the other | ||
| // side of the field or the sidelines. | ||
| float y_bound{FieldDimensions::kDefaultDimensions.length() / 2}; | ||
| float marker_follow_cutoff{FieldDimensions::kDefaultDimensions.width() / 2}; | ||
|
|
||
| public: | ||
| Marker(FieldDimensions field_dimensions); | ||
| ~Marker() = default; | ||
| Marker(const Marker& other) = default; | ||
| Marker(Marker&& other) = default; | ||
| Marker& operator=(const Marker& other) = default; | ||
| Marker& operator=(Marker&& other) = default; | ||
|
|
||
| std::optional<RobotIntent> get_task(RobotIntent intent, const WorldState* world_state, | ||
| FieldDimensions field_dimensions) override; | ||
|
|
||
| void choose_target(const WorldState* ws); | ||
| int get_target(); | ||
| bool target_out_of_bounds(const WorldState* ws); | ||
| }; | ||
| } // namespace strategy |