ローカルプランナの実装を切替可能にする

.github/workflows/build_test.yaml

@@ -4,6 +4,13 @@ name: build test
 on:
   workflow_dispatch:
   pull_request:
+    paths:
+      - '.github/workflows/build-test.yml'
+      - '**/**.cpp'
+      - '**/**.h'
+      - '**/**.hpp'
+      - '**/CMakeLists.txt'
+      - '**/package.xml'
   merge_group:
 
 

crane_bringup/launch/play_switcher.launch.py

@@ -111,7 +111,7 @@ def generate_launch_description():
         on_exit=ShutdownOnce(),
         parameters=[
             {
-                "enable_rvo": False,
+                "planner": "simple",
                 # "non_rvo_gain": 2.15,
                 "non_rvo_p_gain": 2.9,
                 "non_rvo_d_gain": 1.0,

crane_bringup/launch/play_switcher_y.launch.py

@@ -110,7 +110,7 @@ def generate_launch_description():
         on_exit=ShutdownOnce(),
         parameters=[
             {
-                "enable_rvo": False,
+                "planner": "simple",
                 "non_rvo_gain": 2.15,
             }
         ],

crane_local_planner/include/crane_local_planner/gridmap_planner.hpp

@@ -0,0 +1,136 @@
+// Copyright (c) 2023 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_GRIDMAP_PLANNER_HPP
+#define CRANE_GRIDMAP_PLANNER_HPP
+
+#include <crane_msg_wrappers/world_model_wrapper.hpp>
+#include <crane_msgs/msg/robot_commands.hpp>
+#include <grid_map_ros/grid_map_ros.hpp>
+
+namespace crane
+{
+class GridMapPlanner
+{
+public:
+  GridMapPlanner(rclcpp::Node & node) : map({"penalty", "ball_placement", "theirs", "ours", "ball"})
+  {
+    node.declare_parameter("map_resolution", MAP_RESOLUTION);
+    MAP_RESOLUTION = node.get_parameter("map_resolution").as_double();
+
+    gridmap_publisher =
+      node.create_publisher<grid_map_msgs::msg::GridMap>("local_planner/grid_map", 1);
+    map.setFrameId("map");
+    map.setGeometry(grid_map::Length(1.2, 2.0), MAP_RESOLUTION, grid_map::Position(0.0, 0.0));
+  }
+
+  crane_msgs::msg::RobotCommands calculateControlTarget(
+    const crane_msgs::msg::RobotCommands &, WorldModelWrapper::SharedPtr world_model)
+  {
+    // update map size
+
+    if (
+      map.getLength().x() != world_model->field_size.x() ||
+      map.getLength().y() != world_model->field_size.y()) {
+      map.clearAll();
+      map.setGeometry(
+        grid_map::Length(world_model->field_size.x(), world_model->field_size.y()), MAP_RESOLUTION);
+    }
+
+    // DefenseSize更新時にdefense_areaを更新する
+    static Vector2 defense_area_size;
+    if (
+      defense_area_size.x() != world_model->defense_area_size.x() ||
+      defense_area_size.y() != world_model->defense_area_size.y()) {
+      defense_area_size = world_model->defense_area_size;
+      if (not map.exists("defense_area")) {
+        map.add("defense_area");
+      }
+
+      for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
+        grid_map::Position position;
+        map.getPosition(*iterator, position);
+        map.at("defense_area", *iterator) = world_model->isDefenseArea(position);
+      }
+    }
+
+    // ボールプレイスメントMap
+    if (not map.exists("ball_placement")) {
+      map.add("ball_placement");
+    }
+    for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
+      grid_map::Position position;
+      map.getPosition(*iterator, position);
+      map.at("ball_placement", *iterator) = world_model->isBallPlacementArea(position);
+    }
+
+    // 味方ロボットMap
+    if (not map.exists("friend_robot")) {
+      map.add("friend_robot");
+    }
+    map["friend_robot"].setConstant(0.0);
+    for (const auto & robot : world_model->ours.getAvailableRobots()) {
+      for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd();
+           ++iterator) {
+        map.at("friend_robot", *iterator) = 1.0;
+      }
+    }
+
+    // 敵ロボットMap
+    if (not map.exists("enemy_robot")) {
+      map.add("enemy_robot");
+    }
+    map["enemy_robot"].setConstant(0.0);
+    for (const auto & robot : world_model->theirs.getAvailableRobots()) {
+      for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd();
+           ++iterator) {
+        map.at("enemy_robot", *iterator) = 1.0;
+      }
+    }
+
+    // ボールMap
+    if (not map.exists("ball")) {
+      map.add("ball");
+    }
+    map["ball"].setConstant(0.0);
+    for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.1); !iterator.isPastEnd();
+         ++iterator) {
+      map.at("ball", *iterator) = 1.0;
+    }
+
+    // ボールMap (時間)
+    if (not map.exists("ball_time")) {
+      map.add("ball_time");
+    }
+    Vector2 ball_vel_unit = world_model->ball.vel.normalized() * MAP_RESOLUTION;
+    Point ball_pos = world_model->ball.pos;
+    double time = 0.0;
+    const double TIME_STEP = MAP_RESOLUTION / world_model->ball.vel.norm();
+    map["ball_time"].setConstant(100.0);
+    for (int i = 0; i < 100; ++i) {
+      for (grid_map::CircleIterator iterator(map, ball_pos, 0.05); !iterator.isPastEnd();
+           ++iterator) {
+        map.at("ball_time", *iterator) = std::min(map.at("ball_time", *iterator), (float)time);
+      }
+      ball_pos += ball_vel_unit;
+      time += TIME_STEP;
+    }
+    //      map.setTimestamp(now().nanoseconds());
+    std::unique_ptr<grid_map_msgs::msg::GridMap> message;
+    message = grid_map::GridMapRosConverter::toMessage(map);
+
+    gridmap_publisher->publish(std::move(message));
+  }
+
+private:
+  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr gridmap_publisher;
+
+  grid_map::GridMap map;
+
+  double MAP_RESOLUTION = 0.05;
+};
+}  // namespace crane
+#endif  //CRANE_GRIDMAP_PLANNER_HPP

crane_local_planner/include/crane_local_planner/local_planner.hpp

@@ -7,15 +7,17 @@
 #ifndef CRANE_LOCAL_PLANNER__LOCAL_PLANNER_HPP_
 #define CRANE_LOCAL_PLANNER__LOCAL_PLANNER_HPP_
 
-#include <crane_geometry/pid_controller.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <crane_msgs/msg/world_model.hpp>
 #include <functional>
+#include <grid_map_ros/grid_map_ros.hpp>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 
-#include "RVO.h"
+#include "gridmap_planner.hpp"
+#include "rvo_planner.hpp"
+#include "simple_planner.hpp"
 #include "visibility_control.h"
 
 namespace crane
@@ -34,107 +36,55 @@ class LocalPlannerComponent : public rclcpp::Node
   explicit LocalPlannerComponent(const rclcpp::NodeOptions & options)
   : rclcpp::Node("local_planner", options)
   {
-    declare_parameter("enable_rvo", true);
-    enable_rvo = get_parameter("enable_rvo").as_bool();
-
-    declare_parameter("rvo_time_step", RVO_TIME_STEP);
-    RVO_TIME_STEP = get_parameter("rvo_time_step").as_double();
-    declare_parameter("rvo_neighbor_dist", RVO_NEIGHBOR_DIST);
-    RVO_NEIGHBOR_DIST = get_parameter("rvo_neighbor_dist").as_double();
-    declare_parameter("rvo_max_neighbors", RVO_MAX_NEIGHBORS);
-    RVO_MAX_NEIGHBORS = get_parameter("rvo_max_neighbors").as_int();
-    declare_parameter("rvo_time_horizon", RVO_TIME_HORIZON);
-    RVO_TIME_HORIZON = get_parameter("rvo_time_horizon").as_double();
-    // cspell: ignore OBST
-    declare_parameter("rvo_time_horizon_obst", RVO_TIME_HORIZON_OBST);
-    RVO_TIME_HORIZON_OBST = get_parameter("rvo_time_horizon_obst").as_double();
-    declare_parameter("rvo_radius", RVO_RADIUS);
-    RVO_RADIUS = get_parameter("rvo_radius").as_double();
-    declare_parameter("rvo_max_speed", RVO_MAX_SPEED);
-    RVO_MAX_SPEED = get_parameter("rvo_max_speed").as_double();
-    declare_parameter("rvo_trapezoidal_max_acc", RVO_TRAPEZOIDAL_MAX_ACC);
-    RVO_TRAPEZOIDAL_MAX_ACC = get_parameter("rvo_trapezoidal_max_acc").as_double();
-    declare_parameter("rvo_trapezoidal_frame_rate", RVO_TRAPEZOIDAL_FRAME_RATE);
-    RVO_TRAPEZOIDAL_FRAME_RATE = get_parameter("rvo_trapezoidal_frame_rate").as_double();
-    declare_parameter("rvo_trapezoidal_max_speed", RVO_TRAPEZOIDAL_MAX_SPEED);
-    RVO_TRAPEZOIDAL_MAX_SPEED = get_parameter("rvo_trapezoidal_max_speed").as_double();
-    declare_parameter("non_rvo_max_vel", NON_RVO_MAX_VEL);
-    NON_RVO_MAX_VEL = get_parameter("non_rvo_max_vel").as_double();
-
-    declare_parameter("non_rvo_p_gain", NON_RVO_P_GAIN);
-    NON_RVO_P_GAIN = get_parameter("non_rvo_p_gain").as_double();
-    declare_parameter("non_rvo_i_gain", NON_RVO_I_GAIN);
-    NON_RVO_I_GAIN = get_parameter("non_rvo_i_gain").as_double();
-    declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN);
-    NON_RVO_D_GAIN = get_parameter("non_rvo_d_gain").as_double();
-
-    for (auto & controller : vx_controllers) {
-      controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN);
+    declare_parameter("planner", "simple");
+    auto planner_str = get_parameter("planner").as_string();
+
+    if (planner_str == "simple") {
+      simple_planner = std::make_shared<SimplePlanner>(*this);
+      calculate_control_target =
+        [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands {
+        return simple_planner->calculateControlTarget(msg, world_model);
+      };
+    } else if (planner_str == "rvo") {
+      rvo_planner = std::make_shared<RVOPlanner>(*this);
+      calculate_control_target =
+        [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands {
+        return rvo_planner->calculateControlTarget(msg, world_model);
+      };
+    } else if (planner_str == "gridmap") {
+      gridmap_planner = std::make_shared<GridMapPlanner>(*this);
+      calculate_control_target =
+        [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands {
+        return gridmap_planner->calculateControlTarget(msg, world_model);
+      };
+    } else {
+      RCLCPP_ERROR(get_logger(), "Unknown planner: %s", planner_str.c_str());
+      throw std::runtime_error("Unknown planner: " + planner_str);
     }
 
-    for (auto & controller : vy_controllers) {
-      controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN);
-    }
-
-    rvo_sim = std::make_unique<RVO::RVOSimulator>(
-      RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST,
-      RVO_RADIUS, RVO_MAX_SPEED);
-
     world_model = std::make_shared<WorldModelWrapper>(*this);
 
-    // TODO(HansRobo): add goal area as obstacles
-
-    // TODO(HansRobo): add external area as obstacles
-    // friend robots -> 0~19
-    // enemy robots -> 20~39
-    // ball 40
-    for (int i = 0; i < 41; i++) {
-      rvo_sim->addAgent(RVO::Vector2(20.0f, 20.0f));
-    }
-
     commands_pub = this->create_publisher<crane_msgs::msg::RobotCommands>("/robot_commands", 10);
     control_targets_sub = this->create_subscription<crane_msgs::msg::RobotCommands>(
       "/control_targets", 10,
       std::bind(&LocalPlannerComponent::callbackControlTarget, this, std::placeholders::_1));
   }
 
-  void reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands &);
-
-  crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim(
-    const crane_msgs::msg::RobotCommands &);
-
   void callbackControlTarget(const crane_msgs::msg::RobotCommands &);
 
 private:
   rclcpp::Subscription<crane_msgs::msg::RobotCommands>::SharedPtr control_targets_sub;
 
   rclcpp::Publisher<crane_msgs::msg::RobotCommands>::SharedPtr commands_pub;
 
-  std::unique_ptr<RVO::RVOSimulator> rvo_sim;
-
   WorldModelWrapper::SharedPtr world_model;
 
-  bool enable_rvo;
-
-  std::array<PIDController, 20> vx_controllers;
-  std::array<PIDController, 20> vy_controllers;
-
-  float RVO_TIME_STEP = 1.0 / 60.0f;
-  float RVO_NEIGHBOR_DIST = 2.0f;
-  int RVO_MAX_NEIGHBORS = 5;
-  float RVO_TIME_HORIZON = 1.f;
-  float RVO_TIME_HORIZON_OBST = 1.f;
-  float RVO_RADIUS = 0.09f;
-  float RVO_MAX_SPEED = 10.0f;
-
-  float RVO_TRAPEZOIDAL_MAX_ACC = 8.0;
-  float RVO_TRAPEZOIDAL_FRAME_RATE = 60;
-  float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0;
+  std::function<crane_msgs::msg::RobotCommands(const crane_msgs::msg::RobotCommands &)>
+    calculate_control_target;
 
-  double NON_RVO_MAX_VEL = 4.0;
-  double NON_RVO_P_GAIN = 4.0;
-  double NON_RVO_I_GAIN = 0.0;
-  double NON_RVO_D_GAIN = 0.0;
+  std::shared_ptr<SimplePlanner> simple_planner = nullptr;
+  std::shared_ptr<RVOPlanner> rvo_planner = nullptr;
+  std::shared_ptr<GridMapPlanner> gridmap_planner = nullptr;
 };
 
 }  // namespace crane