Rewrite Offense Position

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,13 @@ 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/settle_path_planner.cpp

@@ -19,6 +19,13 @@ 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;
@@ -460,6 +467,7 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta
     dampen_end.set_debug_text("Damping");
 
     if (!previous_.empty()) {
+        SPDLOG_INFO("stamp {} {}",previous_.last().stamp.time_since_epoch().count(), dampen_end.first().stamp.time_since_epoch().count());
         dampen_end = Trajectory(previous_, dampen_end);
     }
 

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

@@ -41,6 +41,10 @@
         "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 @@
             get_communication();
             check_communication_timeout();
         });
-
 }
 
 void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg) {
@@ -103,6 +106,14 @@
     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;
 }
@@ -176,7 +187,6 @@
 
 void AgentActionClient::goal_response_callback(
     std::shared_future<GoalHandleRobotMove::SharedPtr> future) {
-
     auto goal_handle = future.get();
     if (!goal_handle) {
         current_position_->set_goal_canceled();
@@ -185,15 +195,13 @@
 
 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
@@ -351,7 +359,6 @@
 }
 
 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]);
@@ -360,5 +367,4 @@
     }
 }
 
-
 }  // 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::vector<u_int8_t> 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