Offense fsm skel (#2204)

Co-authored-by: Sid Parikh <[email protected]>
Co-authored-by: p-nayak11 <[email protected]>

CMakeLists.txt

@@ -66,19 +66,19 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 
 # Because we use ninja, we have to explicitly turn on color output for the compiler
 if("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fcolor-diagnostics -Werror=return-stack-address")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fcolor-diagnostics -Werror=return-stack-address -Werror=switch")
   set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld")
   set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fuse-ld=lld")
 else()
   message(WARNING "You are using GCC; prefer to use clang if it is installed with the flags `-DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++`.")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Werror=return-local-addr")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Werror=return-local-addr -Werror=switch")
 endif()
 
 # Use compiler optimization if we are making a release build.
 set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O2 -march=native")
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g -Og")
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Og")
 set(CMAKE_CXX_FLAGS_DEBUG
-    "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor")
+    "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor -Werror=switch")
 
 # ======================================================================
 # Testing

makefile

@@ -72,7 +72,7 @@ run-sim:
 # run our stack with default flags
 # TODO: actually name our software stack something
 run-our-stack:
-	ros2 launch rj_robocup soccer.launch.py run_sim:=True
+	ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True
 
 # run sim with external referee (SSL Game Controller)
 run-sim-external:
@@ -86,7 +86,7 @@ run-real:
 
 # run on real field computer with real robots and external ref (SSL GC)
 run-real-ex:
-	ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False use_internal_ref:=False 
+	ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False use_internal_ref:=False
 
 # run on real field comp, with real robots and manual control node to override AI movement
 # use util/manual_control_connect.bash to connect
@@ -104,7 +104,7 @@ run-sim2play:
 run-sim2: run-sim2play
 
 # control two teams of robots at once on the field
-# 
+#
 # as of 9/22/22, hardcoded for one team's server port=25565 (the one-team
 # default), other=25564
 run-real2play:

soccer/src/soccer/planning/planner/collect_path_planner.cpp

@@ -14,6 +14,12 @@ using namespace rj_geometry;
 namespace planning {
 
 Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) {
+    const auto state = plan_request.play_state.state();
+    if (state == PlayState::Stop || state == PlayState::Halt) {
+        // This planner automatically fails if the robot is prohibited from touching the ball.
+        return Trajectory{};
+    }
+
     BallState ball = plan_request.world_state->ball;
 
     const RJ::Time cur_time = plan_request.start.stamp;

soccer/src/soccer/planning/planner/escape_obstacles_path_planner.cpp

@@ -17,7 +17,7 @@ Trajectory EscapeObstaclesPathPlanner::plan(const PlanRequest& plan_request) {
     const auto& motion_constraints = plan_request.constraints.mot;
 
     rj_geometry::ShapeSet obstacles;
-    fill_obstacles(plan_request, &obstacles, nullptr, false, nullptr);
+    fill_obstacles(plan_request, &obstacles, nullptr, true, nullptr);
 
     if (!obstacles.hit(start_instant.position())) {
         // Keep moving, but slow down the current velocity. This allows us to

soccer/src/soccer/planning/planner/line_kick_path_planner.cpp

@@ -53,7 +53,7 @@ Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) {
     const BallState& ball = plan_request.world_state->ball;
 
     // Distance to stay away from the ball
-    auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy;
+    auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy * 4;
 
     // In case the ball is (slowly) moving
     auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position;
@@ -96,7 +96,7 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
 void LineKickPathPlanner::process_state_transition() {
     // Let PathTarget decide when the first stage is done
     // Possible problem: can PathTarget get stuck and loop infinitely?
-    if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) {
+    if (current_state_ == INITIAL_APPROACH && path_target_.is_done()) {
         current_state_ = FINAL_APPROACH;
     }
 }

soccer/src/soccer/planning/planner/pivot_path_planner.cpp

@@ -105,7 +105,6 @@ bool PivotPathPlanner::is_done() const {
     if (!cached_angle_change_.has_value()) {
         return false;
     }
-
     return cached_angle_change_.value() < IS_DONE_ANGLE_CHANGE_THRESH;
 }
 

soccer/src/soccer/planning/planner/pivot_path_planner.hpp

@@ -32,6 +32,7 @@ class PivotPathPlanner : public PathPlanner {
         previous_ = Trajectory{};
         cached_pivot_target_ = std::nullopt;
         cached_pivot_point_ = std::nullopt;
+        cached_angle_change_ = std::nullopt;
     }
     [[nodiscard]] bool is_done() const override;
 

soccer/src/soccer/planning/planner/settle_path_planner.cpp

@@ -19,6 +19,12 @@ using namespace rj_geometry;
 namespace planning {
 
 Trajectory SettlePathPlanner::plan(const PlanRequest& plan_request) {
+    const auto state = plan_request.play_state.state();
+    if (state == PlayState::Stop || state == PlayState::Halt) {
+        // This planner automatically fails if the robot is prohibited from touching the ball.
+        return Trajectory{};
+    }
+
     BallState ball = plan_request.world_state->ball;
 
     const RJ::Time cur_time = plan_request.start.stamp;

soccer/src/soccer/planning/tests/planner_test.cpp

@@ -90,7 +90,7 @@ TEST(Planning, collect_basic) {
     world_state.ball.position = Point{1, 1};
     world_state.ball.velocity = Point{0, 0};
     world_state.ball.timestamp = RJ::now();
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"collect"},
                         RobotConstraints{},
@@ -114,7 +114,7 @@ TEST(Planning, collect_obstructed) {
     world_state.ball.timestamp = RJ::now();
     ShapeSet obstacles;
     obstacles.add(std::make_shared<Circle>(Point{.5, .5}, .2));
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"collect"},
                         RobotConstraints{},
@@ -141,7 +141,7 @@ TEST(Planning, collect_pointless_obs) {
     obstacles.add(std::make_shared<Circle>(Point{3, 3}, .2));
     obstacles.add(std::make_shared<Circle>(Point{-2, 3}, .2));
     obstacles.add(std::make_shared<Circle>(Point{0, 5}, .2));
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"collect"},
                         RobotConstraints{},
@@ -165,7 +165,7 @@ TEST(Planning, collect_moving_ball_quick) {
     world_state.ball.timestamp = RJ::now();
     ShapeSet obstacles;
     obstacles.add(std::make_shared<Circle>(Point{0, .5}, .2));
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"collect"},
                         RobotConstraints{},
@@ -189,7 +189,7 @@ TEST(Planning, collect_moving_ball_slow) {
     world_state.ball.timestamp = RJ::now();
     ShapeSet obstacles;
     obstacles.add(std::make_shared<Circle>(Point{-0.5, .5}, .2));
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"collect"},
                         RobotConstraints{},
@@ -213,7 +213,7 @@ TEST(Planning, collect_moving_ball_slow_2) {
     world_state.ball.timestamp = RJ::now();
     ShapeSet obstacles;
     obstacles.add(std::make_shared<Circle>(Point{0, .5}, .2));
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"collect"},
                         RobotConstraints{},
@@ -249,7 +249,8 @@ TEST(Planning, collect_random) {
                 Point{TestingUtils::random(&gen, -2.0, 2.0), TestingUtils::random(&gen, 0.5, 1.5)},
                 .2));
         }
-        PlayState play_state = PlayState::halt();
+        PlayState play_state =
+            PlayState::playing();  // Some planners now return no trajectory in halt
         PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                             MotionCommand{"collect"},
                             RobotConstraints{},
@@ -282,7 +283,7 @@ TEST(Planning, settle_basic) {
     world_state.ball.timestamp = RJ::now();
     ShapeSet obstacles;
     obstacles.add(std::make_shared<Circle>(Point{.5, .5}, .2));
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"settle"},
                         RobotConstraints{},
@@ -308,7 +309,7 @@ TEST(Planning, settle_pointless_obs) {
     world_state.ball.timestamp = RJ::now();
     ShapeSet obstacles;
     obstacles.add(std::make_shared<Circle>(Point{-1, 1.0}, .2));
-    PlayState play_state = PlayState::halt();
+    PlayState play_state = PlayState::playing();  // Some planners now return no trajectory in halt
     PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                         MotionCommand{"settle"},
                         RobotConstraints{},
@@ -345,7 +346,8 @@ TEST(Planning, settle_random) {
                 Point{TestingUtils::random(&gen, -2.0, 2.0), TestingUtils::random(&gen, .5, 1.5)},
                 .2));
         }
-        PlayState play_state = PlayState::halt();
+        PlayState play_state =
+            PlayState::playing();  // Some planners now return no trajectory in halt
         PlanRequest request{RobotInstant{{}, {}, RJ::now()},
                             MotionCommand{"settle"},
                             RobotConstraints{},

soccer/src/soccer/strategy/agent/agent_action_client.cpp

@@ -41,6 +41,10 @@ AgentActionClient::AgentActionClient(int r_id)
         "config/game_settings", 1,
         [this](rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); });
 
+    goalie_id_sub_ = create_subscription<rj_msgs::msg::Goalie>(
+        ::referee::topics::kGoalieTopic, 1,
+        [this](rj_msgs::msg::Goalie::SharedPtr msg) { goalie_id_callback(msg->goalie_id); });
+
     robot_communication_srv_ = create_service<rj_msgs::srv::AgentCommunication>(
         fmt::format("agent_{}_incoming", r_id),
         [this](const std::shared_ptr<rj_msgs::srv::AgentCommunication::Request> request,
@@ -67,7 +71,6 @@ AgentActionClient::AgentActionClient(int r_id)
             get_communication();
             check_communication_timeout();
         });
-
 }
 
 void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg) {
@@ -103,6 +106,14 @@ void AgentActionClient::field_dimensions_callback(
     current_position_->update_field_dimensions(field_dimensions);
 }
 
+void AgentActionClient::goalie_id_callback(int goalie_id) {
+    if (current_position_) {
+        current_position_->set_goalie_id(goalie_id);
+    }
+
+    goalie_id_ = goalie_id;
+}
+
 void AgentActionClient::alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg) {
     alive_robots_ = msg->alive_robots;
 }
@@ -175,7 +186,6 @@ void AgentActionClient::send_new_goal() {
 
 void AgentActionClient::goal_response_callback(
     std::shared_future<GoalHandleRobotMove::SharedPtr> future) {
-
     auto goal_handle = future.get();
     if (!goal_handle) {
         current_position_->set_goal_canceled();
@@ -184,15 +194,13 @@ void AgentActionClient::goal_response_callback(
 
 void AgentActionClient::feedback_callback(
     GoalHandleRobotMove::SharedPtr, const std::shared_ptr<const RobotMove::Feedback> feedback) {
-
     double time_left = rj_convert::convert_from_ros(feedback->time_left).count();
     if (current_position_ == nullptr) {
         current_position_->set_time_left(time_left);
     }
 }
 
 void AgentActionClient::result_callback(const GoalHandleRobotMove::WrappedResult& result) {
-
     switch (result.code) {
         case rclcpp_action::ResultCode::SUCCEEDED:
             // TODO: handle other return codes
@@ -350,7 +358,6 @@ void AgentActionClient::receive_response_callback(
 }
 
 void AgentActionClient::check_communication_timeout() {
-
     for (u_int32_t i = 0; i < buffered_responses_.size(); i++) {
         if (RJ::now() - buffered_responses_[i].created > timeout_duration_) {
             current_position_->receive_communication_response(buffered_responses_[i]);
@@ -359,5 +366,4 @@ void AgentActionClient::check_communication_timeout() {
     }
 }
 
-
 }  // namespace strategy

soccer/src/soccer/strategy/agent/agent_action_client.hpp

@@ -13,6 +13,7 @@
 #include <rj_convert/ros_convert.hpp>
 #include <rj_msgs/msg/alive_robots.hpp>
 #include <rj_msgs/msg/game_settings.hpp>
+#include <rj_msgs/msg/goalie.hpp>
 #include <rj_msgs/msg/world_state.hpp>
 #include <rj_utils/logging.hpp>
 
@@ -54,6 +55,7 @@ class AgentActionClient : public rclcpp::Node {
     rclcpp::Subscription<rj_msgs::msg::FieldDimensions>::SharedPtr field_dimensions_sub_;
     rclcpp::Subscription<rj_msgs::msg::AliveRobots>::SharedPtr alive_robots_sub_;
     rclcpp::Subscription<rj_msgs::msg::GameSettings>::SharedPtr game_settings_sub_;
+    rclcpp::Subscription<rj_msgs::msg::Goalie>::SharedPtr goalie_id_sub_;
     // TODO(Kevin): communication module pub/sub here (e.g. passing)
 
     // callbacks for subs
@@ -62,6 +64,7 @@ class AgentActionClient : public rclcpp::Node {
     void field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg);
     void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg);
     void game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg);
+    void goalie_id_callback(int goalie_id);
 
     std::unique_ptr<Position> current_position_;
 
@@ -129,6 +132,7 @@ class AgentActionClient : public rclcpp::Node {
     std::array<bool, kNumShells> alive_robots_{};
     bool is_simulated_ = false;
     static constexpr double field_padding_ = 0.3;
+    int goalie_id_;
 
     /**
      * @brief checks whether a robot is visible, in the field, and (if the game is not