diff --git a/crane_bringup/launch/play_switcher.launch.py b/crane_bringup/launch/play_switcher.launch.py index 6a56fe3f9..b8b906ddc 100755 --- a/crane_bringup/launch/play_switcher.launch.py +++ b/crane_bringup/launch/play_switcher.launch.py @@ -176,6 +176,10 @@ def generate_launch_description(): # output="screen" ) + receive = Node(package="crane_planner_plugins", executable="receive_node", + # output="screen" + ) + play_switcher = Node( package="crane_play_switcher", executable="play_switcher_node", @@ -226,6 +230,7 @@ def generate_launch_description(): kickoff, marker, attacker, + receive, world_model_publisher, play_switcher, visualizer, diff --git a/crane_bringup/launch/play_switcher_y.launch.py b/crane_bringup/launch/play_switcher_y.launch.py index 91fcc7868..a2c8b8dbd 100755 --- a/crane_bringup/launch/play_switcher_y.launch.py +++ b/crane_bringup/launch/play_switcher_y.launch.py @@ -178,6 +178,10 @@ def generate_launch_description(): # output="screen" ) + receive = Node(package="crane_planner_plugins", executable="receive_node", + # output="screen" + ) + play_switcher = Node( package="crane_play_switcher", executable="play_switcher_node", output="screen" ) @@ -226,6 +230,7 @@ def generate_launch_description(): goalie, kickoff, attacker, + receive, world_model_publisher, play_switcher, visualizer, diff --git a/session/crane_planner_plugins/CMakeLists.txt b/session/crane_planner_plugins/CMakeLists.txt index 520e379b2..bdd3f362e 100755 --- a/session/crane_planner_plugins/CMakeLists.txt +++ b/session/crane_planner_plugins/CMakeLists.txt @@ -44,10 +44,11 @@ add_planner_plugin( CLASS_NAME "crane::BallPlacementPlanner" ) -#add_planner_plugin( -# NAME "receive" -# CLASS_NAME "crane::ReceivePlanner" -#) +add_planner_plugin( + NAME "receive" + CLASS_NAME "crane::ReceivePlanner" +) + ament_auto_add_executable(receive_test_node src/receive/test.cpp) ament_target_dependencies(receive_test_node "Python3") diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp index 2668935d4..15b66d926 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp @@ -13,7 +13,9 @@ #include "crane_game_analyzer/evaluations/evaluations.hpp" #include "crane_geometry/eigen_adapter.hpp" +#include "crane_geometry/interval.hpp" #include "crane_msg_wrappers/world_model_wrapper.hpp" +#include "crane_msg_wrappers/robot_command_wrapper.hpp" #include "crane_msgs/msg/pass_info.hpp" #include "crane_msgs/msg/receiver_plan.hpp" #include "crane_msgs/msg/world_model.hpp" @@ -27,8 +29,12 @@ namespace crane * ReceivePlannerは現在進行形でパスされているボールに対して, * ボールを受けるロボット(passer),その次にボールを受けるロボット(receiver)を指定するだけで * 最適なパス地点を計算し,その2台に対する指令を生成するプランナーです + * + * 何もないとき:パスの通る隙の多い地点に陣取る + * ボールが向かってきているとき:最適なパス地点を計算し,その地点に向かう + * */ -class ReceivePlanner : public rclcpp::Node +class ReceivePlanner : public rclcpp::Node, public PlannerBase { public: enum class ReceivePhase { @@ -53,8 +59,9 @@ class ReceivePlanner : public rclcpp::Node COMPOSITION_PUBLIC explicit ReceivePlanner(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : rclcpp::Node("receive_planner", options) + : rclcpp::Node("receive_planner", options), PlannerBase("receive", *this) { + RCLCPP_INFO(get_logger(), "initializing"); pass_info_pub = create_publisher("path_info", 1); using namespace std::placeholders; pass_req_service = create_service( @@ -64,6 +71,58 @@ class ReceivePlanner : public rclcpp::Node [this](void) -> void { pass_info.world_model = world_model->getMsg(); }); } + std::vector calculateControlTarget( + const std::vector & robots) override +{ + auto dpps_points = getDPPSPoints(world_model->ball.pos, 0.5, 8); + + Point best_position; + double best_score = 0.0; + for (const auto & dpps_point : dpps_points) { + Segment line{world_model->ball.pos, dpps_point}; + double closest_distance = [&]() -> double{ + double closest_distance = std::numeric_limits::max(); + for (const auto & robot : world_model->theirs.getAvailableRobots()) { + ClosestPoint result; + bg::closest_point(robot->pose.pos, line, result); + if (result.distance < closest_distance) { + closest_distance = result.distance; + } + } + return closest_distance; + }(); + + if(closest_distance < 0.4) { + continue; + } + + const double score = getLargestGoalAngleWidthFromPosition(dpps_point); + if (score > best_score) { + best_score = score; + best_position = dpps_point; + } + } + + std::vector commands; + for(const auto & robot: robots){ + crane::RobotCommandWrapper target(robot.robot_id, world_model); + target.setTargetPosition(best_position); + target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); + commands.push_back(target.getMsg()); + } + + return commands; +} +auto getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots) + -> std::vector override +{ + return this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, [this](const std::shared_ptr & robot) { + return 100. / world_model->getSquareDistanceFromRobotToBall({true, robot->id}); + }); +} + void passRequestHandle( const std::shared_ptr request_header, const std::shared_ptr request, @@ -146,6 +205,29 @@ class ReceivePlanner : public rclcpp::Node return ret; } + + auto getLargestGoalAngleWidthFromPosition(const Point point) -> double + { + Interval goal_range; + + auto goal_posts = world_model->getTheirGoalPosts(); + goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point)); + + for (auto & enemy : world_model->theirs.robots) { + double distance = (point - enemy->pose.pos).norm(); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = getAngle(enemy->pose.pos - point); + double diff_angle = + atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS)); + + goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); + } + + auto largest_interval = goal_range.getLargestInterval(); + return largest_interval.second - largest_interval.first; + } + std::vector> getPositionsWithScore(Segment ball_line, Point next_target) { auto points = getPoints(ball_line, 0.05); @@ -181,6 +263,21 @@ class ReceivePlanner : public rclcpp::Node return points; } + std::vector getDPPSPoints(Point center, double r_resolution, int theta_div_num){ + std::vector points; + for (int theta_index = 0; theta_index < theta_div_num; theta_index++){ + double theta = 2.0 * M_PI * theta_index / theta_div_num; + for (double r = r_resolution; r <= 10.0; r += r_resolution){ + points.emplace_back(Point(center.x() + r * cos(theta), center.y() + r * sin(theta))); + } + } + points.erase(std::remove_if(points.begin(), points.end(), [&](const auto & point){ + return not world_model->isFieldInside(point); + }), points.end()); + + return points; + } + double getPointScore(Point p, Point next_target) { double nearest_dist; diff --git a/session/crane_planner_plugins/src/receive/receive_node.cpp b/session/crane_planner_plugins/src/receive/receive_node.cpp index 2c9dab150..1a000aac9 100755 --- a/session/crane_planner_plugins/src/receive/receive_node.cpp +++ b/session/crane_planner_plugins/src/receive/receive_node.cpp @@ -6,7 +6,7 @@ #include -#include "crane_planner{plugins/receive_planner.hpp" +#include "crane_planner_plugins/receive_planner.hpp" int main(int argc, char * argv[]) { diff --git a/session/crane_planner_plugins/src/receive/test.cpp b/session/crane_planner_plugins/src/receive/test.cpp index 36ffc0edc..7b415b94d 100644 --- a/session/crane_planner_plugins/src/receive/test.cpp +++ b/session/crane_planner_plugins/src/receive/test.cpp @@ -14,8 +14,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - crane::ReceivePlanner receive_planner(options); + crane::ReceivePlanner receive_planner; receive_planner.session_info.receiver_id = 2; auto world_model = std::make_shared(); // ball diff --git a/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml b/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml index 223bf0c77..826770650 100644 --- a/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml +++ b/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml @@ -6,6 +6,8 @@ sessions: capacity: 1 - name: "attacker" capacity: 1 + - name: "receive" + capacity: 1 - name: "defender" capacity: 4 - name: "waiter" diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index 72739f778..1ad865189 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -25,6 +25,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions session_planners["kickoff"] = std::make_shared("kickoff"); session_planners["attacker"] = std::make_shared("attacker"); session_planners["marker"] = std::make_shared("marker"); + session_planners["receive"] = std::make_shared("receive"); for (auto & planner : session_planners) { planner.second->construct(*this); }