diff --git a/controllers/soccer_robot/soccer_robot.exe b/controllers/soccer_robot/soccer_robot.exe index 018862a0..97e775bd 100644 Binary files a/controllers/soccer_robot/soccer_robot.exe and b/controllers/soccer_robot/soccer_robot.exe differ diff --git a/controllers/supervisor/constants.hpp b/controllers/supervisor/constants.hpp index 4eeb2334..340ff76f 100644 --- a/controllers/supervisor/constants.hpp +++ b/controllers/supervisor/constants.hpp @@ -81,14 +81,19 @@ namespace constants { constexpr std::size_t KEY_LENGTH = 10; // Game settings - constexpr std::size_t WAIT_READY = 30; // s - constexpr std::size_t WAIT_KILL = 30; // s - constexpr std::size_t WAIT_STABLE = 1; // s - constexpr std::size_t WAIT_GOAL = 3; // s - constexpr std::size_t GAME_TIME = 300; // s - constexpr std::size_t PERIOD_MS = 50; // ms - constexpr std::size_t FOUL_DURATION = 5; // s + constexpr std::size_t WAIT_READY_MS = 30 * 1000; // ms + constexpr std::size_t WAIT_KILL_MS = 30 * 1000; // ms + constexpr std::size_t WAIT_STABLE_MS = 1 * 1000; // ms + constexpr std::size_t WAIT_GOAL_MS = 3 * 1000; // ms + constexpr std::size_t WAIT_END_MS = 3 * 1000; // ms + constexpr std::size_t DEFAULT_GAME_TIME_MS = 300 * 1000; // ms + constexpr std::size_t PERIOD_MS = 50; // ms + constexpr std::size_t FOUL_DURATION_MS = 5 * 1000; // ms constexpr double FOUL_THRESHOLD = 4.; // number of robots in penalty area + constexpr double DEADLOCK_DURATION_MS = 5000; // ms + constexpr double DEADLOCK_THRESHOLD = 0.05; // m/s + + constexpr std::size_t NUM_COMMENTS = 10; constexpr std::size_t MSG_MAX_SIZE = 90000; // bytes @@ -98,6 +103,7 @@ namespace constants { SCORE_RED_TEAM = 2, SCORE_BLUE_TEAM = 3, GAME_END = 4, + DEADLOCK = 5, }; constexpr std::array CODEWORDS = { diff --git a/controllers/supervisor/game.cpp b/controllers/supervisor/game.cpp index 627c3aa5..7defaaa0 100644 --- a/controllers/supervisor/game.cpp +++ b/controllers/supervisor/game.cpp @@ -6,6 +6,8 @@ #include #include +#include + namespace c = constants; namespace bp = boost::process; @@ -58,15 +60,19 @@ namespace msgpack { game::game(supervisor& sv) : sv_(sv) -{ } + , game_time_ms_(sv_.get_game_time_ms()) +{ + for(auto& fc : foul_counter_) { + fc.set_capacity(c::FOUL_DURATION_MS / c::PERIOD_MS); + } +} -int game::run() +void game::run() { if(work_) { throw std::runtime_error("Game already started"); } - int ret = 0; events_stop_ = false; // launch io thread @@ -114,40 +120,79 @@ int game::run() const std::string& name_op = std::get<0>(tup_op); const double& rating_op = std::get<1>(tup_op); - const auto ret = player_team_infos_.emplace(random_string(c::KEY_LENGTH), - team_info(name, rating, exe, data, - team == T_RED, - 1000 * c::FOUL_DURATION / c::PERIOD_MS - )); + const auto ret = player_team_infos_.emplace(std::piecewise_construct, + std::make_tuple(random_string(c::KEY_LENGTH)), + std::make_tuple(name, rating, exe, data, + ROLE_PLAYER, team == T_RED) + ); assert(ret.second); - auto& ti = ret.first->second; // create information for aiwc.get_info() in advance using map = msgpack::type::assoc_vector; map info; - info.emplace_back("field", msgpack::object(std::make_tuple(c::FIELD_LENGTH, c::FIELD_WIDTH), ti.z_info)); - info.emplace_back("goal", msgpack::object(std::make_tuple(c::GOAL_DEPTH, c::GOAL_WIDTH), ti.z_info)); + info.emplace_back("field", msgpack::object(std::make_tuple(c::FIELD_LENGTH, c::FIELD_WIDTH), z_info_)); + info.emplace_back("goal", msgpack::object(std::make_tuple(c::GOAL_DEPTH, c::GOAL_WIDTH), z_info_)); info.emplace_back("penalty_area", msgpack::object(std::make_tuple(c::PENALTY_AREA_DEPTH, - c::PENALTY_AREA_WIDTH), ti.z_info)); - info.emplace_back("ball_radius", msgpack::object(c::BALL_RADIUS, ti.z_info)); - info.emplace_back("robot_size", msgpack::object(c::ROBOT_SIZE, ti.z_info)); - info.emplace_back("axle_length", msgpack::object(c::AXLE_LENGTH, ti.z_info)); - info.emplace_back("max_linear_velocity", msgpack::object(c::MAX_LINEAR_VELOCITY, ti.z_info)); + c::PENALTY_AREA_WIDTH), z_info_)); + info.emplace_back("ball_radius", msgpack::object(c::BALL_RADIUS, z_info_)); + info.emplace_back("robot_size", msgpack::object(c::ROBOT_SIZE, z_info_)); + info.emplace_back("axle_length", msgpack::object(c::AXLE_LENGTH, z_info_)); + info.emplace_back("max_linear_velocity", msgpack::object(c::MAX_LINEAR_VELOCITY, z_info_)); - info.emplace_back("resolution", msgpack::object(std::make_tuple(c::RESOLUTION_X, c::RESOLUTION_Y), ti.z_info)); - info.emplace_back("number_of_robots", msgpack::object(c::NUMBER_OF_ROBOTS, ti.z_info)); - info.emplace_back("codewords", msgpack::object(c::CODEWORDS, ti.z_info)); - info.emplace_back("game_time", msgpack::object(c::GAME_TIME, ti.z_info)); + info.emplace_back("resolution", msgpack::object(std::make_tuple(c::RESOLUTION_X, c::RESOLUTION_Y), z_info_)); + info.emplace_back("number_of_robots", msgpack::object(c::NUMBER_OF_ROBOTS, z_info_)); + info.emplace_back("codewords", msgpack::object(c::CODEWORDS, z_info_)); + info.emplace_back("game_time", msgpack::object(game_time_ms_ / 1000., z_info_)); info.emplace_back("team_info", - msgpack::object(std::make_tuple(map{std::make_pair("name", msgpack::object(name, ti.z_info)), - /* */ std::make_pair("rating", msgpack::object(rating, ti.z_info))}, - /* */ map{std::make_pair("name", msgpack::object(name_op, ti.z_info)), - /* */ std::make_pair("rating", msgpack::object(rating_op, ti.z_info))}), - /* */ ti.z_info)); + msgpack::object(std::make_tuple(map{std::make_pair("name", msgpack::object(name, z_info_)), + /* */ std::make_pair("rating", msgpack::object(rating, z_info_))}, + /* */ map{std::make_pair("name", msgpack::object(name_op, z_info_)), + /* */ std::make_pair("rating", msgpack::object(rating_op, z_info_))}), + /* */ z_info_)); + + info_[team] = msgpack::object(info, z_info_); + } + + // gets commentator information + { + const auto tup = sv_.get_commentator_info(); + + const std::string& name = std::get<0>(tup); + const std::string& exe = std::get<1>(tup); + const std::string& data = std::get<2>(tup); + + if(!exe.empty()) { + // commentator is treated as red team with rating 0 + const auto ret = player_team_infos_.emplace(std::piecewise_construct, + std::make_tuple(random_string(c::KEY_LENGTH)), + std::make_tuple(name, 0, exe, data, + ROLE_COMMENTATOR, true) + ); + + assert(ret.second); + } + } + + // gets reporter information + { + const auto tup = sv_.get_reporter_info(); - ti.info = msgpack::object(info, ti.z_info); + const std::string& name = std::get<0>(tup); + const std::string& exe = std::get<1>(tup); + const std::string& data = std::get<2>(tup); + + if(!exe.empty()) { + // reporter is treated as red team with rating 0 + const auto ret = player_team_infos_.emplace(std::piecewise_construct, + std::make_tuple(random_string(c::KEY_LENGTH)), + std::make_tuple(name, 0, exe, data, + ROLE_REPORTER, true) + ); + + assert(ret.second); + } } // initialize promises and futures @@ -158,19 +203,19 @@ int game::run() connect_to_server(); - // bootup VMs & wait until 2 players boot up + // bootup VMs & wait until app players boot up bootup_vm(); bootup_future.wait(); // wait until 2 players are ready for c::WAIT_READY seconds ready_future.wait_until(std::chrono::steady_clock::now() - + std::chrono::seconds(c::WAIT_READY)); + + std::chrono::milliseconds(c::WAIT_READY_MS)); // run or finish the game { - std::unique_lock lck(player_team_infos_mutex_); const auto count = std::count_if(std::cbegin(player_team_infos_), std::cend(player_team_infos_), - [](const auto& kv) { return kv.second.is_ready == true; }); + [](const auto& kv) { return kv.second.role == ROLE_PLAYER + && kv.second.is_ready == true; }); if(count == 0) { // send the result(draw) to the matching server } @@ -180,38 +225,30 @@ int game::run() assert(it != std::cend(player_team_infos_)); // TODO: send winning frame to team pointed by it. - ret = 0; } else { - lck.unlock(); - try { std::cout << "Starting a new game" << std::endl; run_game(); - } - catch(const webots_revert_exception& e) { - ret = -1; - } - } - } - if(ret == -1) { // if webots reverts - terminate_vm(); - } - else { // Not webots revert - // now players have c::WAIT_KILL seconds to finish - const auto until = std::chrono::steady_clock::now() + std::chrono::seconds(c::WAIT_KILL); + // now players have c::WAIT_KILL seconds to finish + const auto until = std::chrono::steady_clock::now() + std::chrono::milliseconds(c::WAIT_KILL_MS); - std::cout << "Waiting players to finish" << std::endl; + std::cout << "Waiting players to finish" << std::endl; - for(auto& kv : player_team_infos_) { - auto& ti = kv.second; + for(auto& kv : player_team_infos_) { + auto& ti = kv.second; - // boost 1.65 or lower has a bug in child::wait_until(). use child::wait_for(). - // ti.c.wait_until(until); - ti.c.wait_for(until - std::chrono::steady_clock::now()); - if(ti.c.running()) { - ti.c.terminate(); + // boost 1.65 or lower has a bug in child::wait_until(). use child::wait_for(). + // ti.c.wait_until(until); + ti.c.wait_for(until - std::chrono::steady_clock::now()); + if(ti.c.running()) { + ti.c.terminate(); + } + } + } + catch(const webots_revert_exception& e) { + terminate_vm(); } } } @@ -233,8 +270,6 @@ int game::run() }); io_thread_.join(); - - return ret; } void game::connect_to_server() @@ -256,41 +291,60 @@ void game::connect_to_server() session_->join(constants::REALM).get(); // register calles - session_->provide("aiwc.bootup", [&](autobahn::wamp_invocation i) { return on_bootup(std::move(i)); }).get(); - session_->provide("aiwc.ready", [&](autobahn::wamp_invocation i) { return on_ready(std::move(i)); }).get(); - session_->provide("aiwc.get_info", [&](autobahn::wamp_invocation i) { return on_info(std::move(i)); }).get(); - session_->provide("aiwc.set_speed", [&](autobahn::wamp_invocation i) { return on_set_speed(std::move(i)); }).get(); + session_->provide("aiwc.bootup", [&](autobahn::wamp_invocation i) { return on_bootup(i); }).get(); + session_->provide("aiwc.ready", [&](autobahn::wamp_invocation i) { return on_ready(i); }).get(); + session_->provide("aiwc.get_info", [&](autobahn::wamp_invocation i) { return on_info(i); }).get(); + session_->provide("aiwc.set_speed", [&](autobahn::wamp_invocation i) { return on_set_speed(i); }).get(); + session_->provide("aiwc.commentate", [&](autobahn::wamp_invocation i) { return on_commentate(i); }).get(); + session_->provide("aiwc.report", [&](autobahn::wamp_invocation i) { return on_report(i); }).get(); } void game::bootup_vm() { - std::unique_lock lck(player_team_infos_mutex_); - // bootup VMs. replaced to process. - for(auto& kv : player_team_infos_) { - const auto& key = kv.first; - auto& ti = kv.second; - // send bootup signal. VM's startup script is supposed to do this. - session_->call("aiwc.bootup", std::make_tuple(key)); + // send bootup signal. VM's startup script is supposed to do this. + std::vector > bootup_futures; + for(const auto& kv : player_team_infos_) { + bootup_futures.emplace_back(session_->call("aiwc.bootup", std::make_tuple(kv.first))); + } + + // wait for all bootup + for(auto& f : bootup_futures) { + f.get(); + } + + try { + for(auto& kv : player_team_infos_) { + const auto& key = kv.first; + auto& ti = kv.second; + + // launch process + boost::filesystem::path p_exe = ti.executable; - // launch process - boost::filesystem::path p_exe = ti.executable; + ti.c = bp::child(bp::exe = ti.executable, + bp::args = {c::SERVER_IP, + std::to_string(c::RS_PORT), + c::REALM, + key, + boost::filesystem::absolute(ti.datapath).string()}, + bp::start_dir = p_exe.parent_path()); + } + } + catch(const boost::process::process_error& err) { + for(auto& kv : player_team_infos_) { + auto& ti = kv.second; - ti.c = bp::child(bp::exe = ti.executable, - bp::args = {c::SERVER_IP, - std::to_string(c::RS_PORT), - c::REALM, - key, - boost::filesystem::absolute(ti.datapath).string()}, - bp::start_dir = p_exe.parent_path()); + if(ti.c) { + ti.c.terminate(); + } + } + throw std::runtime_error("one of the given executables does not exist, or cannot run"); } } void game::terminate_vm() { - std::unique_lock lck(player_team_infos_mutex_); - for(auto& kv : player_team_infos_) { kv.second.c.terminate(); } @@ -299,56 +353,111 @@ void game::terminate_vm() void game::update_label() { sv_.setLabel(0, - (boost::format("score %d:%d, time %.2f") % score_[0] % score_[1] % time_).str(), + (boost::format("score %d:%d, time %.2f") % score_[0] % score_[1] % (time_ms_ / 1000.)).str(), 0.4, 0.95, // x, y 0.10, 0x00000000, // size, color 0, "Arial" // transparency, font ); + + constexpr std::size_t comments_start = 1; + + std::unique_lock lck(m_comments_); + for(std::size_t i = 0; i < comments_.size(); ++i) { + sv_.setLabel(comments_start + i, + comments_[i], + 0.01, 0.02 + 0.04 * i, // x, y + 0.08, 0x00000000, // size, color + 0, "Arial" // transparency, font + ); + } } +// game state control functions void game::step(std::size_t ms) { - if(sv_.step(ms) == -1) { - throw webots_revert_exception(); + // we assume that world.basicTimeStep doesn't change and the given 'ms' is multiple of the basic time step + const std::size_t basic_time_step = static_cast(sv_.getBasicTimeStep()); + + const auto step_throw_if_revert = [&](std::size_t ms) { + if(sv_.step(ms) == -1) { + throw webots_revert_exception(); + } + }; + + for(std::size_t i = 0; i < ms / basic_time_step; ++i) { + if(paused_.load()) { + stop_robots(); + } + else { + send_speed(); + time_ms_ += basic_time_step; + } + + update_label(); + step_throw_if_revert(basic_time_step); } } -void game::reset_position() { +void game::pause() +{ + paused_.store(true); +} + +void game::reset() +{ sv_.reset_position(); + // reset activeness for(auto& team_activeness : activeness_) { for(auto& robot_activeness : team_activeness) { robot_activeness = true; } } - std::unique_lock lck(player_team_infos_mutex_); - for(auto& kv : player_team_infos_) { - auto& ti = kv.second; + stop_robots(); - // reset wheel speed to 0 - for(std::size_t id = 0; id < c::NUMBER_OF_ROBOTS; ++id) { - ti.wheel_speed[id] = {0, 0}; - } - - // reset foul counter - ti.foul_count.clear(); + // reset foul counter + for(auto& fc : foul_counter_) { + fc.clear(); } + + deadlock_time_ = time_ms_; } -void game::set_speed(bool stop_all) +void game::resume() +{ + paused_.store(false); +} + +void game::stop_robots() +{ + std::promise done; + auto done_fut = done.get_future(); + + // set wheel speed, defer it to io_thread and wait + io_.dispatch([&]() { + io_thread_wheel_speed_ = {}; + wheel_speed_.write(io_thread_wheel_speed_); + done.set_value(); + }); + done_fut.get(); + + send_speed(); +} + +void game::send_speed() { constexpr std::array stop = {0, 0}; - std::unique_lock lck(player_team_infos_mutex_); - for(auto& kv : player_team_infos_) { - auto& ti = kv.second; + const auto ws = wheel_speed_.read(); + + for(const auto& team : {T_RED, T_BLUE}) { for(std::size_t id = 0; id < c::NUMBER_OF_ROBOTS; ++id) { - if(stop_all || !activeness_[ti.is_red ? T_RED : T_BLUE][id]) { - sv_.set_linear_wheel_speed(ti.is_red, id, stop); + if(activeness_[team][id]) { + sv_.set_linear_wheel_speed(team == T_RED, id, ws[team][id]); } else { - sv_.set_linear_wheel_speed(ti.is_red, id, ti.wheel_speed[id]); + sv_.set_linear_wheel_speed(team == T_RED, id, stop); } } } @@ -359,14 +468,14 @@ std::size_t game::count_robots_in_penalty_area(bool is_red) const std::size_t ret = 0; constexpr auto is_in_goal_area = [](double x, double y) { - return (x < -c::FIELD_LENGTH / 2) && (std::abs(y) < c::GOAL_WIDTH); + return (x < -c::FIELD_LENGTH / 2) && (std::abs(y) < c::GOAL_WIDTH / 2); }; constexpr auto is_in_penalty_area = [](double x, double y) { return (x >= -c::FIELD_LENGTH / 2) && (x < -c::FIELD_LENGTH / 2 + c::PENALTY_AREA_DEPTH) - && (std::abs(y) < c::PENALTY_AREA_WIDTH); + && (std::abs(y) < c::PENALTY_AREA_WIDTH / 2); }; for(std::size_t id = 0; id < c::NUMBER_OF_ROBOTS; ++id) { @@ -402,7 +511,6 @@ void game::publish_current_frame(std::size_t reset_reason) std::vector > events; { - std::unique_lock lck(player_team_infos_mutex_); for(auto& kv : player_team_infos_) { const auto& topic = kv.first; auto& ti = kv.second; @@ -431,7 +539,7 @@ void game::publish_current_frame(std::size_t reset_reason) msgpack::zone z; // The first frame contains time, score, reset_reason - msg.emplace_back("time", msgpack::object(time_, z)); + msg.emplace_back("time", msgpack::object(time_ms_ / 1000., z)); msg.emplace_back("score", msgpack::object(score, z)); msg.emplace_back("reset_reason", msgpack::object(reason, z)); @@ -488,7 +596,7 @@ void game::publish_current_frame(std::size_t reset_reason) void game::run_game() { - time_ = 0; + time_ms_ = 0; score_ = {0, 0}; for(auto& team_activeness : activeness_) { @@ -500,79 +608,99 @@ void game::run_game() update_label(); // reset and wait 1s for stabilizing - reset_position(); - set_speed(true); - step(c::WAIT_STABLE * 1000); + pause(); + reset(); + step(c::WAIT_STABLE_MS); - std::size_t reset_reason = c::GAME_START; + resume(); + publish_current_frame(c::GAME_START); + update_label(); + + auto reset_reason = c::NONE; for(;;) { + step(c::PERIOD_MS); update_label(); - // if game ends - if(time_ >= c::GAME_TIME) { + // special case: game ended. finish the game without checking game rules. + if(time_ms_ >= game_time_ms_) { publish_current_frame(c::GAME_END); - break; + pause(); + stop_robots(); + step(c::WAIT_END_MS); + return; } - // if a team scored - { + // publish current frame + publish_current_frame(reset_reason); + reset_reason = c::NONE; + + // check rules + { // if a team scored const auto ball_x = std::get<0>(sv_.get_ball_position()); if(std::abs(ball_x) > c::FIELD_LENGTH / 2) { ++score_[(ball_x > 0) ? T_RED : T_BLUE]; update_label(); // stop all and wait for c::WAIT_GOAL seconds - set_speed(true); - step(c::WAIT_GOAL * 1000); + pause(); + stop_robots(); + step(c::WAIT_GOAL_MS); - reset_position(); - step(c::WAIT_STABLE * 1000); + // reset and wait until stabilized + reset(); + step(c::WAIT_STABLE_MS); + resume(); reset_reason = (ball_x > 0) ? c::SCORE_RED_TEAM : c::SCORE_BLUE_TEAM; } } + // if the ball is not moved for c::DEADLOCK_THRESHOLD + if(reset_reason == c::NONE) { + if(sv_.get_ball_velocity() >= c::DEADLOCK_THRESHOLD) { + deadlock_time_ = time_ms_; + } + else if((time_ms_ - deadlock_time_) >= c::DEADLOCK_DURATION_MS) { + pause(); + stop_robots(); + reset(); + step(c::WAIT_STABLE_MS); + resume(); + + reset_reason = c::DEADLOCK; + } + } + // if a team is blocking the goal area { - std::unique_lock lck(player_team_infos_mutex_); - for(auto& kv : player_team_infos_) { - auto& ti = kv.second; - auto& foul_counter = ti.foul_count; - - foul_counter.push_back(count_robots_in_penalty_area(ti.is_red)); + for(const auto& team : {T_RED, T_BLUE}) { + foul_counter_[team].push_back(count_robots_in_penalty_area(team == T_RED)); - const auto sum = std::accumulate(std::cbegin(foul_counter), std::cend(foul_counter), (std::size_t)0); - if(sum >= c::FOUL_THRESHOLD * foul_counter.capacity()) { + const auto sum = std::accumulate(std::cbegin(foul_counter_[team]), std::cend(foul_counter_[team]), (std::size_t)0); + if(sum >= c::FOUL_THRESHOLD * foul_counter_[team].capacity()) { std::mt19937 rng{std::random_device{}()}; std::uniform_int_distribution dist(0, 4); - auto& team_activeness = activeness_[ti.is_red ? T_RED : T_BLUE]; + auto& team_activeness = activeness_[team]; if(std::any_of(std::begin(team_activeness), std::end(team_activeness), [](const auto& is_active) { return is_active; })) { for(;;) { std::size_t id = dist(rng); - auto& is_active = activeness_[ti.is_red ? T_RED : T_BLUE][id]; + auto& is_active = activeness_[team][id]; if(is_active) { is_active = false; - sv_.send_to_foulzone(ti.is_red, id); + sv_.send_to_foulzone(team == T_RED, id); break; } } } - foul_counter.clear(); + foul_counter_[team].clear(); } } } - - publish_current_frame(reset_reason); - reset_reason = c::NONE; - - set_speed(); - step(c::PERIOD_MS); - time_ += c::PERIOD_MS / 1000.; } } @@ -581,23 +709,32 @@ void game::on_bootup(autobahn::wamp_invocation invocation) { const std::string key = invocation->argument(0); - std::unique_lock lck(player_team_infos_mutex_); - const auto it = player_team_infos_.find(key); if(it == std::cend(player_team_infos_)) { invocation->error("wamp.error.invalid_argument"); return; } + if(state_.load() != STATE_WAITING_BOOTUP) { + invocation->empty_result(); + return; + } + auto& ti = it->second; ti.is_bootup = true; + bootup_waiting_list_.push_back(invocation); + if(std::all_of(std::cbegin(player_team_infos_), std::cend(player_team_infos_), [](const auto& kv) { return kv.second.is_bootup == true; })) { + std::cout << "Everyone bootup" << std::endl; + state_.store(STATE_WAITING_READY); + for(auto& inv : bootup_waiting_list_) { + inv->empty_result(); + } + bootup_waiting_list_.clear(); bootup_promise_.set_value(); } - - invocation->empty_result(); } // called from io thread @@ -605,7 +742,7 @@ void game::on_ready(autobahn::wamp_invocation invocation) { const std::string key = invocation->argument(0); - std::unique_lock lck(player_team_infos_mutex_); + assert(state_.load() != STATE_WAITING_BOOTUP); const auto it = player_team_infos_.find(key); if(it == std::cend(player_team_infos_)) { @@ -615,7 +752,12 @@ void game::on_ready(autobahn::wamp_invocation invocation) auto& ti = it->second; - if(!ti.is_ready) { // the first time it's ready + { // reset image buffer + std::unique_lock lck(ti.m); + ti.imbuf.reset(); + } + + if(state_.load() != STATE_STARTED) { ti.is_ready = true; if(std::all_of(std::cbegin(player_team_infos_), std::cend(player_team_infos_), @@ -623,9 +765,6 @@ void game::on_ready(autobahn::wamp_invocation invocation) ready_promise_.set_value(); } } - else { // maybe reconnect - ti.imbuf.reset(); - } invocation->empty_result(); } @@ -634,7 +773,6 @@ void game::on_ready(autobahn::wamp_invocation invocation) void game::on_info(autobahn::wamp_invocation invocation) { const std::string caller = invocation->argument(0); - std::unique_lock lck(player_team_infos_mutex_); // if the caller is not a player, then error auto it = player_team_infos_.find(caller); @@ -644,32 +782,76 @@ void game::on_info(autobahn::wamp_invocation invocation) } auto& ti = it->second; - invocation->result(std::make_tuple(ti.info)); + invocation->result(std::make_tuple(info_[ti.is_red ? T_RED : T_BLUE])); } +// called from io thread. it just sets the wheel speed and not transfer it to the simulator. void game::on_set_speed(autobahn::wamp_invocation invocation) { const auto caller = invocation->argument(0); - const auto wheel_speed = invocation->argument >(1); - std::unique_lock lck(player_team_infos_mutex_); + // if the caller is not a player, then error + auto it = player_team_infos_.find(caller); + if((it == std::cend(player_team_infos_)) + || (it->second.role != ROLE_PLAYER) + ) { + invocation->error("wamp.error.invalid_argument"); + return; + } + + if(!paused_.load()) { // if it's paused, ignore the call and return + const bool is_red = it->second.is_red; + + const auto ws = invocation->argument >(1); + + for(std::size_t i = 0; i < c::NUMBER_OF_ROBOTS; ++i) { + io_thread_wheel_speed_[is_red ? T_RED : T_BLUE][i][0] = ws[2*i + 0]; + io_thread_wheel_speed_[is_red ? T_RED : T_BLUE][i][1] = ws[2*i + 1]; + } + + wheel_speed_.write(io_thread_wheel_speed_); + } + + invocation->empty_result(); +} + +// called from io thread. +void game::on_commentate(autobahn::wamp_invocation invocation) +{ + const auto caller = invocation->argument(0); // if the caller is not a player, then error auto it = player_team_infos_.find(caller); - if(it == std::cend(player_team_infos_)) { + if((it == std::cend(player_team_infos_)) + || (it->second.role != ROLE_COMMENTATOR) + ) { invocation->error("wamp.error.invalid_argument"); return; } - auto& ti = it->second; - auto it_w = std::begin(wheel_speed); + std::unique_lock lck(m_comments_); + comments_.push_back(invocation->argument(1)); - for(std::size_t id = 0; id < c::NUMBER_OF_ROBOTS; ++id) { - std::array wheel; - wheel[0] = *it_w++; - wheel[1] = *it_w++; - ti.wheel_speed[id] = wheel; + invocation->empty_result(); +} + +// called from io thread. +void game::on_report(autobahn::wamp_invocation invocation) +{ + const auto caller = invocation->argument(0); + + // if the caller is not a player, then error + auto it = player_team_infos_.find(caller); + if((it == std::cend(player_team_infos_)) + || (it->second.role != ROLE_REPORTER) + ) { + invocation->error("wamp.error.invalid_argument"); + return; } + const auto report = invocation->argument(1); + + // we will handle this report internally. + invocation->empty_result(); } diff --git a/controllers/supervisor/game.hpp b/controllers/supervisor/game.hpp index 94545ce1..68b7aa34 100644 --- a/controllers/supervisor/game.hpp +++ b/controllers/supervisor/game.hpp @@ -33,6 +33,18 @@ struct webots_revert_exception class game { + enum state_t { + STATE_WAITING_BOOTUP = 0, + STATE_WAITING_READY = 1, + STATE_STARTED = 2, + }; + + enum role_t { + ROLE_PLAYER = 0, + ROLE_COMMENTATOR = 1, + ROLE_REPORTER = 2, + }; + public: game(supervisor& sv); @@ -41,8 +53,7 @@ class game game(game&&) = default; game& operator=(game&&) = default; - // return -1 when webots tries to revert - int run(); + void run(); // throws webots_revert_exceptions private: void connect_to_server(); @@ -52,14 +63,15 @@ class game void update_label(); - // throw webots_revert_exception when webots reverts - void step(std::size_t ms); + // game state control functions + void step(std::size_t ms); // throw webots_revert_exception when webots reverts + void pause(); + void reset(); + void resume(); + void stop_robots(); - // reset_position and wait for stabilizing_time_ms - void reset_position(); - - // send wheel speed to the simulator - void set_speed(bool stop_all = false); + // simulator-related functions + void send_speed(); // send wheel speed to the simulator std::size_t count_robots_in_penalty_area(bool is_red) const; @@ -72,6 +84,8 @@ class game void on_ready(autobahn::wamp_invocation invocation); void on_info(autobahn::wamp_invocation invocation); void on_set_speed(autobahn::wamp_invocation invocation); + void on_commentate(autobahn::wamp_invocation invocation); + void on_report(autobahn::wamp_invocation invocation); private: supervisor& sv_; @@ -94,41 +108,58 @@ class game struct team_info { - team_info(std::string name, double rating, std::string exe, std::string datapath, bool is_red, std::size_t num_foul_record) + team_info(std::string name, double rating, std::string exe, std::string datapath, + const role_t& role, bool is_red) + // std::size_t num_foul_record) : name(std::move(name)), rating(rating) , executable(std::move(exe)), datapath(std::move(datapath)) - , is_red(is_red) + , role(role), is_red(is_red) , is_bootup{false}, is_ready{false} - , foul_count(num_foul_record) , imbuf(constants::RESOLUTION_X, constants::RESOLUTION_Y, constants::SUBIMAGE_NX, constants::SUBIMAGE_NY) { } - std::string name; - double rating; - std::string executable; - std::string datapath; + const std::string name; + const double rating; + const std::string executable; + const std::string datapath; + const role_t role; + const bool is_red; + boost::process::child c; - bool is_red; bool is_bootup; bool is_ready; - boost::circular_buffer foul_count; - - msgpack::zone z_info; - msgpack::object info; // premade aiwc.info return value - + std::mutex m; // for shared members image_buffer imbuf; - std::array, constants::NUMBER_OF_ROBOTS> wheel_speed; }; - std::mutex player_team_infos_mutex_; std::map player_team_infos_; - double time_ = 0; + msgpack::zone z_info_; + msgpack::object info_[2]; // for red team's view and blue team's view + + std::atomic state_{STATE_WAITING_BOOTUP}; + + const std::size_t game_time_ms_; + std::size_t time_ms_ = 0; std::array score_ = {{0, 0}}; std::array, 2> activeness_; + std::atomic paused_{true}; + + std::vector bootup_waiting_list_; + + std::array, 2> foul_counter_; + std::size_t deadlock_time_ = 0; + + using wheel_speed_t = std::array, constants::NUMBER_OF_ROBOTS>, 2>; + + wheel_speed_t io_thread_wheel_speed_ = {}; // only used in io_thread + aiwc::spsc_buffer wheel_speed_ = {}; // producer: io thread, consumer: game thread + + std::mutex m_comments_; + boost::circular_buffer comments_{constants::NUM_COMMENTS}; std::promise bootup_promise_; std::promise ready_promise_; diff --git a/controllers/supervisor/spsc_buffer.hpp b/controllers/supervisor/spsc_buffer.hpp new file mode 100644 index 00000000..fb96c0d2 --- /dev/null +++ b/controllers/supervisor/spsc_buffer.hpp @@ -0,0 +1,97 @@ +#ifndef H_SPSC_BUFFER_HPP +#define H_SPSC_BUFFER_HPP +#pragma once + +#include +#include +#include + +#include + +// TODO: give appropriate memory orders to atomic operations for performance + +namespace aiwc { + namespace detail { + + class flag + { + public: + using rep_type = std::uint_fast8_t; // XNDD CCSS + + constexpr flag() + : flag(false, 0, 1, 2) + { } + + constexpr bool is_new_write() const { return rep_ & 0x40; } + constexpr std::size_t dirty() const { return (rep_ >> 4) & 0x03; } + constexpr std::size_t clean() const { return (rep_ >> 2) & 0x03; } + constexpr std::size_t snap() const { return (rep_ ) & 0x03; } + + static constexpr flag consume(const flag& f) // swap clean and snap + { return flag{false, f.dirty(), f.snap(), f.clean()}; } + + static constexpr flag produce(const flag& f) // swap dirty and clean + { return flag{true, f.clean(), f.dirty(), f.snap()}; } + + private: + constexpr flag(bool is_new, std::size_t dirty, std::size_t clean, std::size_t snap) + : rep_{static_cast((is_new ? 0x40u : 0x00) | (dirty << 4) | (clean << 2) | (snap << 0))} + { } + + private: + rep_type rep_; + }; + + } // namespace detail + + template + class spsc_buffer + { + public: + using value_type = T; + + public: + spsc_buffer(const value_type& v = value_type{}) + : flag_{{}} + , data_{v, v, v} + { } + + // noncopyable but movable + spsc_buffer(const spsc_buffer&) = delete; + spsc_buffer& operator=(const spsc_buffer&) = delete; + spsc_buffer(spsc_buffer&&) = default; + spsc_buffer& operator=(spsc_buffer&&) = default; + + bool is_lock_free() const { return flag_.is_lock_free(); } + + value_type read() const + { + auto f = flag_.load(); + + do { + if(!f.is_new_write()) { // if it's not new, just return snap + break; + } + } while(!flag_.compare_exchange_weak(f, detail::flag::consume(f))); + + return data_[f.snap()]; + } + + template + void write(U&& u) + { + auto f = flag_.load(); + + data_[f.dirty()] = std::forward(u); + + while(!flag_.compare_exchange_weak(f, detail::flag::produce(f))); + } + + private: + mutable std::atomic flag_; + std::array data_; + }; + +} // namespace aiwc + +#endif // H_SPSC_BUFFER_HPP diff --git a/controllers/supervisor/supervisor b/controllers/supervisor/supervisor index 1982da82..b4a7c071 100755 Binary files a/controllers/supervisor/supervisor and b/controllers/supervisor/supervisor differ diff --git a/controllers/supervisor/supervisor.cpp b/controllers/supervisor/supervisor.cpp index 48d7eb56..1f2ee5f4 100644 --- a/controllers/supervisor/supervisor.cpp +++ b/controllers/supervisor/supervisor.cpp @@ -17,16 +17,13 @@ int main() }); supervisor sv; - - for(;;) { - game g(sv); - if(g.run() == -1) { - break; - } - } + game g(sv); + g.run(); wr.shutdown(); wamp_router_th.join(); + sv.simulationRevert(); + return 0; } diff --git a/controllers/supervisor/supervisor.exe b/controllers/supervisor/supervisor.exe index dbd62cb1..0dfcbf24 100644 Binary files a/controllers/supervisor/supervisor.exe and b/controllers/supervisor/supervisor.exe differ diff --git a/controllers/supervisor/supervisor.hpp b/controllers/supervisor/supervisor.hpp index ada4f820..361d03cd 100644 --- a/controllers/supervisor/supervisor.hpp +++ b/controllers/supervisor/supervisor.hpp @@ -3,6 +3,7 @@ #pragma once #include "constants.hpp" +#include "spsc_buffer.hpp" #include #include @@ -119,32 +120,27 @@ class supervisor remove_ball_and_robots(); place_ball_and_robots(); - // get Node* of ball and robots and check if it's not null - if(is_null(pn_ball_ = getFromDef(constants::DEF_BALL))) { - throw std::runtime_error("No ball in world"); - } - for(int t = 0; t < 2; ++t) { - for(std::size_t id = 0; id < constants::NUMBER_OF_ROBOTS; ++id) { - if(is_null(pn_robots_[t][id] = getFromDef(robot_name(t==T_RED, id)))) { - throw std::runtime_error("No robot in world"); - } - } - } - // control visibility to cams control_visibility(); - - reset_position(); enable_cameras(constants::CAM_PERIOD_MS); } - // void run() - // { - // while(step(1000) != -1) { - // const auto p = get_robot_posture(true, 0); - // std::cout << "x: " << std::get<0>(p) << ", y: " << std::get<1>(p) << ", th: " << std::get<2>(p) << std::endl; - // } - // } + std::size_t get_game_time_ms() const + { + const auto pf = getSelf()->getField("gameTime"); + + std::size_t gt_ms = 0; + + if(pf) { + gt_ms = static_cast(pf->getSFFloat() * 1000); + } + + if(gt_ms == 0) { + gt_ms = constants::DEFAULT_GAME_TIME_MS; + } + + return gt_ms / constants::PERIOD_MS * constants::PERIOD_MS; + } // name rating executable data directory path std::tuple get_team_info(bool is_red) const @@ -158,6 +154,26 @@ class supervisor ); } + std::tuple get_commentator_info() const + { + const auto prefix = std::string("commentator"); + + return std::make_tuple(getSelf()->getField(prefix + "Name")->getSFString(), + getSelf()->getField(prefix + "Executable")->getSFString(), + getSelf()->getField(prefix + "DataPath")->getSFString() + ); + } + + std::tuple get_reporter_info() const + { + const auto prefix = std::string("reporter"); + + return std::make_tuple(getSelf()->getField(prefix + "Name")->getSFString(), + getSelf()->getField(prefix + "Executable")->getSFString(), + getSelf()->getField(prefix + "DataPath")->getSFString() + ); + } + const unsigned char* get_image(bool is_red) const { return pc_cams_[is_red ? C_CAMA : C_CAMB]->getImage(); @@ -201,6 +217,18 @@ class supervisor return {x, y}; } + double get_ball_velocity() const + { + webots::Node* pn_ball = getFromDef(constants::DEF_BALL); + + const double* vels = pn_ball->getVelocity(); + const double x = vels[0]; + const double y = vels[1]; + const double z = vels[2]; + + return std::sqrt(x * x + y * y + z * z); + } + // x y th std::tuple get_robot_posture(bool is_red, std::size_t id) const { @@ -250,6 +278,8 @@ class supervisor private: // private member functions void remove_ball_and_robots() { + using namespace constants; + const auto& remove_node = [&](const std::string& defname) { auto* const pn = getFromDef(defname); if(pn) { @@ -331,8 +361,10 @@ class supervisor // patches { - for(const auto& pn_team : pn_robots_) { - for(const auto& pn_robot : pn_team) { + for(const auto& team : {T_RED, T_BLUE}) { + for(std::size_t id = 0; id < constants::NUMBER_OF_ROBOTS; ++id) { + const auto* pn_robot = getFromDef(robot_name(team == T_RED, id)); + auto* pf_patches = pn_robot->getField("patches"); assert(pf_patches && (pf_patches->getCount() == 3)); @@ -362,9 +394,6 @@ class supervisor private: // private member variables std::array pn_cams_; std::array pc_cams_; - - webots::Node* pn_ball_; - std::array, 2> pn_robots_; }; #endif // H_SUPERVISOR_HPP diff --git a/examples b/examples index 91cdfb0b..597237d6 160000 --- a/examples +++ b/examples @@ -1 +1 @@ -Subproject commit 91cdfb0b8783967ffdc6e65ceb1f38a0237cc62a +Subproject commit 597237d67bb9295e2794847a872ee556547165a3 diff --git a/extlibs/autobahn-cpp/include/autobahn/.wamp_session.hpp.swp b/extlibs/autobahn-cpp/include/autobahn/.wamp_session.hpp.swp deleted file mode 100644 index cf9f71af..00000000 Binary files a/extlibs/autobahn-cpp/include/autobahn/.wamp_session.hpp.swp and /dev/null differ diff --git a/protos/TestReferee.proto b/protos/TestReferee.proto index 76586237..9228b36b 100644 --- a/protos/TestReferee.proto +++ b/protos/TestReferee.proto @@ -15,14 +15,25 @@ PROTO TestReferee [ field SFBool synchronization TRUE # fields specific to TestReferee + field SFFloat gameTime 300 + field SFString teamAName "teamA" field SFFloat teamARating 0 field SFString teamAExecutable "" field SFString teamADataPath "" + field SFString teamBName "teamB" field SFFloat teamBRating 0 field SFString teamBExecutable "" field SFString teamBDataPath "" + + field SFString commentatorName "commentator" + field SFString commentatorExecutable "" + field SFString commentatorDataPath "" + + field SFString reporterName "reporter" + field SFString reporterExecutable "" + field SFString reporterDataPath "" ] { %{ diff --git a/protos/soccerfield/SoccerFieldLine.proto b/protos/soccerfield/SoccerFieldLine.proto index c79fa3d8..a1227699 100644 --- a/protos/soccerfield/SoccerFieldLine.proto +++ b/protos/soccerfield/SoccerFieldLine.proto @@ -15,6 +15,8 @@ PROTO SoccerFieldLine [ field SFFloat penaltyAreaWidth 0.8 field SFFloat goalAreaDepth 0.15 field SFFloat goalAreaWidth 0.5 + field SFFloat penaltyArcProportion 0.25 + field SFFloat penaltyArcSubdivision 7 field SFNode lineAppearance White { } field SFFloat lineThickness 0.01 @@ -32,6 +34,8 @@ PROTO SoccerFieldLine [ local paw = fields.penaltyAreaWidth.value local gad = fields.goalAreaDepth.value local gaw = fields.goalAreaWidth.value + local pap = fields.penaltyArcProportion.value + local pas = fields.penaltyArcSubdivision.value local lt = fields.lineThickness.value local lh = fields.lineHeight.value @@ -104,7 +108,7 @@ PROTO SoccerFieldLine [ Shape { appearance IS lineAppearance geometry Box { - size %{=lt}% %{=lh}% %{=paw}% + size %{=lt}% %{=lh}% %{=paw + lt}% } } ] @@ -149,7 +153,7 @@ PROTO SoccerFieldLine [ Shape { appearance IS lineAppearance geometry Box { - size %{=lt}% %{=lh}% %{=gaw}% + size %{=lt}% %{=lh}% %{=gaw + lt}% } } ] @@ -186,24 +190,77 @@ PROTO SoccerFieldLine [ ] } - # DEF LEFTGOALLINE Transform { - # translation %{=-(l + lt) / 2}% 0 0 - # children [ - # Shape { - # appearance IS lineAppearance - # geometry Box { - # size %{=lt}% %{=lh}% %{=paw}% - # } - # } - # ] - # } - - # DEF RIGHTGOALLINE Transform { - # rotation 0 1 0 %{=math.pi}% - # children [ - # USE LEFTGOALLINE - # ] - # } + DEF LEFTGOALLINE Transform { + translation %{=-(l + lt) / 2}% 0 0 + children [ + Shape { + appearance IS lineAppearance + geometry Box { + size %{=lt}% %{=lh}% %{=paw}% + } + } + ] + } + + DEF RIGHTGOALLINE Transform { + rotation 0 1 0 %{=math.pi}% + children [ + USE LEFTGOALLINE + ] + } + + DEF LEFTPENALTYARC Transform { + translation %{=-(l + lt) / 2 + pad - (1 - pap)*ccr + lt/2}% 0 0 + children [ + Shape { + appearance IS lineAppearance + geometry IndexedFaceSet { + coord Coordinate { + point [ + %{ + local hca = math.cos(1 - pap) + for i = 0, (pas - 1) do + local th = -hca + i * 2 * hca / (pas - 1) + + local ix = (ccr - lt / 2) * math.cos(th) + local iy = (ccr - lt / 2) * math.sin(th) + local ox = (ccr + lt / 2) * math.cos(th) + local oy = (ccr + lt / 2) * math.sin(th) + }% + %{=ix}% %{=-lh/2}% %{=iy}% # 0 : il + %{=ix}% %{=lh/2}% %{=iy}% # 1 : iu + %{=ox}% %{=-lh/2}% %{=oy}% # 2 : ol + %{=ox}% %{=lh/2}% %{=oy}% # 3 : ou + %{ end }% + ] + } + coordIndex [ + %{ + local il = function (i) return (i % pas) * 4 + 0 end + local iu = function (i) return (i % pas) * 4 + 1 end + local ol = function (i) return (i % pas) * 4 + 2 end + local ou = function (i) return (i % pas) * 4 + 3 end + + for i = 0, (pas - 1) do + }% + + %{=ol(i)}% %{=il(i)}% %{=il(i+1)}% %{=ol(i+1)}% -1 # floor + %{=ou(i)}% %{=iu(i)}% %{=iu(i+1)}% %{=ou(i+1)}% -1 # top + %{=iu(i)}% %{=il(i)}% %{=il(i+1)}% %{=iu(i+1)}% -1 # inner + %{=ol(i)}% %{=ou(i)}% %{=ou(i+1)}% %{=ol(i+1)}% -1 # outer + %{ end }% + ] + } + } + ] + } + + DEF RIGHTPENALTYARC Transform { + rotation 0 1 0 %{=math.pi}% + children[ + USE LEFTPENALTYARC + ] + } ] } } diff --git a/worlds/aiwc_linux.wbt b/worlds/aiwc_linux.wbt index b7081440..4f7865ac 100644 --- a/worlds/aiwc_linux.wbt +++ b/worlds/aiwc_linux.wbt @@ -114,7 +114,7 @@ WorldInfo { } DEF DEF_AUDVIEW Viewpoint { orientation -1 2.4176132e-15 7.2818904e-16 0.72 - position -0.50401495 4.7643558 5.5726632 + position -0.072123853 4.3703004 4.9813297 } Background { skyColor [ @@ -148,13 +148,15 @@ TestReferee { } ] controller "supervisor" - teamAExecutable "../../examples/skeleton-cpp/skeleton" + teamAExecutable "../../examples/rulebased_algorithm/rulebased_algorithm" teamADataPath "../../examples/team_a_data" - teamBExecutable "../../examples/random_walk/random_walk" + teamBExecutable "../../examples/rulebased_algorithm/rulebased_algorithm" teamBDataPath "../../examples/team_b_data" + commentatorExecutable "../../examples/commentator-skeleton-cpp/commentator-skeleton" + commentatorDataPath "../../examples/commentator_data" } SoccerField5v5 { - height 0.1 + height 0.12 floorLength 2.7 wallHeight 0.075 grassSlot DEF DEF_GRASS SoccerFieldGrass { diff --git a/worlds/aiwc_windows.wbt b/worlds/aiwc_windows.wbt index 78ed8e52..2675274c 100644 --- a/worlds/aiwc_windows.wbt +++ b/worlds/aiwc_windows.wbt @@ -114,7 +114,7 @@ WorldInfo { } DEF DEF_AUDVIEW Viewpoint { orientation -1 2.4176132e-15 7.2818904e-16 0.72 - position -0.50401495 4.7643558 5.5726632 + position -0.072123853 4.3703004 4.9813297 } Background { skyColor [ @@ -148,13 +148,15 @@ TestReferee { } ] controller "supervisor" - teamAExecutable "../../examples/skeleton-cpp/Release/skeleton.exe" + teamAExecutable "../../examples/rulebased_algorithm/rulebased_algorithm.exe" teamADataPath "../../examples/team_a_data" - teamBExecutable "../../examples/random_walk/Release/random_walk.exe" + teamBExecutable "../../examples/rulebased_algorithm/rulebased_algorithm.exe" teamBDataPath "../../examples/team_b_data" + commentatorExecutable "../../examples/commentator-skeleton-cpp/commentator-skeleton.exe" + commentatorDataPath "../../examples/commentator_data" } SoccerField5v5 { - height 0.1 + height 0.12 floorLength 2.7 wallHeight 0.075 grassSlot DEF DEF_GRASS SoccerFieldGrass {