Planner mutex lock (#2144)

Add mutex locks to planner Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: petergarud <[email protected]> Co-authored-by: Sid Parikh <[email protected]> Co-authored-by: p-nayak11 <[email protected]>
RoboJackets · Nov 14, 2023 · d8ebe43 · d8ebe43
1 parent ba45730
commit d8ebe43
Show file tree

Hide file tree

Showing 4 changed files with 97 additions and 49 deletions.
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -96,9 +96,6 @@ jobs:
             # check formatting style (C++)
             git diff -U0 --no-color ${{ github.base_ref }} ${{ github.head_ref }} | python3 util/clang-format-diff.py -binary clang-format-10 -i -p1
 
-            # for Python files
-            git diff -U0 --no-color ${{ github.base_ref }} ${{ github.head_ref }} | black . --preview
-
             if ! git diff-index --quiet HEAD; then
               echo "::set-output name=changed::true"
             else

diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt
@@ -24,6 +24,7 @@ set(ROBOCUP_LIB_SRC
     optimization/parallel_gradient_ascent_1d.cpp
     optimization/nelder_mead_2d.cpp
     optimization/python_function_wrapper.cpp
+    planning/global_state.cpp
     planning/primitives/angle_planning.cpp
     planning/primitives/create_path.cpp
     planning/primitives/path_smoothing.cpp

diff --git a/soccer/src/soccer/planning/global_state.cpp b/soccer/src/soccer/planning/global_state.cpp
@@ -0,0 +1,79 @@
+#include "planning/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),
+        [this](rj_msgs::msg::PlayState::SharedPtr state) {  // NOLINT
+            auto lock = std::lock_guard(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),
+        [this](rj_msgs::msg::GameSettings::SharedPtr settings) {  // NOLINT
+            auto lock = std::lock_guard(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),
+        [this](rj_msgs::msg::Goalie::SharedPtr goalie) {  // NOLINT
+            auto lock = std::lock_guard(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),
+        [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr global_obstacles) {  // NOLINT
+            auto lock = std::lock_guard(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),
+        [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr def_area_obstacles) {  // NOLINT
+            auto lock = std::lock_guard(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),
+        [this](rj_msgs::msg::WorldState::SharedPtr world_state) {  // NOLINT
+            auto lock = std::lock_guard(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),
+        [this](rj_msgs::msg::CoachState::SharedPtr coach_state) {  // NOLINT
+            auto lock = std::lock_guard(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]] const WorldState* GlobalState::world_state() const {
+    auto lock = std::lock_guard(last_world_state_mutex_);
+    return &last_world_state_;
+}
+[[nodiscard]] const rj_msgs::msg::CoachState GlobalState::coach_state() const {
+    auto lock = std::lock_guard(last_coach_state_mutex_);
+    return last_coach_state_;
+}
+
+}  // namespace planning
diff --git a/soccer/src/soccer/planning/global_state.hpp b/soccer/src/soccer/planning/global_state.hpp
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <mutex>
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <rj_constants/topic_names.hpp>
@@ -24,53 +26,15 @@ namespace planning {
  */
 class GlobalState {
 public:
-    GlobalState(rclcpp::Node* node) {
-        play_state_sub_ = node->create_subscription<rj_msgs::msg::PlayState>(
-            referee::topics::kPlayStateTopic, rclcpp::QoS(1),
-            [this](rj_msgs::msg::PlayState::SharedPtr state) {  // NOLINT
-                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),
-            [this](rj_msgs::msg::GameSettings::SharedPtr settings) {  // NOLINT
-                last_game_settings_ = rj_convert::convert_from_ros(*settings);
-            });
-        goalie_sub_ = node->create_subscription<rj_msgs::msg::Goalie>(
-            referee::topics::kGoalieTopic, rclcpp::QoS(1),
-            [this](rj_msgs::msg::Goalie::SharedPtr goalie) {  // NOLINT
-                last_goalie_id_ = goalie->goalie_id;
-            });
-        global_obstacles_sub_ = node->create_subscription<rj_geometry_msgs::msg::ShapeSet>(
-            planning::topics::kGlobalObstaclesTopic, rclcpp::QoS(1),
-            [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr global_obstacles) {  // NOLINT
-                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),
-            [this](rj_geometry_msgs::msg::ShapeSet::SharedPtr def_area_obstacles) {  // NOLINT
-                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),
-            [this](rj_msgs::msg::WorldState::SharedPtr world_state) {  // NOLINT
-                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),
-            [this](rj_msgs::msg::CoachState::SharedPtr coach_state) {  // NOLINT
-                last_coach_state_ = *coach_state;
-            });
-    }
+    GlobalState(rclcpp::Node* node);
 
-    [[nodiscard]] PlayState play_state() const { return last_play_state_; }
-    [[nodiscard]] GameSettings game_settings() const { return last_game_settings_; }
-    [[nodiscard]] int goalie_id() const { return last_goalie_id_; }
-    [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const { return last_global_obstacles_; }
-    [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const {
-        return last_def_area_obstacles_;
-    }
-    [[nodiscard]] const WorldState* world_state() const { return &last_world_state_; }
-    [[nodiscard]] const rj_msgs::msg::CoachState coach_state() const { return last_coach_state_; }
+    [[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]] const WorldState* world_state() const;
+    [[nodiscard]] const rj_msgs::msg::CoachState coach_state() const;
 
 private:
     rclcpp::Subscription<rj_msgs::msg::PlayState>::SharedPtr play_state_sub_;
@@ -82,12 +46,19 @@ class GlobalState {
     rclcpp::Subscription<rj_msgs::msg::CoachState>::SharedPtr coach_state_sub_;
 
     PlayState last_play_state_ = PlayState::halt();
+    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