Planner State Simplification

makefile

@@ -30,7 +30,7 @@ endef
 
 define cmake_build_target_perf
 	mkdir -p build-release-debug
- 	cd build-release-debug && cmake -GNinja -Wno-dev -DNO_WALL=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo $(CMAKE_FLAGS) --target -DBUILD_TESTS=ON .. && ninja $(NINJA_FLAGS) $1 install
+ 	cd build-release-debug && cmake -DCMAKE_CXX_INCLUDE_WHAT_YOU_USE=/home/robojackets/iwyu/build/bin/include-what-you-use -GNinja -Wno-dev -DNO_WALL=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo $(CMAKE_FLAGS) --target -DBUILD_TESTS=ON .. && ninja $(NINJA_FLAGS) $1 install
 endef
 
 all-perf:
@@ -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:

soccer/src/soccer/CMakeLists.txt

@@ -42,6 +42,8 @@ set(ROBOCUP_LIB_SRC
     planning/planner/settle_path_planner.cpp
     planning/planner/goalie_idle_path_planner.cpp
     planning/planner_node.cpp
+    planning/planner_for_robot.cpp
+    planning/global_state.cpp
     planning/trajectory.cpp
     planning/trajectory_utils.cpp
     planning/trajectory_collection.cpp
@@ -95,7 +97,6 @@ set(SOCCER_TEST_SRC
     planning/tests/angle_planning_test.cpp
     planning/tests/bezier_path_test.cpp
     planning/tests/conversion_tests.cpp
-    planning/tests/planner_test.cpp
     planning/tests/create_path_test.cpp
     planning/tests/testing_utils.cpp
     planning/tests/trajectory_test.cpp

soccer/src/soccer/game_state.hpp

@@ -15,6 +15,8 @@
 
 struct PlayState {
 public:
+    PlayState() : PlayState{State::Halt, Restart::None, false, {}} {};
+
     using Msg = rj_msgs::msg::PlayState;
 
     enum State {

soccer/src/soccer/planning/global_state.cpp

@@ -0,0 +1,90 @@
+#include "global_state.hpp"
+
+namespace planning {
+
+GlobalState::GlobalState(rclcpp::Node* node) {
+    play_state_sub_ = node->create_subscription<rj_msgs::msg::PlayState>(
+        referee::topics::kPlayStateTopic, rclcpp::QoS(1),
+        [&](rj_msgs::msg::PlayState::SharedPtr state) {
+            auto lock = std::lock_guard<std::mutex>(last_play_state_mutex_);
+            last_play_state_ = rj_convert::convert_from_ros(*state);
+        });
+
+    game_settings_sub_ = node->create_subscription<rj_msgs::msg::GameSettings>(
+        config_server::topics::kGameSettingsTopic, rclcpp::QoS(1),
+        [&](rj_msgs::msg::GameSettings::SharedPtr settings) {
+            auto lock = std::lock_guard<std::mutex>(last_game_settings_mutex_);
+            last_game_settings_ = rj_convert::convert_from_ros(*settings);
+        });
+
+    goalie_sub_ = node->create_subscription<rj_msgs::msg::Goalie>(
+        referee::topics::kGoalieTopic, rclcpp::QoS(1), [&](rj_msgs::msg::Goalie::SharedPtr goalie) {
+            auto lock = std::lock_guard<std::mutex>(last_goalie_id_mutex_);
+            last_goalie_id_ = goalie->goalie_id;
+        });
+
+    global_obstacles_sub_ = node->create_subscription<rj_geometry_msgs::msg::ShapeSet>(
+        planning::topics::kGlobalObstaclesTopic, rclcpp::QoS(1),
+        [&](rj_geometry_msgs::msg::ShapeSet::SharedPtr global_obstacles) {
+            auto lock = std::lock_guard<std::mutex>(last_global_obstacles_mutex_);
+            last_global_obstacles_ = rj_convert::convert_from_ros(*global_obstacles);
+        });
+
+    def_area_obstacles_sub_ = node->create_subscription<rj_geometry_msgs::msg::ShapeSet>(
+        planning::topics::kDefAreaObstaclesTopic, rclcpp::QoS(1),
+        [&](rj_geometry_msgs::msg::ShapeSet::SharedPtr def_area_obstacles) {
+            auto lock = std::lock_guard<std::mutex>(last_def_area_obstacles_mutex_);
+            last_def_area_obstacles_ = rj_convert::convert_from_ros(*def_area_obstacles);
+        });
+
+    world_state_sub_ = node->create_subscription<rj_msgs::msg::WorldState>(
+        vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1),
+        [&](rj_msgs::msg::WorldState::SharedPtr world_state) {
+            auto lock = std::lock_guard<std::mutex>(last_world_state_mutex_);
+            last_world_state_ = rj_convert::convert_from_ros(*world_state);
+        });
+
+    coach_state_sub_ = node->create_subscription<rj_msgs::msg::CoachState>(
+        "/strategy/coach_state", rclcpp::QoS(1),
+        [&](rj_msgs::msg::CoachState::SharedPtr coach_state) {
+            auto lock = std::lock_guard<std::mutex>(last_coach_state_mutex_);
+            last_coach_state_ = *coach_state;
+        });
+}
+
+[[nodiscard]] PlayState GlobalState::play_state() const {
+    auto lock = std::lock_guard(last_play_state_mutex_);
+    return last_play_state_;
+}
+
+[[nodiscard]] GameSettings GlobalState::game_settings() const {
+    auto lock = std::lock_guard(last_game_settings_mutex_);
+    return last_game_settings_;
+}
+
+[[nodiscard]] int GlobalState::goalie_id() const {
+    auto lock = std::lock_guard(last_goalie_id_mutex_);
+    return last_goalie_id_;
+}
+
+[[nodiscard]] rj_geometry::ShapeSet GlobalState::global_obstacles() const {
+    auto lock = std::lock_guard(last_global_obstacles_mutex_);
+    return last_global_obstacles_;
+}
+
+[[nodiscard]] rj_geometry::ShapeSet GlobalState::def_area_obstacles() const {
+    auto lock = std::lock_guard(last_def_area_obstacles_mutex_);
+    return last_def_area_obstacles_;
+}
+
+[[nodiscard]] WorldState GlobalState::world_state() const {
+    auto lock = std::lock_guard(last_world_state_mutex_);
+    return last_world_state_;
+}
+
+[[nodiscard]] rj_msgs::msg::CoachState GlobalState::coach_state() const {
+    auto lock = std::lock_guard(last_coach_state_mutex_);
+    return last_coach_state_;
+}
+
+}  // namespace planning

soccer/src/soccer/planning/global_state.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <mutex>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <context.hpp>
+#include <rj_common/time.hpp>
+#include <rj_constants/topic_names.hpp>
+#include <rj_msgs/action/robot_move.hpp>
+#include <rj_msgs/msg/coach_state.hpp>
+#include <rj_msgs/msg/goalie.hpp>
+#include <rj_msgs/msg/manipulator_setpoint.hpp>
+#include <rj_msgs/msg/robot_status.hpp>
+#include <rj_msgs/srv/plan_hypothetical_path.hpp>
+#include <rj_param_utils/ros2_local_param_provider.hpp>
+
+#include "node.hpp"
+#include "world_state.hpp"
+#include "game_settings.hpp"
+#include "rj_geometry/shape_set.hpp"
+#include "game_state.hpp"
+
+namespace planning {
+
+class GlobalState {
+public:
+    GlobalState(rclcpp::Node* node);
+
+    [[nodiscard]] PlayState play_state() const;
+    [[nodiscard]] GameSettings game_settings() const;
+    [[nodiscard]] int goalie_id() const;
+    [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const;
+    [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const;
+    [[nodiscard]] WorldState world_state() const;
+    [[nodiscard]] rj_msgs::msg::CoachState coach_state() const;
+
+private:
+    rclcpp::Subscription<rj_msgs::msg::PlayState>::SharedPtr play_state_sub_;
+    rclcpp::Subscription<rj_msgs::msg::GameSettings>::SharedPtr game_settings_sub_;
+    rclcpp::Subscription<rj_msgs::msg::Goalie>::SharedPtr goalie_sub_;
+    rclcpp::Subscription<rj_geometry_msgs::msg::ShapeSet>::SharedPtr global_obstacles_sub_;
+    rclcpp::Subscription<rj_geometry_msgs::msg::ShapeSet>::SharedPtr def_area_obstacles_sub_;
+    rclcpp::Subscription<rj_msgs::msg::WorldState>::SharedPtr world_state_sub_;
+    rclcpp::Subscription<rj_msgs::msg::CoachState>::SharedPtr coach_state_sub_;
+
+    PlayState last_play_state_;
+    mutable std::mutex last_play_state_mutex_;
+
+    GameSettings last_game_settings_;
+    mutable std::mutex last_game_settings_mutex_;
+
+    int last_goalie_id_;
+    mutable std::mutex last_goalie_id_mutex_;
+
+    rj_geometry::ShapeSet last_global_obstacles_;
+    mutable std::mutex last_global_obstacles_mutex_;
+
+    rj_geometry::ShapeSet last_def_area_obstacles_;
+    mutable std::mutex last_def_area_obstacles_mutex_;
+
+    WorldState last_world_state_;
+    mutable std::mutex last_world_state_mutex_;
+
+    rj_msgs::msg::CoachState last_coach_state_;
+    mutable std::mutex last_coach_state_mutex_;
+};
+} // namespace planning

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

@@ -14,7 +14,7 @@ using namespace rj_geometry;
 namespace planning {
 
 Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) {
-    BallState ball = plan_request.world_state->ball;
+    BallState ball = plan_request.world_state.ball;
 
     const RJ::Time cur_time = plan_request.start.stamp;
 
@@ -99,29 +99,24 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) {
     // Check if we should transition to control from approach
     process_state_transition(ball, start_instant);
 
-    // List of obstacles
-    ShapeSet static_obstacles;
-    std::vector<DynamicObstacle> dynamic_obstacles;
-    fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false);
-
     switch (current_state_) {
         // Moves from the current location to the slow point of approach
         case CoarseApproach:
             previous_ =
-                coarse_approach(plan_request, start_instant, static_obstacles, dynamic_obstacles);
+                coarse_approach(plan_request, start_instant, plan_request.static_obstacles, plan_request.dynamic_obstacles);
             break;
         // Moves from the slow point of approach to just before point of contact
         case FineApproach:
             previous_ =
-                fine_approach(plan_request, start_instant, static_obstacles, dynamic_obstacles);
+                fine_approach(plan_request, start_instant, plan_request.static_obstacles, plan_request.dynamic_obstacles);
             break;
         // Move through the ball and stop
         case Control:
-            previous_ = control(plan_request, partial_start_instant, partial_path, static_obstacles,
-                                dynamic_obstacles);
+            previous_ = control(plan_request, partial_start_instant, partial_path, plan_request.static_obstacles,
+                                plan_request.dynamic_obstacles);
             break;
         default:
-            previous_ = invalid(plan_request, static_obstacles, dynamic_obstacles);
+            previous_ = invalid(plan_request, plan_request.static_obstacles, plan_request.dynamic_obstacles);
             break;
     }
 
@@ -173,7 +168,7 @@ Trajectory CollectPathPlanner::coarse_approach(
     const PlanRequest& plan_request, RobotInstant start,
     const rj_geometry::ShapeSet& static_obstacles,
     const std::vector<DynamicObstacle>& dynamic_obstacles) {
-    BallState ball = plan_request.world_state->ball;
+    BallState ball = plan_request.world_state.ball;
 
     // There are two paths that get combined together
     //
@@ -231,7 +226,7 @@ Trajectory CollectPathPlanner::fine_approach(
     const PlanRequest& plan_request, RobotInstant start_instant,
     const rj_geometry::ShapeSet& /* static_obstacles */,
     const std::vector<DynamicObstacle>& /* dynamic_obstacles */) {
-    BallState ball = plan_request.world_state->ball;
+    BallState ball = plan_request.world_state.ball;
     RobotConstraints robot_constraints_hit = plan_request.constraints;
     MotionConstraints& motion_constraints_hit = robot_constraints_hit.mot;
 
@@ -287,7 +282,7 @@ Trajectory CollectPathPlanner::control(const PlanRequest& plan_request, RobotIns
                                        const Trajectory& /* partial_path */,
                                        const rj_geometry::ShapeSet& static_obstacles,
                                        const std::vector<DynamicObstacle>& dynamic_obstacles) {
-    BallState ball = plan_request.world_state->ball;
+    BallState ball = plan_request.world_state.ball;
     RobotConstraints robot_constraints = plan_request.constraints;
     MotionConstraints& motion_constraints = robot_constraints.mot;
 
@@ -396,7 +391,7 @@ Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request,
     Replanner::PlanParams params{
         plan_request.start,       target,
         static_obstacles,         dynamic_obstacles,
-        plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position)};
+        plan_request.constraints, AngleFns::face_point(plan_request.world_state.ball.position)};
     Trajectory path = Replanner::create_plan(params, previous_);
     path.set_debug_text("Invalid state in collect");
 

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

@@ -1,25 +1,30 @@
 #include "escape_obstacles_path_planner.hpp"
-
-#include <optional>
-#include <vector>
-
-#include "planning/planning_params.hpp"
-#include "planning/primitives/angle_planning.hpp"
-#include "planning/primitives/create_path.hpp"
-#include "planning/primitives/robo_cup_state_space.hpp"
-#include "planning/primitives/rrt_util.hpp"
-
+#include <stddef.h>                                      // for size_t
+#include <functional>                                    // for function
+#include <memory>                                        // for make_shared
+#include <optional>                                      // for optional
+#include <rrt/Tree.hpp>                                  // for Tree, Node
+#include <vector>                                        // for allocator
+#include "planning/instant.hpp"                          // for RobotInstant
+#include "planning/planner/plan_request.hpp"             // for PlanRequest
+#include "planning/planning_params.hpp"                  // for PARAM_goal_c...
+#include "planning/primitives/angle_planning.hpp"        // for plan_angles
+#include "planning/primitives/create_path.hpp"           // for simple
+#include "planning/primitives/robo_cup_state_space.hpp"  // for RoboCupState...
+#include "planning/robot_constraints.hpp"                // for RobotConstra...
+#include "rj_common/field_dimensions.hpp"                // for FieldDimensions
+#include "rj_common/time.hpp"                            // for now
+#include "rj_geometry/point.hpp"                         // for Point, rj_ge...
+#include "rj_geometry/pose.hpp"                          // for Twist
+#include "rj_geometry/shape_set.hpp"                     // for ShapeSet
 using namespace rj_geometry;
 namespace planning {
 
 Trajectory EscapeObstaclesPathPlanner::plan(const PlanRequest& plan_request) {
     const RobotInstant& start_instant = plan_request.start;
     const auto& motion_constraints = plan_request.constraints.mot;
 
-    rj_geometry::ShapeSet obstacles;
-    fill_obstacles(plan_request, &obstacles, nullptr, false, nullptr);
-
-    if (!obstacles.hit(start_instant.position())) {
+    if (!plan_request.static_obstacles.hit(start_instant.position())) {
         // Keep moving, but slow down the current velocity. This allows us to
         // keep continuity when we have short disruptions in planners (i.e.
         // single frame delay).
@@ -32,7 +37,7 @@ Trajectory EscapeObstaclesPathPlanner::plan(const PlanRequest& plan_request) {
     }
 
     Point unblocked =
-        find_non_blocked_goal(start_instant.position(), previous_target_, obstacles, 300);
+        find_non_blocked_goal(start_instant.position(), previous_target_, plan_request.static_obstacles, 300);
 
     std::optional<Point> opt_prev_pt;
 

soccer/src/soccer/planning/planner/escape_obstacles_path_planner.hpp

@@ -1,17 +1,13 @@
 #pragma once
 
-#include <functional>
-#include <optional>
-
-#include <rj_geometry/point.hpp>
-#include <rrt/Tree.hpp>
-
-#include "path_target_path_planner.hpp"
+#include <optional>                      // for optional, nullopt
+#include <rj_geometry/point.hpp>         // for Point
+#include <string>                        // for allocator, string
+#include "planning/planning_params.hpp"  // for PARAM_step_size
+#include "planning/trajectory.hpp"       // for Trajectory
 #include "planning/planner/path_planner.hpp"
-#include "planning/planner/plan_request.hpp"
-
-class Configuration;
-class ConfigDouble;
+namespace planning { struct PlanRequest; }
+namespace rj_geometry { class ShapeSet; }
 
 namespace planning {
 /**