diff --git a/README.md b/README.md index a083d55cb888cda3ffe26c512028914a9baf74fc..5e71fe045115183935980aaacfcf073a4ed94bd8 100644 --- a/README.md +++ b/README.md @@ -69,12 +69,16 @@ colcon build 新建一个终端，输入： ```Shell ros2 launch start start_launch.py + # 点球则运行 + # ros2 launch penalty_start start_launch.py ``` + 启动比赛控制器 新建一个终端，进入到`SEURoboCup2020`路径，输入： ```Shell ros2 run gamectrl gamectrl + # 点球则运行 + # ros2 run penalty_gamectrl gamectrl ``` + 启动机器人的控制节点 diff --git a/src/penalty_gamectrl/CMakeLists.txt b/src/penalty_gamectrl/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..bb525583b92c6ec68e1e2f610614eb5cb022a03c --- /dev/null +++ b/src/penalty_gamectrl/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.5) +project(penalty_gamectrl) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(common REQUIRED) +find_package(Qt5Widgets CONFIG REQUIRED) +find_package(Qt5Core CONFIG REQUIRED) +find_package(Qt5Gui CONFIG REQUIRED) +set(CMAKE_AUTOMOC ON) + +include_directories( + ${Qt5Widgets_INCLUDE_DIRS} +) + +add_executable(gamectrl src/gamectrl.cpp src/ctrlwindow.cpp) + +target_link_libraries(gamectrl + Qt5::Widgets + Qt5::Core + Qt5::Gui +) + +target_include_directories(gamectrl PUBLIC + $ + $) + +ament_target_dependencies( + gamectrl + "rclcpp" + "common" +) + +install(TARGETS gamectrl + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/penalty_gamectrl/package.xml b/src/penalty_gamectrl/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..beb108479782f15e5d16c56637358385ef0dced9 --- /dev/null +++ b/src/penalty_gamectrl/package.xml @@ -0,0 +1,21 @@ + + + + penalty_gamectrl + 0.0.0 + TODO: Package description + rocky + TODO: License declaration + + ament_cmake + + rclcpp + common + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/penalty_gamectrl/src/ctrlwindow.cpp b/src/penalty_gamectrl/src/ctrlwindow.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a1adb6c7df46b4366d1845e4a0016ba3726032c7 --- /dev/null +++ b/src/penalty_gamectrl/src/ctrlwindow.cpp @@ -0,0 +1,304 @@ +#include "ctrlwindow.hpp" +#include +#include +#include + +using namespace std; + +map states = { + {common::msg::GameData::STATE_INIT, "Init"}, + {common::msg::GameData::STATE_READY, "Ready"}, + {common::msg::GameData::STATE_PLAY, "Play"}, + {common::msg::GameData::STATE_PAUSE, "Pause"}, + {common::msg::GameData::STATE_END, "End"}, +}; + +TeamLabel::TeamLabel(QString name, TeamColor color) +{ + colorLabel = new QLabel(); + colorLabel->setFixedWidth(16); + QString cstr = color == TEAM_RED ? "background-color:red;" : "background-color:blue"; + colorLabel->setStyleSheet(cstr); + nameLabel = new QLabel(name); + nameLabel->setStyleSheet("font-size:20px;"); + scoreLabel = new QLabel("0"); + scoreLabel->setFixedWidth(30); + scoreLabel->setAlignment(Qt::AlignCenter); + scoreLabel->setStyleSheet("font-size:20px;"); + + QHBoxLayout *lay = new QHBoxLayout(); + lay->addWidget(colorLabel); + lay->addWidget(nameLabel); + lay->addWidget(scoreLabel); + this->setLayout(lay); +} + +StartDlg::StartDlg(std::string cfg) +{ + std::set teamSet; + ifstream ifs(cfg); + if(ifs) { + string line; + while(getline(ifs, line)) { + if(!line.empty() && line.back() == '\n') { + line.pop_back(); + } + if(!line.empty()) { + teamSet.insert(line); + } + line.clear(); + } + ifs.close(); + } + QStringList teams; + for (auto team : teamSet) { + teams << QString::fromStdString(team); + } + QVBoxLayout *leftLayout, *rightLayout; + leftLayout = new QVBoxLayout; + rightLayout = new QVBoxLayout; + QLabel *redLabel = new QLabel(); + redLabel->setStyleSheet("background-color: red;"); + QLabel *blueLabel = new QLabel(); + blueLabel->setStyleSheet("background-color: blue;"); + redTeamBox = new QComboBox(); + blueTeamBox = new QComboBox(); + redTeamBox->addItems(teams); + blueTeamBox->addItems(teams); + leftLayout->addWidget(redLabel); + leftLayout->addWidget(redTeamBox); + rightLayout->addWidget(blueLabel); + rightLayout->addWidget(blueTeamBox); + + startBtn = new QPushButton("Start"); + connect(startBtn, &QPushButton::clicked, this, &StartDlg::OnStart); + + QVBoxLayout *mainLayout = new QVBoxLayout; + QHBoxLayout *upLayout = new QHBoxLayout; + upLayout->addLayout(leftLayout); + upLayout->addLayout(rightLayout); + mainLayout->addLayout(upLayout); + mainLayout->addWidget(startBtn); + setLayout(mainLayout); +} + +void StartDlg::OnStart() +{ + redName = redTeamBox->currentText(); + blueName = blueTeamBox->currentText(); + + if(redName.size() == 0 || blueName.size() == 0) { + return; + } + if(redName == blueName) { + QMessageBox::warning(this, "Error", "Two teams is the same!"); + return; + } + this->close(); +} + +std::string g_redName, g_blueName; + +void GetColorService(const std::shared_ptr req, + std::shared_ptr res) +{ + if (req->team == g_redName) { + res->color = "red"; + } else if (req->team == g_blueName) { + res->color = "blue"; + } else { + res->color = "invalid"; + } + std::cout << "request: " << req->team << ' ' << res->color << std::endl; +} + +CtrlWindow::CtrlWindow(QString red, QString blue) +{ + redName = red; + blueName = blue; + g_redName = redName.toStdString(); + g_blueName = blueName.toStdString(); + std::cout << g_redName << ' ' << g_blueName << std::endl; + totalTime = 60 * 5; // 点球仅有 5 个，每个 1 分钟 + + QHBoxLayout *mainLayout = new QHBoxLayout(); + QVBoxLayout *leftLayout, *rightLayout; + leftLayout = new QVBoxLayout(); + rightLayout = new QVBoxLayout(); + + timeLabel = new QLabel(QString::number(totalTime)); + timeLabel->setAlignment(Qt::AlignCenter); + timeLabel->setStyleSheet("font-size:36px;"); + stateLabel = new QLabel("Init"); + stateLabel->setAlignment(Qt::AlignCenter); + stateLabel->setStyleSheet("font-size:36px;"); + infoLabel = new QLabel(); + + redTeam = new TeamLabel(red, TEAM_RED); + blueTeam = new TeamLabel(blue, TEAM_BLUE); + + + leftLayout->addWidget(timeLabel); + leftLayout->addWidget(stateLabel); + leftLayout->addWidget(infoLabel); + leftLayout->addWidget(redTeam); + leftLayout->addWidget(blueTeam); + + stateInitBtn = new QPushButton("Init"); + stateInitBtn->setMinimumHeight(40); + stateInitBtn->setStyleSheet("font-size:20px;"); + rightLayout->addWidget(stateInitBtn); + stateReadyBtn = new QPushButton("Ready"); + stateReadyBtn->setMinimumHeight(40); + stateReadyBtn->setStyleSheet("font-size:20px;"); + rightLayout->addWidget(stateReadyBtn); + statePlayBtn = new QPushButton("Play"); + statePlayBtn->setMinimumHeight(40); + statePlayBtn->setStyleSheet("font-size:20px;"); + rightLayout->addWidget(statePlayBtn); + statePauseBtn = new QPushButton("Pause"); + statePauseBtn->setMinimumHeight(40); + statePauseBtn->setStyleSheet("font-size:20px;"); + rightLayout->addWidget(statePauseBtn); + stateFinishBtn = new QPushButton("Finish"); + stateFinishBtn->setMinimumHeight(40); + stateFinishBtn->setStyleSheet("font-size:20px;"); + rightLayout->addWidget(stateFinishBtn); + + mainLayout->addLayout(leftLayout); + mainLayout->addLayout(rightLayout); + QWidget *mainWidget = new QWidget(); + mainWidget->setLayout(mainLayout); + setCentralWidget(mainWidget); + + fTimer = new QTimer(); + connect(fTimer, &QTimer::timeout, this, &CtrlWindow::OnFTimer); + connect(stateInitBtn, &QPushButton::clicked, this, &CtrlWindow::OnBtnInitClicked); + connect(stateReadyBtn, &QPushButton::clicked, this, &CtrlWindow::OnBtnReadyClicked); + connect(statePlayBtn, &QPushButton::clicked, this, &CtrlWindow::OnBtnPlayClicked); + connect(statePauseBtn, &QPushButton::clicked, this, &CtrlWindow::OnBtnPauseClicked); + connect(stateFinishBtn, &QPushButton::clicked, this, &CtrlWindow::OnBtnFinishClicked); + + fTimer->start(mBasicTime); + remainTime = kickstartTime = totalTime; + runTime = 0; + mGdata.mode = mGdata.MODE_KICK; + + mGdata.state = common::msg::GameData::STATE_INIT; + mGdata.red_players[0].name = "red_1"; + mGdata.blue_players[0].name = "blue_1"; + mGdata.remain_time = totalTime; + for (size_t i = 0; i < 1; i++) { + mGdata.red_players[i].state = common::msg::Player::PLAYER_NORMAL; + mGdata.blue_players[i].state = common::msg::Player::PLAYER_NORMAL; + } + gameDataPublisher = std::make_shared(); + fieldDataSubscriber = std::make_shared(); + teamColorServiceNode = rclcpp::Node::make_shared("gamectrl_GetColorSrv"); + getColorSrv = teamColorServiceNode->create_service("gamectrl/get_color", &GetColorService); +} + +void CtrlWindow::OnFTimer() +{ + std::map infos = { + {mFdata.BALL_OUT, "ball out of field"}, + {mFdata.BALL_NOMOVE, "ball does not move"}, + {mFdata.BALL_GOAL, "goal"}, + }; + rclcpp::spin_some(gameDataPublisher); + rclcpp::spin_some(fieldDataSubscriber); + rclcpp::spin_some(teamColorServiceNode); + mFdata = fieldDataSubscriber->GetData(); + if (mGdata.state == mGdata.STATE_PLAY) { + if (mFdata.ball_state != mFdata.BALL_NORMAL) { + mGdata.red_score = mFdata.red_score; + mGdata.blue_score = mFdata.blue_score; + mGdata.state = mGdata.STATE_PAUSE; + OnBtnPauseClicked(); + infoLabel->setText(infos[mFdata.ball_state]); + blueTeam->SetScore(mGdata.blue_score); + redTeam->SetScore(mGdata.red_score); + } + runTime += mBasicTime; + if(runTime % 1000 == 0) { + remainTime--; + for (size_t i = 0; i < 1; i++) { + if (mGdata.red_players[i].state == common::msg::Player::PLAYER_WAIT) { + if (mGdata.red_players[i].wait_time - remainTime > 30) { + mGdata.red_players[i].state = common::msg::Player::PLAYER_NORMAL; + } + } + if (mGdata.blue_players[i].state == common::msg::Player::PLAYER_WAIT) { + if (mGdata.blue_players[i].wait_time - remainTime > 30) { + mGdata.blue_players[i].state = common::msg::Player::PLAYER_NORMAL; + } + } + } + } + } + timeLabel->setText(QString::number(remainTime)); + if(kickstartTime - remainTime >= 60) { + OnBtnPauseClicked(); + } + if(remainTime <= 0) { + OnBtnFinishClicked(); + } + mGdata.remain_time = remainTime; + for (size_t i = 0; i < 1; i++) { + if (mFdata.red_players[i].state == common::msg::Player::PALYER_OUT) { + mGdata.red_players[i].state = common::msg::Player::PLAYER_WAIT; + mGdata.red_players[i].wait_time = remainTime; + } + if (mFdata.blue_players[i].state == common::msg::Player::PALYER_OUT) { + mGdata.blue_players[i].state = common::msg::Player::PLAYER_WAIT; + mGdata.blue_players[i].wait_time = remainTime; + } + } + gameDataPublisher->Publish(mGdata); +} + +void CtrlWindow::OnBtnInitClicked() +{ + mGdata.state = common::msg::GameData::STATE_INIT; + for (size_t i = 0; i < 1; i++) { + mGdata.red_players[i].state = common::msg::Player::PLAYER_NORMAL; + mGdata.blue_players[i].state = common::msg::Player::PLAYER_NORMAL; + } + stateLabel->setText(QString::fromStdString(states[mGdata.state])); + infoLabel->setText(""); + remainTime = remainTime / 60 * 60; // Init 后把上一个点球的剩余时间扣除 + kickstartTime = remainTime; +} + +void CtrlWindow::OnBtnReadyClicked() +{ + mGdata.state = common::msg::GameData::STATE_READY; + stateLabel->setText(QString::fromStdString(states[mGdata.state])); +} + +void CtrlWindow::OnBtnPlayClicked() +{ + mGdata.state = common::msg::GameData::STATE_PLAY; + stateLabel->setText(QString::fromStdString(states[mGdata.state])); +} + +void CtrlWindow::OnBtnPauseClicked() +{ + mGdata.state = common::msg::GameData::STATE_PAUSE; + stateLabel->setText(QString::fromStdString(states[mGdata.state])); +} + +void CtrlWindow::OnBtnFinishClicked() +{ + mGdata.state = common::msg::GameData::STATE_END; + stateLabel->setText(QString::fromStdString(states[mGdata.state])); + QString endTime = QTime::currentTime().toString("HH:mm:ss"); + ofstream ofs("results.txt", ios::out | ios::app); + if(!ofs) { + return; + } + ofs << startTime.toStdString() << " --- " << endTime.toStdString() << endl; + ofs << redName.toStdString() << " : " << blueName.toStdString() << "\t" << mFdata.red_score << " : " << mFdata.blue_score << endl; + ofs << endl; + ofs.close(); +} \ No newline at end of file diff --git a/src/penalty_gamectrl/src/ctrlwindow.hpp b/src/penalty_gamectrl/src/ctrlwindow.hpp new file mode 100644 index 0000000000000000000000000000000000000000..b58003cad80468a04d9b1136cddfc5e2dd14baa6 --- /dev/null +++ b/src/penalty_gamectrl/src/ctrlwindow.hpp @@ -0,0 +1,128 @@ +#pragma once + +#include +#include +#include +#include +#include + +enum TeamColor +{ + TEAM_RED, + TEAM_BLUE +}; + +class TeamLabel: public QWidget +{ +Q_OBJECT +public: + TeamLabel(QString name, TeamColor color); + QString Name() + { + return nameLabel->text(); + } + void SetScore(int s) + { + scoreLabel->setText(QString::number(s)); + } + void SetName(QString name) + { + nameLabel->setText(name); + } +private: + QLabel *colorLabel; + QLabel *nameLabel; + QLabel *scoreLabel; +}; + +class StartDlg: public QDialog +{ +Q_OBJECT +public: + StartDlg(std::string cfg); + QString redName, blueName; +public slots: + void OnStart(); + +private: + QComboBox *redTeamBox, *blueTeamBox; + QPushButton *startBtn; +}; + +class FieldDataSubscriber: public rclcpp::Node +{ +public: + FieldDataSubscriber(): Node("field_data_subscriber") + { + subscription_ = this->create_subscription( + "/sensor/field", 5, std::bind(&FieldDataSubscriber::topic_callback, this, + std::placeholders::_1)); + } + + const common::msg::FieldData& GetData() + { + return fieldData_; + } + +private: + void topic_callback(const common::msg::FieldData::SharedPtr msg) + { + fieldData_ = *msg; + } + + common::msg::FieldData fieldData_; + rclcpp::Subscription::SharedPtr subscription_; +}; + +class GameDataPublisher : public rclcpp::Node +{ +public: + GameDataPublisher(): Node("game_publisher") + { + publisher_ = this->create_publisher("/sensor/game", 5); + } + + void Publish(const common::msg::GameData& data) + { + publisher_->publish(data); + } + + rclcpp::Publisher::SharedPtr publisher_; +}; + +class CtrlWindow: public QMainWindow +{ +Q_OBJECT +public: + CtrlWindow(QString red, QString blue); + +public slots: + void OnFTimer(); + void OnBtnInitClicked(); + void OnBtnReadyClicked(); + void OnBtnPlayClicked(); + void OnBtnPauseClicked(); + void OnBtnFinishClicked(); + +private: + QLabel *timeLabel, *stateLabel, *infoLabel; + TeamLabel *redTeam, *blueTeam; + QTimer *fTimer; + + QPushButton *stateInitBtn, *stateReadyBtn, *statePlayBtn, *statePauseBtn, *stateFinishBtn; + + common::msg::GameData mGdata; + common::msg::FieldData mFdata; + std::shared_ptr gameDataPublisher; + std::shared_ptr fieldDataSubscriber; + std::shared_ptr teamColorServiceNode; + std::shared_ptr> getColorSrv; + + QString startTime, goalTime; + QString redName, blueName; + int runTime; + int remainTime; + int totalTime; + const int mBasicTime = 20; + int kickstartTime; // 一个点球的开始时间 +}; diff --git a/src/penalty_gamectrl/src/gamectrl.cpp b/src/penalty_gamectrl/src/gamectrl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8788f8ba5fb3237f78638dcacded38e967947e03 --- /dev/null +++ b/src/penalty_gamectrl/src/gamectrl.cpp @@ -0,0 +1,21 @@ +#include +#include +#include "ctrlwindow.hpp" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + QApplication app(argc, argv); + std::string cfg = "teams.cfg"; + if(argc > 1) { + cfg = std::string(argv[1]); + } + StartDlg sdlg(cfg); + sdlg.exec(); + if(sdlg.redName.size() > 0) { + CtrlWindow foo(sdlg.redName, sdlg.blueName); + foo.show(); + return app.exec(); + } + return 0; +} \ No newline at end of file diff --git a/src/penalty_start/CMakeLists.txt b/src/penalty_start/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..1a0ced0e6553e30b0fe2156a900c2c6cce24b162 --- /dev/null +++ b/src/penalty_start/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(penalty_start) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install(FILES start_launch.py + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/penalty_start/package.xml b/src/penalty_start/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..acac0796c4eb3a6a4e377ebb7bee7a58afd5ab21 --- /dev/null +++ b/src/penalty_start/package.xml @@ -0,0 +1,18 @@ + + + + penalty_start + 0.0.0 + TODO: Package description + rocky + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/penalty_start/start_launch.py b/src/penalty_start/start_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..e0e8dd1ae727e30c6a49f69a22849847039612f1 --- /dev/null +++ b/src/penalty_start/start_launch.py @@ -0,0 +1,54 @@ +import os +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from webots_ros2_core.webots_launcher import WebotsLauncher +from webots_ros2_core.utils import ControllerLauncher +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + webots = WebotsLauncher( + world=os.path.join(get_package_share_directory('webots'), 'models/worlds', 'penalty-robot.wbt') + ) + synchronization = launch.substitutions.LaunchConfiguration('synchronization', default=False) + red_1 = ControllerLauncher(package='penalty_controller', + executable='controller', + parameters=[{'synchronization': synchronization}], + arguments=['red_1'], + output='screen') + blue_1 = ControllerLauncher(package='penalty_controller', + executable='controller', + parameters=[{'synchronization': synchronization}], + arguments=['blue_1'], + output='screen') + supervisor = ControllerLauncher(package='penalty_controller', + executable='supervisor', + parameters=[{'synchronization': synchronization}], + output='screen') + return LaunchDescription([ + webots, + red_1, + blue_1, + supervisor, + Node( + package='params', + executable='params' + ), + Node( + package='motion', + executable='motion', + arguments=['red_1'] + ), + Node( + package='motion', + executable='motion', + arguments=['blue_1'] + ), + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=webots, + on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + ) + ) + ]) \ No newline at end of file diff --git a/src/simulation/penalty_controller/CMakeLists.txt b/src/simulation/penalty_controller/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..241e04d9cc228ddfd495c6769f708f99c11bd68c --- /dev/null +++ b/src/simulation/penalty_controller/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.5) +project(penalty_controller) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(common REQUIRED) +find_package(seumath REQUIRED) +find_package(sensor_msgs REQUIRED) + +set(WEBOTS_HOME /usr/local/webots) + +include_directories( + ${WEBOTS_HOME}/include/controller/cpp + ${Qt5Widgets_INCLUDE_DIRS} +) + +link_directories( + ${WEBOTS_HOME}/lib/controller +) + +add_executable(controller src/controller.cpp src/SimRobot.cpp) + +add_executable(supervisor src/supervisor.cpp) + +target_link_libraries(controller + CppController +) + +target_link_libraries(supervisor + CppController +) + +target_include_directories(controller PUBLIC + $ + $) + +ament_target_dependencies( + controller + "rclcpp" + "sensor_msgs" + "common" + "seumath" +) + +target_include_directories(supervisor PUBLIC + $ + $) + +ament_target_dependencies( + supervisor + "rclcpp" + "sensor_msgs" + "common" + "seumath" +) + +install(TARGETS controller + DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS supervisor + DESTINATION lib/${PROJECT_NAME}) + +install(FILES controller_launch.py + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/simulation/penalty_controller/controller_launch.py b/src/simulation/penalty_controller/controller_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..3dbd8d852e5af8ff9722203612784724f1d1df32 --- /dev/null +++ b/src/simulation/penalty_controller/controller_launch.py @@ -0,0 +1,27 @@ +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from webots_ros2_core.utils import ControllerLauncher + + +def generate_launch_description(): + synchronization = launch.substitutions.LaunchConfiguration('synchronization', default=False) + red_1 = ControllerLauncher(package='controller', + executable='controller', + parameters=[{'synchronization': synchronization}], + arguments=['red_1'], + output='screen') + blue_1 = ControllerLauncher(package='controller', + executable='controller', + parameters=[{'synchronization': synchronization}], + arguments=['blue_1'], + output='screen') + supervisor = ControllerLauncher(package='controller', + executable='supervisor', + parameters=[{'synchronization': synchronization}], + output='screen') + return LaunchDescription([ + red_1, + blue_1, + supervisor + ]) \ No newline at end of file diff --git a/src/simulation/penalty_controller/package.xml b/src/simulation/penalty_controller/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..33d48c5718bf38d86a61854d289aaffdc3a06a53 --- /dev/null +++ b/src/simulation/penalty_controller/package.xml @@ -0,0 +1,23 @@ + + + + penalty_controller + 0.0.0 + TODO: Package description + rocky + TODO: License declaration + + ament_cmake + + rclcpp + common + seumath + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/simulation/penalty_controller/src/SimRobot.cpp b/src/simulation/penalty_controller/src/SimRobot.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8868d370301c78a255f0dde987c5af8b5cae3137 --- /dev/null +++ b/src/simulation/penalty_controller/src/SimRobot.cpp @@ -0,0 +1,133 @@ +#include +#include +#include +#include "SimRobot.hpp" + +using namespace std; +using namespace webots; +using namespace common::msg; + +const char *Neck[2] = { "Neck", "Neck2" }; +const char *LeftArmJoint[2] = { "LeftShoulder", "LeftElbow" }; +const char *RightArmJoint[2] = { "RightShoulder", "RightElbow" }; +const char *LeftLegJoint[6] = { "LeftLegX", "LeftLegY", "LeftLegZ", "LeftKnee", "LeftAnkleX", "LeftAnkleY" }; +const char *RightLegJoint[6] = { "RightLegX", "RightLegY", "RightLegZ", "RightKnee", "RightAnkleX", "RightAnkleY" }; +const double fall_thresh = 0.75; + +SimRobot::SimRobot(std::string robot_name) : Robot(), robotName(robot_name) +{ + totalTime = 0; + mTimeStep = getBasicTimeStep(); + mCamera = getCamera("Camera"); + mCamera->enable(5 * mTimeStep); + mIMU = getInertialUnit("IMU"); + mIMU->enable(mTimeStep); + mLEDs.resize(2); + mLEDs[0] = getLED("Led0"); + mLEDs[1] = getLED("Led1"); + + fallType = ImuData::FALL_NONE; + mInitYaw = 0.0; + imuReset = true; + mHAngles.yaw = 0.0; + mHAngles.pitch = 0.0; + ledTask.led1 = 1; + ledTask.led2 = 1; + + mImagePublisher = std::make_shared(robotName); + mImuPublisher = std::make_shared(robotName); + mHeadPublisher = std::make_shared(robotName); +} + +int SimRobot::myStep() +{ + mLEDs[0]->set(0xff0000*ledTask.led1); + mLEDs[1]->set(0xff0000*ledTask.led2); + setPositions(); + checkFall(); + totalTime += mTimeStep; + if (totalTime % (5 * mTimeStep) == 0) { + mImagePublisher->Publish(mCamera->getImage(), mCamera->getWidth(), mCamera->getHeight()); + } + mHeadPublisher->Publish(mHAngles); + rclcpp::spin_some(mImagePublisher); + rclcpp::spin_some(mImuPublisher); + rclcpp::spin_some(mHeadPublisher); + return step(mTimeStep); +} + +void SimRobot::checkFall() +{ + const double *rpy = mIMU->getRollPitchYaw(); + if (rpy[1] > fall_thresh) + fallType = ImuData::FALL_BACKWARD; + else if (rpy[1] < -fall_thresh) + fallType = ImuData::FALL_FORWARD; + else if (rpy[0] < -fall_thresh) + fallType = ImuData::FALL_LEFT; + else if (rpy[0] > fall_thresh) + fallType = ImuData::FALL_RIGHT; + else + fallType = ImuData::FALL_NONE; + ImuData imu; + imu.yaw = seumath::rad2deg(rpy[2]); + imu.pitch = seumath::rad2deg(rpy[1]); + imu.roll = seumath::rad2deg(rpy[0]); + imu.fall = fallType; + imu.stamp = rclcpp::Time().nanoseconds(); + mImuPublisher->Publish(imu); +} + +void SimRobot::wait(int ms) +{ + double startTime = getTime(); + double s = (double)ms / 1000.0; + while (s + startTime >= getTime()) + myStep(); +} + +void SimRobot::setPositions() +{ + Motor *motor; + motor = getMotor(LeftLegJoint[0]); + motor->setPosition(seumath::deg2rad(mAngles.left_hip_roll)); + motor = getMotor(LeftLegJoint[1]); + motor->setPosition(seumath::deg2rad(mAngles.left_hip_pitch)); + motor = getMotor(LeftLegJoint[2]); + motor->setPosition(seumath::deg2rad(-mAngles.left_hip_yaw)); + motor = getMotor(LeftLegJoint[3]); + motor->setPosition(seumath::deg2rad(mAngles.left_knee)); + motor = getMotor(LeftLegJoint[4]); + motor->setPosition(seumath::deg2rad(mAngles.left_ankle_roll)); + motor = getMotor(LeftLegJoint[5]); + motor->setPosition(seumath::deg2rad(mAngles.left_ankle_pitch)); + + motor = getMotor(RightLegJoint[0]); + motor->setPosition(seumath::deg2rad(mAngles.right_hip_roll)); + motor = getMotor(RightLegJoint[1]); + motor->setPosition(seumath::deg2rad(mAngles.right_hip_pitch)); + motor = getMotor(RightLegJoint[2]); + motor->setPosition(seumath::deg2rad(-mAngles.right_hip_yaw)); + motor = getMotor(RightLegJoint[3]); + motor->setPosition(seumath::deg2rad(mAngles.right_knee)); + motor = getMotor(RightLegJoint[4]); + motor->setPosition(seumath::deg2rad(mAngles.right_ankle_roll)); + motor = getMotor(RightLegJoint[5]); + motor->setPosition(seumath::deg2rad(mAngles.right_ankle_pitch)); + + motor = getMotor(LeftArmJoint[0]); + motor->setPosition(seumath::deg2rad(mAngles.left_shoulder)); + motor = getMotor(LeftArmJoint[1]); + motor->setPosition(seumath::deg2rad(mAngles.left_elbow)); + motor = getMotor(RightArmJoint[0]); + motor->setPosition(seumath::deg2rad(-mAngles.right_shoulder)); + motor = getMotor(RightArmJoint[1]); + motor->setPosition(seumath::deg2rad(-mAngles.right_elbow)); + + motor = getMotor(Neck[1]); + motor->setPosition(seumath::deg2rad(-mHAngles.pitch)); + motor = getMotor(Neck[0]); + motor->setPosition(seumath::deg2rad(mHAngles.yaw)); +} + + diff --git a/src/simulation/penalty_controller/src/SimRobot.hpp b/src/simulation/penalty_controller/src/SimRobot.hpp new file mode 100644 index 0000000000000000000000000000000000000000..691d553ffca009cd1792bd3e4e130a0b4edc4685 --- /dev/null +++ b/src/simulation/penalty_controller/src/SimRobot.hpp @@ -0,0 +1,118 @@ +#ifndef __UNIROBOT_HPP +#define __UNIROBOT_HPP + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +class ImagePublisher : public rclcpp::Node +{ +public: + ImagePublisher(std::string robot_name): Node(robot_name + "_image_publisher") + { + publisher_ = this->create_publisher(robot_name + "/sensor/image", 5); + } + + void Publish(const unsigned char* data, int w, int h) + { + auto message = sensor_msgs::msg::Image(); + message.header.stamp = rclcpp::Time(); + message.header.frame_id = "simulation"; + message.width = w; + message.height = h; + message.step = w * 3; + message.encoding = sensor_msgs::image_encodings::RGB8; + message.data.resize(w * h * 3); + for (int i = 0; i < h; i++) { + for (int j = 0; j < w; j++) { + message.data[i * w * 3 + j * 3 + 0] = *(data + i * w * 4 + j * 4 + 2); + message.data[i * w * 3 + j * 3 + 1] = *(data + i * w * 4 + j * 4 + 1); + message.data[i * w * 3 + j * 3 + 2] = *(data + i * w * 4 + j * 4 + 0); + } + } + publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr publisher_; +}; + +class ImuPublisher : public rclcpp::Node +{ +public: + ImuPublisher(std::string robot_name): Node(robot_name + "_imu_publisher") + { + publisher_ = this->create_publisher(robot_name + "/sensor/imu", 5); + } + + void Publish(const common::msg::ImuData& data) + { + publisher_->publish(data); + } + + rclcpp::Publisher::SharedPtr publisher_; +}; + +class HeadAnglePublisher : public rclcpp::Node +{ +public: + HeadAnglePublisher(std::string robot_name): Node(robot_name + "_head_angle_publisher") + { + publisher_ = this->create_publisher(robot_name + "/sensor/joint/head", 5); + } + + void Publish(const common::msg::HeadAngles& data) + { + publisher_->publish(data); + } + + rclcpp::Publisher::SharedPtr publisher_; +}; + +class SimRobot: public webots::Robot +{ +public: + SimRobot(std::string robot_name="maxwell"); + int myStep(); + + common::msg::BodyAngles mAngles; + common::msg::HeadAngles mHAngles; + +private: + int mTimeStep; + int totalTime; + + std::shared_ptr mImagePublisher; + std::shared_ptr mImuPublisher; + std::shared_ptr mHeadPublisher; + + void setPositions(); + void checkFall(); + + void wait(int ms); + + webots::Camera *mCamera; + webots::InertialUnit *mIMU; + std::vector mLEDs; + + common::msg::LedTask ledTask; + int fallType; + bool imuReset; + double mInitYaw; + std::string robotName; +}; + + +#endif diff --git a/src/simulation/penalty_controller/src/controller.cpp b/src/simulation/penalty_controller/src/controller.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6d7153900ab6545b70fe2c30893cbfce7eafddf7 --- /dev/null +++ b/src/simulation/penalty_controller/src/controller.cpp @@ -0,0 +1,73 @@ +#include +#include +#include +#include +#include + +#include "SimRobot.hpp" +#include + + +using namespace std; +using namespace common; +using namespace seumath; +using namespace Eigen; + +int main(int argc, char **argv) +{ + string robotName = "maxwell"; + if (argc > 1) { + robotName = std::string(argv[1]); + } + setenv("WEBOTS_ROBOT_NAME", robotName.c_str(), 0); + rclcpp::init(argc, argv); + + bool ok=false; + int i=0; + std::shared_ptr node = rclcpp::Node::make_shared(robotName + "_controller"); + while(rclcpp::ok() && i++<20) { + FILE *stream; + char buf[1024]; + memset( buf, '\0', sizeof(buf) );//初始化buf + stream = popen( "ls /tmp | grep webots-", "r" ); + fread( buf, sizeof(char), sizeof(buf), stream); //将刚刚FILE* stream的数据流读取到buf中 + pclose( stream ); + string sbuf(buf); + if(sbuf.find("webots") != string::npos) { + string pf = "/tmp/"+sbuf.substr(0, sbuf.find("\n"))+"/WEBOTS_SERVER"; + RCLCPP_WARN(node->get_logger(), "%s", pf.c_str()); + if(access(pf.c_str(), F_OK)!=-1) { + ok = true; + break; + } + } + RCLCPP_WARN(node->get_logger(), "waiting for webots ......"); + usleep(1000000); + } + if(!ok) return 0; + + std::shared_ptr player = std::make_shared(robotName); + + rclcpp::Client::SharedPtr client = + node->create_client(robotName + "/get_angles"); + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(node->get_logger(), "service not available, waiting again..."); + } + int ret = 0; + while (rclcpp::ok() && ret >= 0) { + auto request = std::make_shared(); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + player->mAngles = result.get()->body; + player->mHAngles = result.get()->head; + } + ret = player->myStep(); + + } + return 0; +} + diff --git a/src/simulation/penalty_controller/src/supervisor.cpp b/src/simulation/penalty_controller/src/supervisor.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e5410f06c577bb24ed57944181d60e73cb845756 --- /dev/null +++ b/src/simulation/penalty_controller/src/supervisor.cpp @@ -0,0 +1,256 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +struct ObjectInfo { + Eigen::Vector3d translation; + Eigen::Vector4d rotation; +}; + +const double ballR = 0.07; +const double robotR = 0.2; +const double fieldX = 4.5 + ballR; +const double fieldZ = 3.0 + ballR; + +double Sign(double v) +{ + return v < 0 ? -1 : 1; +} + +bool IsEqual(double v1, double v2) +{ + return fabs(v1 - v2) < 1E-2; +} + +bool BallMoved(const double *p1, const double *p2) +{ + return !(IsEqual(p1[0], p2[0]) && IsEqual(p1[2], p2[2])); +} + +bool IsOutOfField(const double *pos) +{ + if ((fabs(pos[0]) > fieldX + robotR) || (fabs(pos[2]) > fieldZ + robotR)) { + return true; + } else { + return false; + } +} + +class FieldDataPublisher : public rclcpp::Node +{ +public: + FieldDataPublisher(): Node("field_data_publisher") + { + publisher_ = this->create_publisher("/sensor/field", 5); + } + + void Publish(const common::msg::FieldData& data) + { + publisher_->publish(data); + } + + rclcpp::Publisher::SharedPtr publisher_; +}; + +class GameDataSubscriber: public rclcpp::Node +{ +public: + GameDataSubscriber(std::string robotName): Node(robotName + "_game_data_subscriber") + { + subscription_ = this->create_subscription( + "/sensor/game", 5, std::bind(&GameDataSubscriber::topic_callback, this, + std::placeholders::_1)); + } + + const common::msg::GameData& GetData() + { + return gameData_; + } + +private: + void topic_callback(const common::msg::GameData::SharedPtr msg) + { + gameData_ = *msg; + } + + common::msg::GameData gameData_; + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char **argv) +{ + const int basicTime = 20; + const double goalX = 4.5 + ballR; + const double goalZ = 0.7 + ballR; + const double ballPosInit[3] = {-3.25, ballR, 0.0}; + const double zeroVel[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + ObjectInfo redInitInfos[] = { + {Eigen::Vector3d(-2.5, 0.365, 0.0), Eigen::Vector4d(0, 1, 0, M_PI)} + }; + ObjectInfo blueInitInfos[] = { + {Eigen::Vector3d(-4, 0.365, 0.0), Eigen::Vector4d(0, 1, 0, 0)} + }; + + ObjectInfo redWaitInfos[] = { + {Eigen::Vector3d(0.75, 0.365, -3.0), Eigen::Vector4d(0, 1, 0, -M_PI / 2)} + }; + ObjectInfo blueWaitInfos[] = { + {Eigen::Vector3d(-0.75, 0.365, -3.0), Eigen::Vector4d(0, 1, 0, -M_PI / 2)} + }; + + setenv("WEBOTS_ROBOT_NAME", "judge", 0); + rclcpp::init(argc, argv); + auto judgeNode = make_shared("judge"); + auto fieldPublisher = make_shared(); + auto gameSubscriber = make_shared("judge"); + bool ok = false; + int i = 0; + + while (rclcpp::ok() && i++ < 20) { + FILE *stream; + char buf[1024]; + memset(buf, '\0', sizeof(buf)); //初始化buf + stream = popen("ls /tmp | grep webots-", "r"); + fread(buf, sizeof(char), sizeof(buf), stream); //将刚刚FILE* stream的数据流读取到buf中 + pclose(stream); + string sbuf(buf); + + if (sbuf.find("webots") != string::npos) { + string pf = "/tmp/" + sbuf.substr(0, sbuf.find("\n")) + "/WEBOTS_SERVER"; + RCLCPP_WARN(judgeNode->get_logger(), "%s", pf.c_str()); + + if (access(pf.c_str(), F_OK) != -1) { + ok = true; + break; + } + } + + RCLCPP_WARN(judgeNode->get_logger(), "waiting for webots ......"); + usleep(1000000); + } + + if (!ok) { + return 0; + } + + common::msg::GameData gameData; + common::msg::FieldData fieldData; + std::shared_ptr super = make_shared(); + webots::Node *ball = super->getFromDef("Ball"); + webots::Node *red_1 = super->getFromDef("red_1"); + webots::Node *blue_1 = super->getFromDef("blue_1"); + webots::Node *redPlayers[1] = {red_1}; + webots::Node *bluePlayers[1] = {blue_1}; + int ballMoveCnt = 0; + double lastBallPos[3] = {0}; + auto lastGoalTime = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + fieldData.red_players[0].name = "red_1"; + fieldData.blue_players[0].name = "blue_1"; + bool initSet = false; + bool redWaitSet[2] = {false, false}; + bool blueWaitSet[2] = {false, false}; + + while (super->step(basicTime) != -1 && rclcpp::ok()) { + rclcpp::spin_some(judgeNode); + rclcpp::spin_some(fieldPublisher); + rclcpp::spin_some(gameSubscriber); + gameData = gameSubscriber->GetData(); + if (gameData.state == gameData.STATE_INIT) { + ballMoveCnt = 0; + fieldData.ball_state = fieldData.BALL_NORMAL; + fieldData.red_players[0].state = common::msg::Player::PLAYER_NORMAL; + fieldData.blue_players[0].state = common::msg::Player::PLAYER_NORMAL; + if (!initSet) { + ball->setVelocity(zeroVel); + ball->getField("translation")->setSFVec3f(ballPosInit); + for (size_t i = 0; i < 1; i++) { + redPlayers[i]->getField("translation")->setSFVec3f(redInitInfos[i].translation.data()); + redPlayers[i]->getField("rotation")->setSFRotation(redInitInfos[i].rotation.data()); + redPlayers[i]->setVelocity(zeroVel); + bluePlayers[i]->getField("translation")->setSFVec3f(blueInitInfos[i].translation.data()); + bluePlayers[i]->getField("rotation")->setSFRotation(blueInitInfos[i].rotation.data()); + bluePlayers[i]->setVelocity(zeroVel); + } + initSet = true; + } + } else if (gameData.state == gameData.STATE_PLAY) { + initSet = false; + for (size_t i = 0; i < 1; i++) { + if (gameData.red_players[i].state == common::msg::Player::PLAYER_WAIT) { + if (!redWaitSet[i]) { + redWaitSet[i] = true; + redPlayers[i]->getField("translation")->setSFVec3f(redWaitInfos[i].translation.data()); + redPlayers[i]->getField("rotation")->setSFRotation(redWaitInfos[i].rotation.data()); + redPlayers[i]->setVelocity(zeroVel); + } + } else { + redWaitSet[i] = false; + } + if (gameData.blue_players[i].state == common::msg::Player::PLAYER_WAIT) { + if (!blueWaitSet[i]) { + blueWaitSet[i] = true; + bluePlayers[i]->getField("translation")->setSFVec3f(blueWaitInfos[i].translation.data()); + bluePlayers[i]->getField("rotation")->setSFRotation(blueWaitInfos[i].rotation.data()); + bluePlayers[i]->setVelocity(zeroVel); + } + } else { + blueWaitSet[i] = false; + } + } + + fieldData.red_players[0].state = IsOutOfField(red_1->getPosition()) ? + common::msg::Player::PALYER_OUT : common::msg::Player::PLAYER_NORMAL; + fieldData.blue_players[0].state = IsOutOfField(blue_1->getPosition()) ? + common::msg::Player::PALYER_OUT : common::msg::Player::PLAYER_NORMAL; + + const double *ballPos = ball->getPosition(); + bool goal = false; + auto currTime = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + + if (fabs(ballPos[2]) < goalZ) { + if (ballPos[0] < -goalX) { + if (currTime - lastGoalTime > 10) { + fieldData.red_score++; + } + goal = true; + } else if (ballPos[0] > goalX) { + if (currTime - lastGoalTime > 10) { + fieldData.blue_score++; + } + goal = true; + } + } + if (goal) { + lastGoalTime = currTime; + fieldData.ball_state = fieldData.BALL_GOAL; + } else { + if (fabs(ballPos[2]) > fieldZ || fabs(ballPos[0]) > fieldX) { + fieldData.ball_state = fieldData.BALL_OUT; + } else { + if (BallMoved(lastBallPos, ballPos)) { + ballMoveCnt = 0; + memcpy(lastBallPos, ballPos, 3 * sizeof(double)); + } else { + ballMoveCnt++; + } + + if (ballMoveCnt * basicTime > 60 * 1000) { + fieldData.ball_state = fieldData.BALL_NOMOVE; + } else { + fieldData.ball_state = fieldData.BALL_NORMAL; + } + } + } + } + fieldPublisher->Publish(fieldData); + } + + return 0; +} diff --git a/src/simulation/webots/models/worlds/penalty-robot.wbt b/src/simulation/webots/models/worlds/penalty-robot.wbt new file mode 100644 index 0000000000000000000000000000000000000000..d53c0a018c3d3e3021ed7ac1104bd552833d98fb --- /dev/null +++ b/src/simulation/webots/models/worlds/penalty-robot.wbt @@ -0,0 +1,71 @@ +#VRML_SIM R2019b utf8 +WorldInfo { + basicTimeStep 20 +} +Viewpoint { + orientation -0.28700894453019526 0.9447904284346113 0.15810411790338322 1.2004086600611599 + position 4.691031590576385 1.8262293677394228 2.6422202200027587 +} +TexturedBackground { + texture "noon_cloudy_countryside" +} +DirectionalLight { + ambientIntensity 1 + direction 5 -0.5 0 + intensity 2 +} +DirectionalLight { + ambientIntensity 1 + direction -5 -0.5 0 + intensity 2 +} +DirectionalLight { + ambientIntensity 1 + direction 0 -0.5 4 + intensity 2 +} +DirectionalLight { + ambientIntensity 1 + direction 0 -0.5 -4 + intensity 2 +} +SoccerField { + frame1Color 0.6 0.6 0.6 + frame2Color 0.6 0.6 0.6 +} +DEF Ball SoccerBall { + translation -3.25 0.06999999999999999 0 + radius 0.07 + mass 0.5 + linearDamping 0.5 + angularDamping 0.5 + faceUrl [ + "textures/blackball.jpg" + ] +} + +DEF red_1 SEURobot { + translation -2.5 0.365 0 + rotation 0 1 0 3.1415926 + robotColor 0.8 0 0 + name "red_1" + controller "" + numberUrl [ + "textures/number1.png" + ] +} +DEF blue_1 SEURobot { + translation -4 0.365 0 + rotation 0 1 0 0 + robotColor 0 0 0.8 + name "blue_1" + controller "" + numberUrl [ + "textures/number1.png" + ] +} +DEF Judge Robot { + name "judge" + controller "" + supervisor TRUE +} \ No newline at end of file diff --git a/src/template/src/README.md b/src/template/src/README.md index 753db31356b6d28d6119682b56d492ac3f79aa96..41a683e514607a696796efbfa88ebb66c075aecd 100644 --- a/src/template/src/README.md +++ b/src/template/src/README.md @@ -56,5 +56,15 @@ float64 pitch gameData可用于获取比赛情况，允许调用的数据有： - `red_score` 和 `blue_score`：两队得分 - `state`：目前比赛状态，包括 `STATE_INIT`，`STATE_READY`，`STATE_PLAY`，`STATE_PAUSE`，`STATE_END`。 +- `mode`：当前比赛模式，`MODE_NORM` 表示 一般模式，`MODE_KICK` 表示 点球模式 + +例如： +```cpp +if (gameData.mode == gameData.MODE_KICK && myColor == COLOR_BLUE) { + // 我是点球模式的守门员！ +} else if (gameData.mode == gameData.MODE_NORM && myColor == COLOR_BLUE && myId == 1) { + // 我是一般模式的蓝1机器人！ +} +``` 不允许调用其它接口，尤其是不允许调用函数。一旦发现立即取消成绩。 \ No newline at end of file diff --git a/src/template/src/player.cpp b/src/template/src/player.cpp index 48db9d7dd869f459f20e911107d5f1f598e70d8d..7eb03b3065734898268af4f0538d8161019142dd 100644 --- a/src/template/src/player.cpp +++ b/src/template/src/player.cpp @@ -124,17 +124,12 @@ int main(int argc, char ** argv) } else if (myId == 2) { // 2号机器人 } - + if (!image.empty()) { // 在这里写图像处理 cv::circle(image, cv::Point(0, 0), 40, cv::Scalar(255, 0, 0)); resImgPublisher->Publish(image); // 处理完的图像可以通过该方式发布出去，然后通过rqt中的image_view工具查看 } - // write your code here - if (robotName.back() == '1') - { - btask.step = -1; // 1 号机器人后退 - } // 郑重声明：以上说明仅供参考，实际情况以实际为准 // ----------------- 可以修改的部分 end--------------------