diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml index 00c619cbc..13104b570 100644 --- a/.github/workflows/build_test.yaml +++ b/.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: diff --git a/crane_bringup/launch/play_switcher.launch.py b/crane_bringup/launch/play_switcher.launch.py index 470361b45..bdbd8827c 100755 --- a/crane_bringup/launch/play_switcher.launch.py +++ b/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, diff --git a/crane_bringup/launch/play_switcher_y.launch.py b/crane_bringup/launch/play_switcher_y.launch.py index 86bc585bb..d2eb82089 100755 --- a/crane_bringup/launch/play_switcher_y.launch.py +++ b/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, } ], diff --git a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp new file mode 100644 index 000000000..1fbff5545 --- /dev/null +++ b/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 +#include +#include + +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("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 message; + message = grid_map::GridMapRosConverter::toMessage(map); + + gridmap_publisher->publish(std::move(message)); + } + +private: + rclcpp::Publisher::SharedPtr gridmap_publisher; + + grid_map::GridMap map; + + double MAP_RESOLUTION = 0.05; +}; +} // namespace crane +#endif //CRANE_GRIDMAP_PLANNER_HPP diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index d0f749df0..1202903b9 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/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 #include #include #include #include +#include #include #include -#include "RVO.h" +#include "gridmap_planner.hpp" +#include "rvo_planner.hpp" +#include "simple_planner.hpp" #include "visibility_control.h" namespace crane @@ -34,75 +36,40 @@ 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(*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(*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(*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_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(*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("/robot_commands", 10); control_targets_sub = this->create_subscription( "/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: @@ -110,31 +77,14 @@ class LocalPlannerComponent : public rclcpp::Node rclcpp::Publisher::SharedPtr commands_pub; - std::unique_ptr rvo_sim; - WorldModelWrapper::SharedPtr world_model; - bool enable_rvo; - - std::array vx_controllers; - std::array 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 + 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 simple_planner = nullptr; + std::shared_ptr rvo_planner = nullptr; + std::shared_ptr gridmap_planner = nullptr; }; } // namespace crane diff --git a/crane_local_planner/include/crane_local_planner/rvo_planner.hpp b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp new file mode 100644 index 000000000..d8421e21f --- /dev/null +++ b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp @@ -0,0 +1,244 @@ +// Copyright (c) 2022 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_RVO_PLANNER_HPP +#define CRANE_RVO_PLANNER_HPP + +#include +#include + +#include "RVO.h" + +// cspell: ignore OBST +namespace crane +{ +class RVOPlanner +{ +public: + RVOPlanner(rclcpp::Node & node) + { + node.declare_parameter("rvo_time_step", RVO_TIME_STEP); + RVO_TIME_STEP = node.get_parameter("rvo_time_step").as_double(); + node.declare_parameter("rvo_neighbor_dist", RVO_NEIGHBOR_DIST); + RVO_NEIGHBOR_DIST = node.get_parameter("rvo_neighbor_dist").as_double(); + node.declare_parameter("rvo_max_neighbors", RVO_MAX_NEIGHBORS); + RVO_MAX_NEIGHBORS = node.get_parameter("rvo_max_neighbors").as_int(); + node.declare_parameter("rvo_time_horizon", RVO_TIME_HORIZON); + RVO_TIME_HORIZON = node.get_parameter("rvo_time_horizon").as_double(); + node.declare_parameter("rvo_time_horizon_obst", RVO_TIME_HORIZON_OBST); + RVO_TIME_HORIZON_OBST = node.get_parameter("rvo_time_horizon_obst").as_double(); + node.declare_parameter("rvo_radius", RVO_RADIUS); + RVO_RADIUS = node.get_parameter("rvo_radius").as_double(); + node.declare_parameter("rvo_max_speed", RVO_MAX_SPEED); + RVO_MAX_SPEED = node.get_parameter("rvo_max_speed").as_double(); + node.declare_parameter("rvo_trapezoidal_max_acc", RVO_TRAPEZOIDAL_MAX_ACC); + RVO_TRAPEZOIDAL_MAX_ACC = node.get_parameter("rvo_trapezoidal_max_acc").as_double(); + node.declare_parameter("rvo_trapezoidal_frame_rate", RVO_TRAPEZOIDAL_FRAME_RATE); + RVO_TRAPEZOIDAL_FRAME_RATE = node.get_parameter("rvo_trapezoidal_frame_rate").as_double(); + node.declare_parameter("rvo_trapezoidal_max_speed", RVO_TRAPEZOIDAL_MAX_SPEED); + RVO_TRAPEZOIDAL_MAX_SPEED = node.get_parameter("rvo_trapezoidal_max_speed").as_double(); + + rvo_sim = std::make_unique( + RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, + RVO_RADIUS, RVO_MAX_SPEED); + + // 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)); + } + } + + void reflectWorldToRVOSim( + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) + { + bool add_ball = true; + // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新 + for (const auto & command : msg.robot_commands) { + rvo_sim->setAgentPosition( + command.robot_id, RVO::Vector2(command.current_pose.x, command.current_pose.y)); + rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(0.f, 0.f)); + + auto robot = world_model->getOurRobot(command.robot_id); + if (robot->available && command.local_planner_config.disable_collision_avoidance) { + add_ball = false; + } + + float target_x = command.target_x.front(); + float target_y = command.target_y.front(); + + if (command.motion_mode_enable) { + // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する + rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(target_x, target_y)); + } else { + // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する + const auto pos = robot->pose.pos; + auto diff_pos = Point(target_x - pos.x(), target_y - pos.y()); + + // 台形加速制御 + // 2ax = v^2 - v0^2 + // v^2 - 2ax = v0^2 + // v0 = sqrt(v^2 - 2ax) + double max_speed_for_stop = + std::sqrt(0 * 0 - 2.0 * (-RVO_TRAPEZOIDAL_MAX_ACC) * diff_pos.norm()); + double max_speed_for_acc = + robot->vel.linear.norm() + RVO_TRAPEZOIDAL_MAX_ACC / RVO_TRAPEZOIDAL_FRAME_RATE; + + double target_speed = + std::min({max_speed_for_acc, max_speed_for_stop, (double)RVO_TRAPEZOIDAL_MAX_SPEED}); + Velocity vel; + double dist = diff_pos.norm(); + vel << target_speed / dist * diff_pos.x(), target_speed / dist * diff_pos.y(); + + if (command.robot_id == 3) { + std::cout << "current_robot: " << int(command.robot_id) << std::endl; + std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; + std::cout << "to: " << target_x << ", " << target_y << std::endl; + std::cout << "diff_pos: " << diff_pos.x() << ", " << diff_pos.y() << std::endl; + std::cout << "target_speed: " << target_speed << std::endl; + std::cout << "target_vel: " << std::fixed << std::setprecision(5) << vel.x() << ", " + << vel.y() << std::endl; + } + rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(vel.x(), vel.y())); + } + } + + if (add_ball) { + rvo_sim->setAgentPosition( + 40, RVO::Vector2(world_model->ball.pos.x(), world_model->ball.pos.y())); + rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); + } else { + rvo_sim->setAgentPosition(40, RVO::Vector2(20.0f, 20.0f)); + rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); + } + + // for (const auto & friend_robot : world_model->ours.robots) { + // if (not friend_robot->available) { + // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(20.f, 20.f + friend_robot->id)); + // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(0.f, 0.f)); + // + // } else { + // // Visionからのロボット位置の更新 + // const auto & pos = friend_robot->pose.pos; + // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(pos.x(), pos.y())); + // + // auto robot_target = std::find_if( + // msg.robot_commands.begin(), msg.robot_commands.end(), + // [&](const auto & x) { return x.robot_id == friend_robot->id; }); + // + // if (robot_target == msg.robot_commands.end()) { + // // ロボットがcontrol_targetsに含まれていない場合、 + // // 観測された速度をpreferred velocityとして設定する + // rvo_sim->setAgentPrefVelocity( + // friend_robot->id, + // RVO::Vector2(friend_robot->vel.linear.x(), friend_robot->vel.linear.y())); + // } else { + // float target_x = robot_target->target_x.front(); + // float target_y = robot_target->target_y.front(); + // if (robot_target->motion_mode_enable) { + // // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する + // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(target_x, target_y)); + // } else { + // // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する + // auto diff_pos = Point(target_x, target_y) - pos; + // + // // 台形加速制御 + // // TODO : 外部からパラメータを設定できるようにする + // constexpr double MAX_ACC = 10.0; + // constexpr double FRAME_RATE = 300; + // constexpr double MAX_SPEED = 10.0; + // std::cout << "current_robot: " << int(robot_target->robot_id) << std::endl; + // std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; + // std::cout << "to: " << target_x << ", " << target_y << std::endl; + // // 2ax = v^2 - v0^2 + // // v^2 - 2ax = v0^2 + // // v0 = sqrt(v^2 - 2ax) + // double max_speed_for_stop = std::sqrt(0 * 0 - 2.0 * (-MAX_ACC) * diff_pos.norm()); + // double max_speed_for_acc = friend_robot->vel.linear.norm() + MAX_ACC / FRAME_RATE; + // + // double target_speed = -std::min({max_speed_for_acc, max_speed_for_stop, MAX_SPEED}); + // auto vel = diff_pos * target_speed / diff_pos.norm(); + // std::cout << "target_vel: " << vel.x() << ", " << vel.y() << std::endl; + // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(vel.x(), vel.y())); + // } + // } + // } + // } + + for (const auto & enemy_robot : world_model->theirs.robots) { + if (enemy_robot->available) { + const auto & pos = enemy_robot->pose.pos; + const auto & vel = enemy_robot->vel.linear; + rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(pos.x(), pos.y())); + rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(vel.x(), vel.y())); + } else { + rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(20.f, 20.f)); + rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(0.f, 0.f)); + } + } + } + + crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim( + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) + { + crane_msgs::msg::RobotCommands commands = msg; + for (size_t i = 0; i < msg.robot_commands.size(); i++) { + const auto & original_command = msg.robot_commands.at(i); + const auto & robot = world_model->getOurRobot(original_command.robot_id); + crane_msgs::msg::RobotCommand command = original_command; + command.current_pose.x = robot->pose.pos.x(); + command.current_pose.y = robot->pose.pos.y(); + command.current_pose.theta = robot->pose.theta; + // RVOシミュレータの出力をコピーする + // NOTE: RVOシミュレータは角度を扱わないので角度はそのまま + + // RVOシミュレータの出力は速度なので，速度制御モードにする + command.motion_mode_enable = true; + auto vel = rvo_sim->getAgentVelocity(original_command.robot_id); + if (original_command.robot_id == 3) { + // std::cout << "robot_id " << int(original_command.robot_id) << std::endl; + // std::cout << "vel : " << vel.x() << " " << vel.y() << std::endl; + } + command.target_velocity.x = vel.x(); + command.target_velocity.y = vel.y(); + command.target_x.clear(); + command.target_y.clear(); + + commands.robot_commands.at(i) = command; + } + return commands; + } + + crane_msgs::msg::RobotCommands calculateControlTarget( + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) + { + reflectWorldToRVOSim(msg, world_model); + // RVOシミュレータ更新 + rvo_sim->doStep(); + return extractRobotCommandsFromRVOSim(msg, world_model); + } + +private: + std::unique_ptr rvo_sim; + + 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; +}; +} // namespace crane +#endif //CRANE_RVO_PLANNER_HPP diff --git a/crane_local_planner/include/crane_local_planner/simple_planner.hpp b/crane_local_planner/include/crane_local_planner/simple_planner.hpp new file mode 100644 index 000000000..b0a71d80e --- /dev/null +++ b/crane_local_planner/include/crane_local_planner/simple_planner.hpp @@ -0,0 +1,125 @@ +// 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_SIMPLE_PLANNER_HPP +#define CRANE_SIMPLE_PLANNER_HPP + +#include +#include +#include + +namespace crane +{ +class SimplePlanner +{ +public: + SimplePlanner(rclcpp::Node & node) + { + node.declare_parameter("non_rvo_max_vel", NON_RVO_MAX_VEL); + NON_RVO_MAX_VEL = node.get_parameter("non_rvo_max_vel").as_double(); + + node.declare_parameter("non_rvo_p_gain", NON_RVO_P_GAIN); + NON_RVO_P_GAIN = node.get_parameter("non_rvo_p_gain").as_double(); + node.declare_parameter("non_rvo_i_gain", NON_RVO_I_GAIN); + NON_RVO_I_GAIN = node.get_parameter("non_rvo_i_gain").as_double(); + node.declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN); + NON_RVO_D_GAIN = node.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); + } + + for (auto & controller : vy_controllers) { + controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); + } + } + + crane_msgs::msg::RobotCommands calculateControlTarget( + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) + { + crane_msgs::msg::RobotCommands commands = msg; + for (auto & command : commands.robot_commands) { + if ((not command.target_x.empty()) && (not command.target_y.empty())) { + auto robot = world_model->getOurRobot(command.robot_id); + Point target; + target << command.target_x.front(), command.target_y.front(); + + Point robot_to_target = target - robot->pose.pos; + + // double dot1 = (world_model->ball.pos - robot->pose.pos).dot(robot_to_target); + // double dot2 = (-robot_to_target).dot(world_model->ball.pos - target); + // if (dot1 > 0 && dot2 > 0) { + // Point norm_vec; + // norm_vec << robot_to_target.y(), -robot_to_target.x(); + // norm_vec = norm_vec.normalized(); + // double dist_to_line = std::abs(norm_vec.dot(world_model->ball.pos - robot->pose.pos)); + // if (dist_to_line < 0.1) { + // Point p1, p2; + // Point ball_est = world_model->ball.pos + world_model->ball.vel * 4.0; + // p1 = ball_est + 0.2 * norm_vec; + // p2 = ball_est - 0.2 * norm_vec; + // double d1 = (robot->pose.pos - p1).squaredNorm(); + // double d2 = (robot->pose.pos - p2).squaredNorm(); + // target = (d1 < d2) ? p1 : p2; + // } + // command.target_x.front() = target.x(); + // command.target_y.front() = target.y(); + // } + + double max_vel = command.local_planner_config.max_velocity > 0 + ? command.local_planner_config.max_velocity + : NON_RVO_MAX_VEL; + // double max_acc = command.local_planner_config.max_acceleration > 0? command.local_planner_config.max_acceleration : NON_RVO_GAIN; + double max_omega = command.local_planner_config.max_omega > 0 + ? command.local_planner_config.max_omega + : 600.0 * M_PI / 180; + + // 速度に変換する + Velocity vel; + vel << vx_controllers[command.robot_id].update( + command.target_x.front() - command.current_pose.x, 1.f / 30.f), + vy_controllers[command.robot_id].update( + command.target_y.front() - command.current_pose.y, 1.f / 30.f); + vel += vel.normalized() * command.local_planner_config.terminal_velocity; + if (vel.norm() > max_vel) { + vel = vel.normalized() * max_vel; + } + + command.target_velocity.x = vel.x(); + command.target_velocity.y = vel.y(); + + //　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする + // そしてこの過ちを再びおかさぬようここに残しておく． R.I.P. + // double MAX_THETA_DIFF = max_omega / 30.0f; + // // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する + // if (not command.target_theta.empty()) { + // double theta_diff = + // getAngleDiff(command.target_theta.front(), command.current_pose.theta); + // if (std::fabs(theta_diff) > MAX_THETA_DIFF) { + // theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff); + // } + // + // command.target_theta.front() = command.current_pose.theta + theta_diff; + // } + + command.current_ball_x = world_model->ball.pos.x(); + command.current_ball_y = world_model->ball.pos.y(); + } + } + return commands; + } + +private: + std::array vx_controllers; + std::array vy_controllers; + + 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; +}; +} // namespace crane +#endif //CRANE_SIMPLE_PLANNER_HPP diff --git a/crane_local_planner/package.xml b/crane_local_planner/package.xml index a7e5bfaea..7287eca9a 100755 --- a/crane_local_planner/package.xml +++ b/crane_local_planner/package.xml @@ -16,6 +16,8 @@ rclcpp_components rvo2_vendor std_msgs + grid_map_core + grid_map_ros ament_lint_auto crane_lint_common diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index 9cb1b432e..b60a340ea 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -8,251 +8,12 @@ namespace crane { -void LocalPlannerComponent::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg) -{ - bool add_ball = true; - // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新 - for (const auto & command : msg.robot_commands) { - rvo_sim->setAgentPosition( - command.robot_id, RVO::Vector2(command.current_pose.x, command.current_pose.y)); - rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(0.f, 0.f)); - - auto robot = world_model->getOurRobot(command.robot_id); - if (robot->available && command.local_planner_config.disable_collision_avoidance) { - add_ball = false; - } - - float target_x = command.target_x.front(); - float target_y = command.target_y.front(); - - if (command.motion_mode_enable) { - // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する - rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(target_x, target_y)); - } else { - // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する - const auto pos = robot->pose.pos; - auto diff_pos = Point(target_x - pos.x(), target_y - pos.y()); - - // 台形加速制御 - // 2ax = v^2 - v0^2 - // v^2 - 2ax = v0^2 - // v0 = sqrt(v^2 - 2ax) - double max_speed_for_stop = - std::sqrt(0 * 0 - 2.0 * (-RVO_TRAPEZOIDAL_MAX_ACC) * diff_pos.norm()); - double max_speed_for_acc = - robot->vel.linear.norm() + RVO_TRAPEZOIDAL_MAX_ACC / RVO_TRAPEZOIDAL_FRAME_RATE; - - double target_speed = - std::min({max_speed_for_acc, max_speed_for_stop, (double)RVO_TRAPEZOIDAL_MAX_SPEED}); - Velocity vel; - double dist = diff_pos.norm(); - vel << target_speed / dist * diff_pos.x(), target_speed / dist * diff_pos.y(); - - if (command.robot_id == 3) { - std::cout << "current_robot: " << int(command.robot_id) << std::endl; - std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; - std::cout << "to: " << target_x << ", " << target_y << std::endl; - std::cout << "diff_pos: " << diff_pos.x() << ", " << diff_pos.y() << std::endl; - std::cout << "target_speed: " << target_speed << std::endl; - std::cout << "target_vel: " << std::fixed << std::setprecision(5) << vel.x() << ", " - << vel.y() << std::endl; - } - rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(vel.x(), vel.y())); - } - } - - if (add_ball) { - rvo_sim->setAgentPosition( - 40, RVO::Vector2(world_model->ball.pos.x(), world_model->ball.pos.y())); - rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); - } else { - rvo_sim->setAgentPosition(40, RVO::Vector2(20.0f, 20.0f)); - rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); - } - - // for (const auto & friend_robot : world_model->ours.robots) { - // if (not friend_robot->available) { - // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(20.f, 20.f + friend_robot->id)); - // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(0.f, 0.f)); - // - // } else { - // // Visionからのロボット位置の更新 - // const auto & pos = friend_robot->pose.pos; - // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(pos.x(), pos.y())); - // - // auto robot_target = std::find_if( - // msg.robot_commands.begin(), msg.robot_commands.end(), - // [&](const auto & x) { return x.robot_id == friend_robot->id; }); - // - // if (robot_target == msg.robot_commands.end()) { - // // ロボットがcontrol_targetsに含まれていない場合、 - // // 観測された速度をpreferred velocityとして設定する - // rvo_sim->setAgentPrefVelocity( - // friend_robot->id, - // RVO::Vector2(friend_robot->vel.linear.x(), friend_robot->vel.linear.y())); - // } else { - // float target_x = robot_target->target_x.front(); - // float target_y = robot_target->target_y.front(); - // if (robot_target->motion_mode_enable) { - // // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する - // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(target_x, target_y)); - // } else { - // // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する - // auto diff_pos = Point(target_x, target_y) - pos; - // - // // 台形加速制御 - // // TODO : 外部からパラメータを設定できるようにする - // constexpr double MAX_ACC = 10.0; - // constexpr double FRAME_RATE = 300; - // constexpr double MAX_SPEED = 10.0; - // std::cout << "current_robot: " << int(robot_target->robot_id) << std::end; - // std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; - // std::cout << "to: " << target_x << ", " << target_y << std::endl; - // // 2ax = v^2 - v0^2 - // // v^2 - 2ax = v0^2 - // // v0 = sqrt(v^2 - 2ax) - // double max_speed_for_stop = std::sqrt(0 * 0 - 2.0 * (-MAX_ACC) * diff_pos.norm()); - // double max_speed_for_acc = friend_robot->vel.linear.norm() + MAX_ACC / FRAME_RATE; - // - // double target_speed = -std::min({max_speed_for_acc, max_speed_for_stop, MAX_SPEED}); - // auto vel = diff_pos * target_speed / diff_pos.norm(); - // std::cout << "target_vel: " << vel.x() << ", " << vel.y() << std::endl; - // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(vel.x(), vel.y())); - // } - // } - // } - // } - - for (const auto & enemy_robot : world_model->theirs.robots) { - if (enemy_robot->available) { - const auto & pos = enemy_robot->pose.pos; - const auto & vel = enemy_robot->vel.linear; - rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(pos.x(), pos.y())); - rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(vel.x(), vel.y())); - } else { - rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(20.f, 20.f)); - rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(0.f, 0.f)); - } - } -} - -// RVOシミュレータの結果をコマンドにコピー -crane_msgs::msg::RobotCommands LocalPlannerComponent::extractRobotCommandsFromRVOSim( - const crane_msgs::msg::RobotCommands & msg) -{ - crane_msgs::msg::RobotCommands commands = msg; - for (size_t i = 0; i < msg.robot_commands.size(); i++) { - const auto & original_command = msg.robot_commands.at(i); - const auto & robot = world_model->getOurRobot(original_command.robot_id); - crane_msgs::msg::RobotCommand command = original_command; - command.current_pose.x = robot->pose.pos.x(); - command.current_pose.y = robot->pose.pos.y(); - command.current_pose.theta = robot->pose.theta; - // RVOシミュレータの出力をコピーする - // NOTE: RVOシミュレータは角度を扱わないので角度はそのまま - - // RVOシミュレータの出力は速度なので，速度制御モードにする - command.motion_mode_enable = true; - auto vel = rvo_sim->getAgentVelocity(original_command.robot_id); - if (original_command.robot_id == 3) { - // std::cout << "robot_id " << int(original_command.robot_id) << std::endl; - // std::cout << "vel : " << vel.x() << " " << vel.y() << std::endl; - } - command.target_velocity.x = vel.x(); - command.target_velocity.y = vel.y(); - command.target_x.clear(); - command.target_y.clear(); - - commands.robot_commands.at(i) = command; - } - return commands; -} - void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCommands & msg) { if (!world_model->hasUpdated()) { return; } - - if (enable_rvo) { - reflectWorldToRVOSim(msg); - - // RVOシミュレータ更新 - rvo_sim->doStep(); - - commands_pub->publish(extractRobotCommandsFromRVOSim(msg)); - } else { - crane_msgs::msg::RobotCommands commands = msg; - for (auto & command : commands.robot_commands) { - if ((not command.target_x.empty()) && (not command.target_y.empty())) { - auto robot = world_model->getOurRobot(command.robot_id); - Point target; - target << command.target_x.front(), command.target_y.front(); - - Point robot_to_target = target - robot->pose.pos; - - // double dot1 = (world_model->ball.pos - robot->pose.pos).dot(robot_to_target); - // double dot2 = (-robot_to_target).dot(world_model->ball.pos - target); - // if (dot1 > 0 && dot2 > 0) { - // Point norm_vec; - // norm_vec << robot_to_target.y(), -robot_to_target.x(); - // norm_vec = norm_vec.normalized(); - // double dist_to_line = std::abs(norm_vec.dot(world_model->ball.pos - robot->pose.pos)); - // if (dist_to_line < 0.1) { - // Point p1, p2; - // Point ball_est = world_model->ball.pos + world_model->ball.vel * 4.0; - // p1 = ball_est + 0.2 * norm_vec; - // p2 = ball_est - 0.2 * norm_vec; - // double d1 = (robot->pose.pos - p1).squaredNorm(); - // double d2 = (robot->pose.pos - p2).squaredNorm(); - // target = (d1 < d2) ? p1 : p2; - // } - // command.target_x.front() = target.x(); - // command.target_y.front() = target.y(); - // } - - double max_vel = command.local_planner_config.max_velocity > 0 - ? command.local_planner_config.max_velocity - : NON_RVO_MAX_VEL; - // double max_acc = command.local_planner_config.max_acceleration > 0? command.local_planner_config.max_acceleration : NON_RVO_GAIN; - double max_omega = command.local_planner_config.max_omega > 0 - ? command.local_planner_config.max_omega - : 600.0 * M_PI / 180; - - // 速度に変換する - Velocity vel; - vel << vx_controllers[command.robot_id].update( - command.target_x.front() - command.current_pose.x, 1.f / 30.f), - vy_controllers[command.robot_id].update( - command.target_y.front() - command.current_pose.y, 1.f / 30.f); - vel += vel.normalized() * command.local_planner_config.terminal_velocity; - if (vel.norm() > max_vel) { - vel = vel.normalized() * max_vel; - } - - command.target_velocity.x = vel.x(); - command.target_velocity.y = vel.y(); - - //　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする - // そしてこの過ちを再びおかさぬようここに残しておく． R.I.P. - // double MAX_THETA_DIFF = max_omega / 30.0f; - // // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する - // if (not command.target_theta.empty()) { - // double theta_diff = - // getAngleDiff(command.target_theta.front(), command.current_pose.theta); - // if (std::fabs(theta_diff) > MAX_THETA_DIFF) { - // theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff); - // } - // - // command.target_theta.front() = command.current_pose.theta + theta_diff; - // } - - command.current_ball_x = world_model->ball.pos.x(); - command.current_ball_y = world_model->ball.pos.y(); - } - } - commands_pub->publish(commands); - } + commands_pub->publish(calculate_control_target(msg)); } } // namespace crane diff --git a/session/crane_planner_base/include/crane_planner_base/planner_base.hpp b/session/crane_planner_base/include/crane_planner_base/planner_base.hpp index 540441ea0..5139592aa 100644 --- a/session/crane_planner_base/include/crane_planner_base/planner_base.hpp +++ b/session/crane_planner_base/include/crane_planner_base/planner_base.hpp @@ -21,25 +21,45 @@ namespace crane class PlannerBase { public: - PlannerBase() {} + using SharedPtr = std::shared_ptr; - virtual void construct(WorldModelWrapper::SharedPtr world_model) = 0; + using UniquePtr = std::unique_ptr; - void construct(const std::string name, WorldModelWrapper::SharedPtr world_model) + explicit PlannerBase(const std::string name, WorldModelWrapper::SharedPtr & world_model) + : name(name), world_model(world_model) { - this->name = name; - this->world_model = world_model; - world_model->addCallback([&](void) -> void { - if (robots.empty()) { - return; - } - auto control_targets = calculateControlTarget(robots); - crane_msgs::msg::RobotCommands msg; - msg.is_yellow = world_model->isYellow(); - for (auto target : control_targets) { - msg.robot_commands.emplace_back(target); - } - }); + RCLCPP_INFO(rclcpp::get_logger(name), "PlannerBase::PlannerBase"); + } + + crane_msgs::srv::RobotSelect::Response doRobotSelect( + const crane_msgs::srv::RobotSelect::Request::SharedPtr request, + const WorldModelWrapper::SharedPtr & world_model) + { + crane_msgs::srv::RobotSelect::Response response; + response.selected_robots = + getSelectedRobots(request->selectable_robots_num, request->selectable_robots); + + robots.clear(); + for (auto id : response.selected_robots) { + RobotIdentifier robot_id{true, id}; + robots.emplace_back(robot_id); + } + + for (auto && callback : robot_select_callbacks) { + callback(); + } + return response; + } + + auto getControlTargets() -> crane_msgs::msg::RobotCommands + { + auto robot_command_wrappers = calculateControlTarget(robots); + crane_msgs::msg::RobotCommands msg; + msg.is_yellow = world_model->isYellow(); + for (auto command : robot_command_wrappers) { + msg.robot_commands.emplace_back(command); + } + return msg; } void addRobotSelectCallback(std::function f) @@ -47,47 +67,45 @@ class PlannerBase robot_select_callbacks.emplace_back(f); } - std::optional> assign( - std::vector selectable_robots, const int selectable_robots_num) +protected: + virtual auto getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots) + -> std::vector = 0; + + auto getSelectedRobotsByScore( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + std::function &)> score_func) -> std::vector { std::vector> robot_with_score; - for (auto id : selectable_robots) { - robot_with_score.emplace_back(id, getRoleScore(world_model->getRobot({true, id}))); + for (const auto & id : selectable_robots) { + robot_with_score.emplace_back(id, score_func(world_model->getOurRobot(id))); } std::sort( std::begin(robot_with_score), std::end(robot_with_score), [](const auto & a, const auto & b) -> bool { - // greater score forst + // greater score first return a.second > b.second; }); - robots.clear(); + std::vector selected_robots; for (int i = 0; i < selectable_robots_num; i++) { if (i >= robot_with_score.size()) { break; } - robots.push_back({true, robot_with_score.at(i).first}); - } - - for (auto && callback : robot_select_callbacks) { - callback(); + selected_robots.emplace_back(robot_with_score.at(i).first); } - - return robots; + return selected_robots; } -protected: - std::string name; - - virtual double getRoleScore(std::shared_ptr robot) = 0; + const std::string name; std::vector robots; + WorldModelWrapper::SharedPtr world_model; + virtual std::vector calculateControlTarget( const std::vector & robots) = 0; - WorldModelWrapper::SharedPtr world_model = nullptr; - private: std::vector> robot_select_callbacks; }; diff --git a/session/crane_planner_plugins/CMakeLists.txt b/session/crane_planner_plugins/CMakeLists.txt index e528233a6..b25c695bf 100755 --- a/session/crane_planner_plugins/CMakeLists.txt +++ b/session/crane_planner_plugins/CMakeLists.txt @@ -29,13 +29,12 @@ function(add_planner_plugin) message("plugin name = ${PLUGIN_NAME}") message("plugin class name = ${PLUGIN_CLASS_NAME}") - ament_auto_add_library(${PLUGIN_NAME} SHARED src/${PLUGIN_NAME}/${PLUGIN_NAME}.cpp) - rclcpp_components_register_nodes(${PLUGIN_NAME} ${PLUGIN_CLASS_NAME}) -# ament_auto_add_executable(${PLUGIN_NAME}_node src/${PLUGIN_NAME}/${PLUGIN_NAME}_node.cpp) - install(TARGETS ${PLUGIN_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION lib/${PROJECT_NAME}) + # ament_auto_add_library(${PLUGIN_NAME} SHARED src/${PLUGIN_NAME}/${PLUGIN_NAME}.cpp) + # rclcpp_components_register_nodes(${PLUGIN_NAME} ${PLUGIN_CLASS_NAME}) + # install(TARGETS ${PLUGIN_NAME} + # ARCHIVE DESTINATION lib/${PROJECT_NAME} + # LIBRARY DESTINATION lib/${PROJECT_NAME} + # RUNTIME DESTINATION lib/${PROJECT_NAME}) endfunction() add_planner_plugin( @@ -43,10 +42,15 @@ add_planner_plugin( CLASS_NAME "crane::BallPlacementPlanner" ) -#add_planner_plugin( -# NAME "receive" -# CLASS_NAME "crane::ReceivePlanner" -#) +add_planner_plugin( + NAME "ball_placement_with_skill" + CLASS_NAME "crane::BallPlacementWithSkillPlanner" +) + +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") @@ -97,7 +101,7 @@ add_planner_plugin( CLASS_NAME "crane::TemplatePlanner" ) -pluginlib_export_plugin_description_file(crane_planner_base plugins.xml) +#pluginlib_export_plugin_description_file(crane_planner_base plugins.xml) if (BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp index 2860c9792..cbd8b3a87 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp @@ -7,90 +7,82 @@ #ifndef CRANE_PLANNER_PLUGINS__ATTACKER_PLANNER_HPP_ #define CRANE_PLANNER_PLUGINS__ATTACKER_PLANNER_HPP_ +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include "crane_geometry/boost_geometry.hpp" -#include "crane_msg_wrappers/world_model_wrapper.hpp" -#include "crane_msgs/msg/control_target.hpp" -#include "crane_msgs/srv/robot_select.hpp" -#include "crane_planner_base/planner_base.hpp" -#include "crane_planner_plugins/visibility_control.h" +#include "visibility_control.h" namespace crane { class AttackerPlanner : public PlannerBase { public: - void construct(WorldModelWrapper::SharedPtr world_model) override + COMPOSITION_PUBLIC + explicit AttackerPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("attacker", world_model) { - PlannerBase::construct("attacker", world_model); } + std::vector calculateControlTarget( const std::vector & robots) override { std::vector control_targets; for (auto robot_id : robots) { - crane_msgs::msg::RobotCommand target; + crane::RobotCommandWrapper target(robot_id.robot_id, world_model); auto robot = world_model->getRobot(robot_id); - target.current_pose.x = robot->pose.pos.x(); - target.current_pose.y = robot->pose.pos.y(); - target.current_pose.theta = robot->pose.theta; - - target.robot_id = robot_id.robot_id; - target.chip_enable = false; - target.dribble_power = 0.0; - target.kick_power = 0.0; - target.motion_mode_enable = false; - - auto set_target = [&](auto & target_array, auto value) { - if (not target_array.empty()) { - target_array.front() = value; - } else { - target_array.emplace_back(value); - } - }; + + auto [best_target, goal_angle_width] = getBestShootTargetWithWidth(); + + // シュートの隙がないときは仲間へパス + if (goal_angle_width < 0.07) { + auto our_robots = world_model->ours.getAvailableRobots(); + our_robots.erase( + std::remove_if( + our_robots.begin(), our_robots.end(), + [&](const auto & robot) { return robot->id == robot_id.robot_id; }), + our_robots.end()); + auto nearest_robot = + world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots); + best_target = nearest_robot.first->pose.pos; + } // 経由ポイント Point intermediate_point = - world_model->ball.pos + - (world_model->ball.pos - world_model->getTheirGoalCenter()).normalized() * 0.2; - + world_model->ball.pos + (world_model->ball.pos - best_target).normalized() * 0.2; double dot = (robot->pose.pos - world_model->ball.pos) .normalized() - .dot((world_model->ball.pos - world_model->getTheirGoalCenter()).normalized()); - double target_theta = getAngle(world_model->getTheirGoalCenter() - world_model->ball.pos); + .dot((world_model->ball.pos - best_target).normalized()); + double target_theta = getAngle(best_target - world_model->ball.pos); // ボールと敵ゴールの延長線上にいない && 角度があってないときは，中間ポイントを経由 if (dot < 0.95 || std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.05) { - set_target(target.target_x, intermediate_point.x()); - set_target(target.target_y, intermediate_point.y()); - target.local_planner_config.disable_collision_avoidance = false; + target.setTargetPosition(intermediate_point); + target.enableCollisionAvoidance(); } else { - set_target(target.target_x, world_model->ball.pos.x()); - set_target(target.target_y, world_model->ball.pos.y()); - target.dribble_power = 0.5; - target.kick_power = 0.8; - target.chip_enable = false; - target.local_planner_config.disable_collision_avoidance = true; + target.setTargetPosition(world_model->ball.pos); + target.kickStraight(0.7).disableCollisionAvoidance(); + target.enableCollisionAvoidance(); } - set_target( - target.target_theta, getAngle(world_model->getTheirGoalCenter() - world_model->ball.pos)); + target.setTargetTheta(getAngle(best_target - world_model->ball.pos)); - bool is_in_defense = world_model->isEnemyDefenseArea(world_model->ball.pos); + bool is_in_defense = world_model->isDefenseArea(world_model->ball.pos); bool is_in_field = world_model->isFieldInside(world_model->ball.pos); - if (not is_in_field) { + if ((not is_in_field) or is_in_defense) { // stop here - target.motion_mode_enable = false; - set_target(target.target_x, world_model->goal.x() / 2.); - set_target(target.target_y, world_model->goal.y() / 2.); - set_target(target.target_theta, getAngle(-world_model->goal)); + target.stopHere(); } - control_targets.emplace_back(target); + control_targets.emplace_back(target.getMsg()); } return control_targets; } @@ -100,13 +92,42 @@ class AttackerPlanner : public PlannerBase 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.0 / std::max(world_model->getSquareDistanceFromRobotToBall({true, robot->id}), 0.01); - }); + return this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, [this](const std::shared_ptr & robot) { + // ボールに近いほどスコアが高い + return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); + }); + } + + auto getBestShootTargetWithWidth() -> std::pair + { + const auto & ball = world_model->ball.pos; + + Interval goal_range; + + auto goal_posts = world_model->getTheirGoalPosts(); + goal_range.append(getAngle(goal_posts.first - ball), getAngle(goal_posts.second - ball)); + + for (auto & enemy : world_model->theirs.robots) { + double distance = (ball - enemy->pose.pos).norm(); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = getAngle(enemy->pose.pos - ball); + 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(); + std::cout << "interval width: " << largest_interval.second - largest_interval.first + << std::endl; + double target_angle = (largest_interval.first + largest_interval.second) / 2.0; + + return { + ball + getNormVec(target_angle) * 0.5, largest_interval.second - largest_interval.first}; } }; } // namespace crane - #endif // CRANE_PLANNER_PLUGINS__ATTACKER_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_planner.hpp index f75427040..bb079be13 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_planner.hpp @@ -34,9 +34,10 @@ enum class BallPlacementState { class BallPlacementPlanner : public PlannerBase { public: - void construct(WorldModelWrapper::SharedPtr world_model) override + COMPOSITION_PUBLIC + explicit BallPlacementPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("ball_placement", world_model) { - PlannerBase::construct("ball_placement", world_model); addRobotSelectCallback([&]() { state = BallPlacementState::START; }); } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp index b556c7329..f37b3aec8 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp @@ -25,10 +25,12 @@ namespace crane class DefenderPlanner : public PlannerBase { public: - void construct(WorldModelWrapper::SharedPtr world_model) override + COMPOSITION_PUBLIC + explicit DefenderPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("defender", world_model) { - PlannerBase::construct("defender", world_model); } + std::vector calculateControlTarget( const std::vector & robots) override { @@ -234,5 +236,4 @@ class DefenderPlanner : public PlannerBase }; } // namespace crane - #endif // CRANE_PLANNER_PLUGINS__DEFENDER_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index 736be0f7b..6cfa2253a 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -7,25 +7,27 @@ #ifndef CRANE_PLANNER_PLUGINS__GOALIE_PLANNER_HPP_ #define CRANE_PLANNER_PLUGINS__GOALIE_PLANNER_HPP_ +#include +#include +#include +#include +#include +#include #include #include #include -#include "crane_geometry/boost_geometry.hpp" -#include "crane_msg_wrappers/world_model_wrapper.hpp" -#include "crane_msgs/msg/control_target.hpp" -#include "crane_msgs/srv/robot_select.hpp" -#include "crane_planner_base/planner_base.hpp" -#include "crane_planner_plugins/visibility_control.h" +#include "visibility_control.h" namespace crane { class GoaliePlanner : public PlannerBase { public: - void construct(WorldModelWrapper::SharedPtr world_model) override + COMPOSITION_PUBLIC + explicit GoaliePlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("goalie", world_model) { - PlannerBase::construct("goalie", world_model); } std::vector calculateControlTarget( @@ -33,17 +35,8 @@ class GoaliePlanner : public PlannerBase { std::vector control_targets; for (auto robot_id : robots) { - crane_msgs::msg::RobotCommand target; + crane::RobotCommandWrapper target(robot_id.robot_id, world_model); auto robot = world_model->getRobot(robot_id); - target.current_pose.x = robot->pose.pos.x(); - target.current_pose.y = robot->pose.pos.y(); - target.current_pose.theta = robot->pose.theta; - // Stop at same position - target.robot_id = robot_id.robot_id; - target.chip_enable = false; - target.dribble_power = 0.0; - target.kick_power = 0.0; - // control by velocity auto ball = world_model->ball.pos; @@ -61,42 +54,43 @@ class GoaliePlanner : public PlannerBase Point target_point; float target_theta; if (not intersections.empty()) { - std::cout << "Shoot block mode" << std::endl; + // std::cout << "Shoot block mode" << std::endl; ClosestPoint result; bg::closest_point(ball_line, robot->pose.pos, result); target_point << result.closest_point.x(), result.closest_point.y(); // position control - target.motion_mode_enable = false; target_theta = getAngle(-world_model->ball.vel); } else { // go blocking point - std::cout << "Normal blocking mode" << std::endl; - const double BLOCK_DIST = 0.3; - target.motion_mode_enable = false; + // std::cout << "Normal blocking mode" << std::endl; + const double BLOCK_DIST = 0.15; // 範囲外のときは正面に構える - if(not world_model->isFieldInside(world_model->ball.pos)){ - ball << 0,0; + if (not world_model->isFieldInside(world_model->ball.pos)) { + ball << 0, 0; } target_point = goal_center + (ball - goal_center).normalized() * BLOCK_DIST; target_theta = getAngle(ball - target_point); } - setTarget(target.target_x, target_point.x()); - setTarget(target.target_y, target_point.y()); - setTarget(target.target_theta, target_theta); - control_targets.emplace_back(target); + target.setTargetPosition(target_point, target_theta); + + control_targets.emplace_back(target.getMsg()); } return control_targets; } - double getRoleScore(std::shared_ptr robot) override + + auto getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots) + -> std::vector override { - // TODO : 決まったIDを割り当てる - // choose id smaller first - return 15. - static_cast(-robot->id); + return this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, [this](const std::shared_ptr & robot) { + // choose id smaller first + return 15. - static_cast(-robot->id); + }); } }; } // namespace crane - #endif // CRANE_PLANNER_PLUGINS__GOALIE_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/kickoff_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/kickoff_planner.hpp index b99338c38..afce489a9 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/kickoff_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/kickoff_planner.hpp @@ -23,9 +23,10 @@ namespace crane class KickOffPlanner : public PlannerBase { public: - void construct(WorldModelWrapper::SharedPtr world_model) override + COMPOSITION_PUBLIC + explicit KickOffPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("kickoff", world_model) { - PlannerBase::construct("kickoff", world_model); } std::vector calculateControlTarget( @@ -61,5 +62,4 @@ class KickOffPlanner : public PlannerBase }; } // namespace crane - #endif // CRANE_PLANNER_PLUGINS__KICKOFF_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp index ca943b20e..3959b78e8 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/template_planner.hpp @@ -23,10 +23,12 @@ namespace crane class TemplatePlanner : public PlannerBase { public: - void construct(WorldModelWrapper::SharedPtr world_model) override + COMPOSITION_PUBLIC + explicit TemplatePlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("template", world_model) { - PlannerBase::construct("template", world_model); } + std::vector calculateControlTarget( const std::vector & robots) override { @@ -43,9 +45,9 @@ class TemplatePlanner : public PlannerBase // TODO: implement target.motion_mode_enable = false; - setTarget(target.target_x, 0.0); - setTarget(target.target_y, 0.0); - target.target_velocity.theta = 0.0; + // setTarget(target.target_x, 0.0); + // setTarget(target.target_y, 0.0); + // target.target_velocity.theta = 0.0; control_targets.emplace_back(target); } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp index 622a5a55d..fcba0e2f9 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp @@ -23,10 +23,12 @@ namespace crane class WaiterPlanner : public PlannerBase { public: - void construct(WorldModelWrapper::SharedPtr world_model) override + COMPOSITION_PUBLIC + explicit WaiterPlanner(WorldModelWrapper::SharedPtr & world_model) + : PlannerBase("waiter", world_model) { - PlannerBase::construct("waiter", world_model); } + std::vector calculateControlTarget( const std::vector & robots) override { @@ -57,5 +59,4 @@ class WaiterPlanner : public PlannerBase }; } // namespace crane - #endif // CRANE_PLANNER_PLUGINS__WAITER_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/src/attacker/attacker.cpp b/session/crane_planner_plugins/src/attacker/attacker.cpp deleted file mode 100644 index 4b715cdd5..000000000 --- a/session/crane_planner_plugins/src/attacker/attacker.cpp +++ /dev/null @@ -1,15 +0,0 @@ -// Copyright (c) 2022 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. - -#include "crane_planner_plugins/attacker_planner.hpp" - -namespace crane -{ -} - -#include - -PLUGINLIB_EXPORT_CLASS(crane::AttackerPlanner, crane::PlannerBase) diff --git a/session/crane_planner_plugins/src/ball_placement/ball_placement.cpp b/session/crane_planner_plugins/src/ball_placement/ball_placement.cpp deleted file mode 100644 index d42c288d7..000000000 --- a/session/crane_planner_plugins/src/ball_placement/ball_placement.cpp +++ /dev/null @@ -1,15 +0,0 @@ -// Copyright (c) 2021 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. - -#include "crane_planner_plugins/ball_placement_planner.hpp" - -namespace crane -{ -} - -#include - -PLUGINLIB_EXPORT_CLASS(crane::BallPlacementPlanner, crane::PlannerBase) diff --git a/session/crane_planner_plugins/src/defender/defender.cpp b/session/crane_planner_plugins/src/defender/defender.cpp deleted file mode 100644 index 05c81d6f1..000000000 --- a/session/crane_planner_plugins/src/defender/defender.cpp +++ /dev/null @@ -1,15 +0,0 @@ -// Copyright (c) 2021 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. - -#include "crane_planner_plugins/defender_planner.hpp" - -namespace crane -{ -} - -#include - -PLUGINLIB_EXPORT_CLASS(crane::DefenderPlanner, crane::PlannerBase) diff --git a/session/crane_planner_plugins/src/goalie/goalie.cpp b/session/crane_planner_plugins/src/goalie/goalie.cpp deleted file mode 100644 index 682686421..000000000 --- a/session/crane_planner_plugins/src/goalie/goalie.cpp +++ /dev/null @@ -1,14 +0,0 @@ -// Copyright (c) 2021 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. - -#include "crane_planner_plugins/goalie_planner.hpp" - -namespace crane -{ -} -#include - -PLUGINLIB_EXPORT_CLASS(crane::GoaliePlanner, crane::PlannerBase) diff --git a/session/crane_planner_plugins/src/kickoff/kickoff.cpp b/session/crane_planner_plugins/src/kickoff/kickoff.cpp deleted file mode 100644 index 272eaea91..000000000 --- a/session/crane_planner_plugins/src/kickoff/kickoff.cpp +++ /dev/null @@ -1,14 +0,0 @@ -// Copyright (c) 2021 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. - -#include "crane_planner_plugins/kickoff_planner.hpp" - -namespace crane -{ -} -#include - -PLUGINLIB_EXPORT_CLASS(crane::KickOffPlanner, crane::PlannerBase) diff --git a/session/crane_planner_plugins/src/receive/receive.cpp b/session/crane_planner_plugins/src/receive/receive.cpp deleted file mode 100644 index e023a5987..000000000 --- a/session/crane_planner_plugins/src/receive/receive.cpp +++ /dev/null @@ -1,15 +0,0 @@ -// Copyright (c) 2022 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. - -#include "crane_planner_plugins/receive_planner.hpp" - -namespace crane -{ -} - -#include - -PLUGINLIB_EXPORT_CLASS(crane::ReceivePlanner, crane::PlannerBase) diff --git a/session/crane_planner_plugins/src/template/template.cpp b/session/crane_planner_plugins/src/template/template.cpp deleted file mode 100644 index 15e1b4bd3..000000000 --- a/session/crane_planner_plugins/src/template/template.cpp +++ /dev/null @@ -1,14 +0,0 @@ -// Copyright (c) 2022 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. - -#include "crane_planner_plugins/template_planner.hpp" - -namespace crane -{ -} -#include - -PLUGINLIB_EXPORT_CLASS(crane::TemplatePlanner, crane::PlannerBase) diff --git a/session/crane_planner_plugins/src/waiter/waiter.cpp b/session/crane_planner_plugins/src/waiter/waiter.cpp deleted file mode 100644 index 0b16c190d..000000000 --- a/session/crane_planner_plugins/src/waiter/waiter.cpp +++ /dev/null @@ -1,15 +0,0 @@ -// Copyright (c) 2021 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. - -#include "crane_planner_plugins/waiter_planner.hpp" - -namespace crane -{ -} - -#include - -PLUGINLIB_EXPORT_CLASS(crane::WaiterPlanner, crane::PlannerBase) diff --git a/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml b/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml index b816b4ff7..0feb9012b 100644 --- a/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml +++ b/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml @@ -5,6 +5,6 @@ sessions: - name: "goalie" capacity: 1 - name: "attacker" - capacity: 3 + capacity: 1 - name: "waiter" capacity: 20 diff --git a/session/crane_session_controller/include/crane_session_controller/session_module.hpp b/session/crane_session_controller/include/crane_session_controller/session_module.hpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/session/crane_session_controller/package.xml b/session/crane_session_controller/package.xml index 122554f66..0c6d53713 100755 --- a/session/crane_session_controller/package.xml +++ b/session/crane_session_controller/package.xml @@ -17,8 +17,7 @@ rclcpp_components std_msgs yaml-cpp - crane_planner_base - crane_planner_plugins + libgoogle-glog-dev ament_cmake_gtest ament_lint_auto diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index af6e4d74e..897a3f1fc 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -7,32 +7,17 @@ #include #include +#include #include -#include "crane_msg_wrappers/play_situation_wrapper.hpp" #include "crane_session_controller/session_controller.hpp" namespace crane { SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions & options) -: rclcpp::Node("session_controller", options), planner_loader("crane_planner_base", "crane::PlannerBase") +: rclcpp::Node("session_controller", options), + robot_commands_pub(create_publisher("/control_targets", 1)) { - // example of adding planner - // session_planners["replace"] = std::make_shared("replace"); - session_planners["waiter"] = std::make_shared("waiter"); - session_planners["formation"] = std::make_shared("formation"); - session_planners["goalie"] = std::make_shared("goalie"); - session_planners["defender"] = std::make_shared("defender"); - 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"); - session_planners["ball_placement_with_skill"] = - std::make_shared("ball_placement_with_skill"); - for (auto & planner : session_planners) { - planner.second->construct(planner_loader, *this); - } - /* * 各セッションの設定の読み込み */ @@ -100,7 +85,9 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions play_situation_sub = create_subscription( "/play_situation", 1, [this](const crane_msgs::msg::PlaySituation & msg) { // TODO - PlaySituationWrapper play_situation; + if (not world_model_ready) { + return; + } play_situation.update(msg); auto it = event_map.find(play_situation.getSituationCommandText()); if (it != event_map.end()) { @@ -108,8 +95,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions get_logger(), "イベント「%s」に対応するセッション「%s」の設定に従ってロボットを割り当てます", it->first.c_str(), it->second.c_str()); - // TODO: 選択可能なロボットを引っ張ってくる - request(it->second, {1}); + request(it->second, world_model->ours.getAvailableRobotIds()); } else { RCLCPP_ERROR( get_logger(), "イベント「%s」に対応するセッションの設定が見つかりませんでした", @@ -117,33 +103,56 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions } }); + using namespace std::chrono_literals; + timer = create_wall_timer(100ms, [&]() { + auto it = event_map.find(play_situation.getSituationCommandText()); + if (it != event_map.end()) { + // request(it->second, world_model->ours.getAvailableRobotIds()); + } + }); + declare_parameter("initial_session", "HALT"); auto initial_session = get_parameter("initial_session").as_string(); - auto it = event_map.find(initial_session); - if (it != event_map.end()) { - RCLCPP_INFO( - get_logger(), - "初期イベント「%s」に対応するセッション「%s」の設定に従ってロボットを割り当てます", - it->first.c_str(), it->second.c_str()); - request(it->second, {0, 1, 2}); - } else { - RCLCPP_ERROR( - get_logger(), "初期イベント「%s」に対応するセッションの設定が見つかりませんでした", - initial_session.c_str()); - } - world_model = std::make_shared(*this); + + world_model->addCallback([this, initial_session]() { + if (not world_model_ready && not world_model->ours.getAvailableRobotIds().empty()) { + world_model_ready = true; + auto it = event_map.find(initial_session); + if (it != event_map.end()) { + RCLCPP_INFO( + get_logger(), + "初期イベント「%s」に対応するセッション「%s」の設定に従ってロボットを割り当てます", + it->first.c_str(), it->second.c_str()); + request(it->second, world_model->ours.getAvailableRobotIds()); + } else { + RCLCPP_ERROR( + get_logger(), "初期イベント「%s」に対応するセッションの設定が見つかりませんでした", + initial_session.c_str()); + } + } + }); + + world_model->addCallback([this]() { + crane_msgs::msg::RobotCommands msg; + msg.header = world_model->getMsg().header; + msg.is_yellow = world_model->isYellow(); + for (const auto & planner : available_planners) { + auto control_target = planner->getControlTargets(); + msg.robot_commands.insert( + msg.robot_commands.end(), control_target.robot_commands.begin(), + control_target.robot_commands.end()); + } + robot_commands_pub->publish(msg); + }); } void SessionControllerComponent::request( - std::string situation, std::vector selectable_robot_ids) + std::string situation, std::vector selectable_robot_ids) { RCLCPP_INFO( get_logger(), "「%s」というSituationに対してロボット割当を実行します", situation.c_str()); - for(auto planner : session_planners) { - planner.second->clear(); - } std::string ids_string; for (auto id : selectable_robot_ids) { ids_string += std::to_string(id) + " "; @@ -160,42 +169,36 @@ void SessionControllerComponent::request( return; } + available_planners.clear(); + // 優先順位が高いPlannerから順にロボットを割り当てる - for (auto p : map->second) {; + for (auto p : map->second) { + auto req = std::make_shared(); + req->selectable_robots_num = p.selectable_robot_num; + // 使用可能なロボットを詰め込む + for (auto id : selectable_robot_ids) { + req->selectable_robots.emplace_back(id); + } try { - auto planner = session_planners.find(p.session_name); - if (planner == session_planners.end()) { - RCLCPP_ERROR( - get_logger(), - "\t「%" - "s」というセッションに対してロボット割当リクエストが発行されましたが，プランナが見つかり" - "ませんでした（リクエスト発行元Situation：%s）", - p.session_name.c_str(), situation.c_str()); - break; - } - // plannerにロボット割り当てを依頼する - auto selected_robots = planner->second->assign(selectable_robot_ids, p.selectable_robot_num); - if (!selected_robots) { - RCLCPP_ERROR( - get_logger(), - "\t「%" - "s」というプランナかへロボット割当リクエストを発行しましたが，応答がありませんでした", - p.session_name.c_str()); - break; - } + auto response = [&p, &req, this]() { + auto planner = generatePlanner(p.session_name, world_model); + auto response = planner->doRobotSelect(req, world_model); + available_planners.emplace_back(std::move(planner)); + return response; + }(); // 割当依頼結果の反映 std::string ids_string; - for (auto id : *selected_robots) { - ids_string += std::to_string(id.robot_id) + " "; + for (auto id : response.selected_robots) { + ids_string += std::to_string(id) + " "; } RCLCPP_INFO( get_logger(), "\tセッション「%s」に以下のロボットを割り当てました : %s", p.session_name.c_str(), ids_string.c_str()); - for (auto selected_robot_id : *selected_robots) { + for (auto selected_robot_id : response.selected_robots) { // 割当されたロボットを利用可能ロボットリストから削除 selectable_robot_ids.erase( - remove(selectable_robot_ids.begin(), selectable_robot_ids.end(), selected_robot_id.robot_id), + remove(selectable_robot_ids.begin(), selectable_robot_ids.end(), selected_robot_id), selectable_robot_ids.end()); } } catch (std::exception & e) { diff --git a/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp b/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp index f4c0a6fe1..4313e10fc 100755 --- a/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp +++ b/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp @@ -17,11 +17,13 @@ #include #include +#include "eigen_adapter.hpp" + namespace bg = boost::geometry; -using Vector2 = Eigen::Vector2f; -using Point = Eigen::Vector2f; -using Velocity = Eigen::Vector2f; -using Accel = Eigen::Vector2f; +using Vector2 = Eigen::Vector2d; +using Point = Eigen::Vector2d; +using Velocity = Eigen::Vector2d; +using Accel = Eigen::Vector2d; using Segment = bg::model::segment; using Polygon = bg::model::polygon; using LineString = bg::model::linestring; @@ -31,17 +33,17 @@ using ClosestPoint = bg::closest_point_result; struct Circle { Point center; - float radius; + double radius; }; namespace boost::geometry { template -float distance(const Circle & circle, const Geometry1 & geometry1) +double distance(const Circle & circle, const Geometry1 & geometry1) { - float dist = distance(circle.center, geometry1) - circle.radius; + double dist = distance(circle.center, geometry1) - circle.radius; if (dist < 0) { - return 0.0f; + return 0.; } return dist; } @@ -50,13 +52,13 @@ float distance(const Circle & circle, const Geometry1 & geometry1) struct Pose2D { Point pos; - float theta; + double theta; }; struct Velocity2D { Point linear; - float omega; + double omega; }; struct Rect diff --git a/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp b/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp index 08d4b45fd..652734c36 100644 --- a/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp +++ b/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp @@ -7,46 +7,48 @@ #ifndef CRANE_GEOMETRY__EIGEN_ADAPTER_HPP_ #define CRANE_GEOMETRY__EIGEN_ADAPTER_HPP_ -#include "boost_geometry.hpp" +#include +#include namespace boost::geometry::traits { template <> -struct tag +struct tag { typedef point_tag type; }; + template <> -struct coordinate_type +struct coordinate_type { - typedef float type; + typedef double type; }; template <> -struct coordinate_system +struct coordinate_system { typedef cs::cartesian type; }; template <> -struct dimension : boost::mpl::int_<2> +struct dimension : boost::mpl::int_<2> { }; template <> -struct access +struct access { - static float get(Eigen::Vector2f const & p) { return p.x(); } + static double get(Eigen::Vector2d const & p) { return p.x(); } - static void set(Eigen::Vector2f & p, float const & value) { p.x() = value; } + static void set(Eigen::Vector2d & p, double const & value) { p.x() = value; } }; template <> -struct access +struct access { - static float get(Eigen::Vector2f const & p) { return p.y(); } + static double get(Eigen::Vector2d const & p) { return p.y(); } - static void set(Eigen::Vector2f & p, float const & value) { p.y() = value; } + static void set(Eigen::Vector2d & p, double const & value) { p.y() = value; } }; } // namespace boost::geometry::traits #endif // CRANE_GEOMETRY__EIGEN_ADAPTER_HPP_ diff --git a/utility/crane_geometry/include/crane_geometry/interval.hpp b/utility/crane_geometry/include/crane_geometry/interval.hpp index c473ae43d..53b10c903 100644 --- a/utility/crane_geometry/include/crane_geometry/interval.hpp +++ b/utility/crane_geometry/include/crane_geometry/interval.hpp @@ -13,18 +13,18 @@ class Interval { private: - std::vector uppers; - std::vector lowers; + std::vector uppers; + std::vector lowers; public: Interval() {} ~Interval() {} - void append(float a, float b) + void append(double a, double b) { - float upper = std::max(a, b); - float lower = std::min(a, b); + double upper = std::max(a, b); + double lower = std::min(a, b); uppers.emplace_back(upper); lowers.emplace_back(lower); @@ -41,10 +41,10 @@ class Interval } } - void erase(float a, float b) + void erase(double a, double b) { - float upper = std::max(a, b); - float lower = std::min(a, b); + double upper = std::max(a, b); + double lower = std::min(a, b); for (size_t i = 0; i < uppers.size(); i++) { //完全消去 if (uppers[i] < upper && lowers[i] > lower) { @@ -74,22 +74,22 @@ class Interval std::sort(lowers.begin(), lowers.end()); } - float getWidth() + double getWidth() { - float width = 0.f; + double width = 0.f; for (size_t i = 0; i < lowers.size(); i++) { width += uppers[i] - lowers[i]; } return width; } - auto getLargestInterval() -> std::pair + auto getLargestInterval() -> std::pair { - float max_width = 0.f; - float max_lower = 0.f; - float max_upper = 0.f; + double max_width = 0.f; + double max_lower = 0.f; + double max_upper = 0.f; for (size_t i = 0; i < lowers.size(); i++) { - float width = uppers[i] - lowers[i]; + double width = uppers[i] - lowers[i]; if (width > max_width) { max_width = width; max_lower = lowers[i]; diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp index 0c1ca44f3..ba99edade 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp @@ -14,7 +14,7 @@ struct FieldAnalysisWrapper { std::string name; float unit; - Eigen::Vector2f origin; + Eigen::Vector2d origin; int size_x; int size_y; diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp index fd3e33d72..9e96d2361 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp @@ -29,7 +29,7 @@ struct PlaySituationWrapper return situation_command.id == crane_msgs::msg::PlaySituation::INPLAY; } - Eigen::Vector2f placement_position; + Eigen::Vector2d placement_position; auto update(const crane_msgs::msg::PlaySituation & msg) -> void; diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 2e03cc56e..bd9f3796a 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -26,20 +26,42 @@ struct BallContact auto now = std::chrono::system_clock::now(); if (is_contacted) { last_contact_end_time = now; + if (not is_contacted_pre_frame) { + last_contact_start_time = now; + } } else { - last_contact_start_time = now; + last_contact_start_time = last_contact_end_time; } + is_contacted_pre_frame = is_contacted; } auto getContactDuration() { return (last_contact_end_time - last_contact_start_time); } + + auto findPastContact(double duration_sec) + { + auto past = std::chrono::system_clock::now() - std::chrono::duration(duration_sec); + return past < last_contact_end_time; + } + +private: + bool is_contacted_pre_frame = false; }; namespace crane { +struct RobotIdentifier +{ + bool is_ours; + + uint8_t robot_id; +}; + struct RobotInfo { uint8_t id; + RobotIdentifier getID() const { return {true, id}; } + Pose2D pose; Velocity2D vel; @@ -48,7 +70,7 @@ struct RobotInfo using SharedPtr = std::shared_ptr; - Vector2 center_to_kicker() const { return getNormVec(pose.theta) * 0.055; } + Vector2 center_to_kicker() const { return getNormVec(pose.theta) * 0.060; } Point kicker_center() const { return pose.pos + center_to_kicker(); } @@ -140,162 +162,6 @@ struct Ball friend class WorldModelWrapper; }; -struct RobotIdentifier -{ - bool is_ours; - - uint8_t robot_id; -}; -} // namespace crane - -template <> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = Pose2D; - using ros_message_type = geometry_msgs::msg::Pose2D; - - static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) - { - destination.x = source.pos.x(); - destination.y = source.pos.y(); - destination.theta = source.theta; - } - - static void convert_to_custom(const ros_message_type & source, custom_type & destination) - { - destination.pos.x() = source.x; - destination.pos.y() = source.y; - destination.theta = source.theta; - } -}; - -template <> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = Point; - using ros_message_type = geometry_msgs::msg::Pose2D; - - static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) - { - destination.x = source.x(); - destination.y = source.y(); - } - - static void convert_to_custom(const ros_message_type & source, custom_type & destination) - { - destination.x() = source.x; - destination.y() = source.y; - } -}; - -template <> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = Velocity2D; - using ros_message_type = geometry_msgs::msg::Pose2D; - - static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) - { - destination.x = source.linear.x(); - destination.y = source.linear.y(); - destination.theta = source.omega; - } - - static void convert_to_custom(const ros_message_type & source, custom_type & destination) - { - destination.linear << source.x, source.y; - destination.omega = source.theta; - } -}; - -template <> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = crane::Ball; - using ros_message_type = crane_msgs::msg::BallInfo; - - static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) - { - destination.curved = source.is_curve; - destination.detected = true; - destination.disappeared = false; - // destination.detection_time = - rclcpp::TypeAdapter::convert_to_ros_message( - source.pos, destination.pose); - rclcpp::TypeAdapter::convert_to_ros_message( - source.vel, destination.velocity); - } - - static void convert_to_custom(const ros_message_type & source, custom_type & destination) - { - destination.is_curve = source.curved; - rclcpp::TypeAdapter::convert_to_custom( - source.pose, destination.pos); - rclcpp::TypeAdapter::convert_to_custom( - source.velocity, destination.vel); - } -}; - -template <> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = crane::RobotInfo; - using ros_message_type = crane_msgs::msg::RobotInfoOurs; - - static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) - { - destination.disappeared = !source.available; - destination.id = source.id; - rclcpp::TypeAdapter::convert_to_ros_message( - source.pose, destination.pose); - rclcpp::TypeAdapter::convert_to_ros_message( - source.vel, destination.velocity); - } - - static void convert_to_custom(const ros_message_type & source, custom_type & destination) - { - destination.available = !source.disappeared; - destination.id = source.id; - rclcpp::TypeAdapter::convert_to_custom( - source.pose, destination.pose); - rclcpp::TypeAdapter::convert_to_custom( - source.velocity, destination.vel); - } -}; - -template <> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = crane::RobotInfo; - using ros_message_type = crane_msgs::msg::RobotInfoTheirs; - static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) - { - destination.disappeared = !source.available; - destination.id = source.id; - rclcpp::TypeAdapter::convert_to_ros_message( - source.pose, destination.pose); - rclcpp::TypeAdapter::convert_to_ros_message( - source.vel, destination.velocity); - } - static void convert_to_custom(const ros_message_type & source, custom_type & destination) - { - destination.available = !source.disappeared; - destination.id = source.id; - rclcpp::TypeAdapter::convert_to_custom( - source.pose, destination.pose); - rclcpp::TypeAdapter::convert_to_custom( - source.velocity, destination.vel); - } -}; - -namespace crane -{ struct WorldModelWrapper { typedef std::shared_ptr SharedPtr; @@ -333,17 +199,19 @@ struct WorldModelWrapper their_robot->available = false; } + ball.pos << world_model.ball_info.pose.x, world_model.ball_info.pose.y; + ball.vel << world_model.ball_info.velocity.x, world_model.ball_info.velocity.y; + ball.ball_speed_hysteresis.update(ball.vel.norm()); + for (auto & robot : world_model.robot_info_ours) { auto & info = ours.robots.at(robot.id); info->available = !robot.disappeared; if (info->available) { info->id = robot.id; - info->ball_contact.update( - robot.ball_contact.current_time == robot.ball_contact.last_contacted_time); info->pose.pos << robot.pose.x, robot.pose.y; info->pose.theta = robot.pose.theta; info->vel.linear << robot.velocity.x, robot.velocity.y; - // todo : omega + info->ball_contact.update((info->kicker_center() - ball.pos).norm() < 0.1); } else { info->ball_contact.update(false); } @@ -365,11 +233,6 @@ struct WorldModelWrapper } } - ball.pos << world_model.ball_info.pose.x, world_model.ball_info.pose.y; - ball.vel << world_model.ball_info.velocity.x, world_model.ball_info.velocity.y; - ball.ball_speed_hysteresis.update(ball.vel.norm()); - // ball.is_curve = world_model.ball_info.curved; - field_size << world_model.field_info.x, world_model.field_info.y; defense_area_size << world_model.defense_area_size.x, world_model.defense_area_size.y; @@ -390,8 +253,8 @@ struct WorldModelWrapper theirs.defense_area.min << std::min(-ours.defense_area.max.x(), -ours.defense_area.min.x()), ours.defense_area.min.y(); - ball_placement_target << world_model.ball_placement_target.x, - world_model.ball_placement_target.y; + ball_placement_target << world_model.ball_placement_target.x, + world_model.ball_placement_target.y; } const crane_msgs::msg::WorldModel & getMsg() const { return latest_msg; } @@ -414,16 +277,30 @@ struct WorldModelWrapper } } + auto getOurRobot(uint8_t id) { return ours.robots.at(id); } + + auto getTheirRobot(uint8_t id) { return theirs.robots.at(id); } + auto getDistanceFromRobotToBall(RobotIdentifier id) -> double { return getDistanceFromRobot(id, ball.pos); } + auto getDistanceFromRobotToBall(uint8_t our_id) -> double + { + return getDistanceFromRobot({true, our_id}, ball.pos); + } + auto getSquareDistanceFromRobotToBall(RobotIdentifier id) -> double { return getSquareDistanceFromRobot(id, ball.pos); } + auto getSquareDistanceFromRobotToBall(uint8_t our_id) -> double + { + return getSquareDistanceFromRobot({true, our_id}, ball.pos); + } + auto generateFieldPoints(float grid_size) const { std::vector points; @@ -440,11 +317,25 @@ struct WorldModelWrapper return (getRobot(id)->pose.pos - point).norm(); } + auto getDistanceFromRobot(uint8_t our_id, Point point) -> double + { + return (getOurRobot(our_id)->pose.pos - point).norm(); + } + auto getSquareDistanceFromRobot(RobotIdentifier id, Point point) -> double { return (getRobot(id)->pose.pos - point).squaredNorm(); } + auto getSquareDistanceFromRobot(uint8_t our_id, Point point) -> double + { + return (getOurRobot(our_id)->pose.pos - point).squaredNorm(); + } + + auto getDistanceFromBall(Point point) -> double { return (ball.pos - point).norm(); } + + auto getSquareDistanceFromBall(Point point) -> double { return (ball.pos - point).squaredNorm(); } + auto getNearestRobotsWithDistanceFromPoint( Point point, std::vector> & robots) -> std::pair, double> @@ -478,6 +369,18 @@ struct WorldModelWrapper return isInRect(field_rect, p); } + bool isBallPlacementArea(Point p) const + { + // During ball placement, all robots of the non-placing team have to keep + // at least 0.5 meters distance to the line between the ball and the placement position + // (the forbidden area forms a stadium shape). + // ref: https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_placement_interference + // Segment ball_placement_line; + // {Point(ball_placement_target), Point(ball.pos)}; + Segment ball_placement_line(ball_placement_target, ball.pos); + return bg::distance(ball_placement_line, p) <= 0.5; + } + double getDefenseWidth() const { return ours.defense_area.max.y() - ours.defense_area.min.y(); } double getDefenseHeight() const { return ours.defense_area.max.x() - ours.defense_area.min.x(); } @@ -500,7 +403,7 @@ struct WorldModelWrapper Point getTheirGoalCenter() { return Point(-goal.x(), goal.y()); } - Point getBallPlacementTarget() { return ball_placement_target; } + Point getBallPlacementTarget() { return ball_placement_target; } TeamInfo ours; @@ -510,7 +413,7 @@ struct WorldModelWrapper Point goal; - Point ball_placement_target; + Point ball_placement_target; Ball ball;