From 67fcf0654262ce037d58cb64a89cb5d430f5e789 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 21:36:35 +0900 Subject: [PATCH 01/15] =?UTF-8?q?BallPlacementWithSkillPlanner=E3=82=92?= =?UTF-8?q?=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/planners.hpp | 3 - .../ball_placement_with_skill_planner.hpp | 181 ------------------ 2 files changed, 184 deletions(-) delete mode 100644 session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp index 53f32d88d..9ef7ec6a6 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp @@ -19,7 +19,6 @@ #include "receive_planner.hpp" #include "skill_planner.hpp" #include "temporary/ball_placement_planner.hpp" -#include "temporary/ball_placement_with_skill_planner.hpp" #include "tigers_goalie_planner.hpp" #include "waiter_planner.hpp" @@ -32,8 +31,6 @@ auto generatePlanner(const std::string & planner_name, Ts... ts) -> PlannerBase: return std::make_unique(ts...); } else if (planner_name == "ball_placement") { return std::make_unique(ts...); - } else if (planner_name == "ball_placement_with_skill") { - return std::make_unique(ts...); } else if (planner_name == "ball_placement_skill") { return std::make_unique(ts...); } else if (planner_name == "defender") { diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp deleted file mode 100644 index 39b99d45b..000000000 --- a/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp +++ /dev/null @@ -1,181 +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. - -#ifndef CRANE_PLANNER_PLUGINS__TEMPORARY__BALL_PLACEMENT_WITH_SKILL_PLANNER_HPP_ -#define CRANE_PLANNER_PLUGINS__TEMPORARY__BALL_PLACEMENT_WITH_SKILL_PLANNER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "../visibility_control.h" - -namespace crane -{ - -class BallPlacementWithSkillPlanner : public PlannerBase -{ -public: - enum class BallPlacementState { - GO_TO_BALL, - TURN, - GET_BALL_CONTACT, - MOVE_WITH_BALL, - CLEAR_BALL, - }; - - COMPOSITION_PUBLIC - explicit BallPlacementWithSkillPlanner( - WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) - : PlannerBase("ball_placement_with_skill", world_model, visualizer), - state(BallPlacementState::GO_TO_BALL) - { - } - - std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (robots.size() != 1) { - return {}; - } - - placement_target = world_model->getBallPlacementTarget().value(); - - auto robot = world_model->getRobot(robots.front()); - - if (not move_with_ball) { - move_with_ball = std::make_unique(robot->id, world_model); - move_with_ball->setTargetPoint( - placement_target + - (world_model->ball.pos - placement_target).normalized() * robot->center_to_kicker().norm()); - } - - if (not get_ball_contact) { - get_ball_contact = std::make_unique(robot->id, world_model); - } - - crane::RobotCommandWrapper command(robot->id, world_model); - - switch (state) { - case BallPlacementState::GO_TO_BALL: { - command.setTargetPosition( - world_model->ball.pos + (robot->pose.pos - world_model->ball.pos).normalized() * 0.15); - command.setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); - command.setMaxVelocity(2.0); - command.setTerminalVelocity(0.2); - if (auto distance = world_model->getDistanceFromRobotToBall(robot->id); - distance < 0.20 && distance > 0.15) { - state = BallPlacementState::TURN; - } - break; - } - - case BallPlacementState::TURN: { - if (not turn_around_point) { - turn_around_point = std::make_unique(robot->id, world_model); - turn_around_point->setTargetPoint(world_model->ball.pos); - turn_around_point->setTargetAngle(getAngle(world_model->ball.pos - placement_target)); - turn_around_point->setParameter("max_velocity", 1.5); - turn_around_point->setParameter("max_turn_omega", M_PI); - } - if (turn_around_point->run(command, visualizer) == skills::Status::SUCCESS) { - std::cout << "GET_BALL_CONTACT" << std::endl; - state = BallPlacementState::GET_BALL_CONTACT; - turn_around_point = nullptr; - } - command.setMaxVelocity(1.0); - command.setTerminalVelocity(0.5); - break; - } - - case BallPlacementState::GET_BALL_CONTACT: { - if (get_ball_contact->run(command, visualizer) == skills::Status::SUCCESS) { - Point target_point = - placement_target + (world_model->ball.pos - placement_target).normalized() * - robot->center_to_kicker().norm(); - move_with_ball->setTargetPoint(target_point); - move_with_ball_success_count = 0; - state = BallPlacementState::MOVE_WITH_BALL; - } - command.setMaxVelocity(0.5); - break; - } - - case BallPlacementState::MOVE_WITH_BALL: { - auto status = move_with_ball->run(command, visualizer); - command.setMaxVelocity(0.5); - command.setTerminalVelocity(0.1); - // command.setTerminalVelocity( - // std::min(1.0, - // std::max((double)(robot->pose.pos - placement_target).norm() - 0.1, 0.0))); - command.setMaxOmega(M_PI / 2.0); - if (status == skills::Status::FAILURE) { - state = BallPlacementState::GO_TO_BALL; - } else if (status == skills::Status::SUCCESS) { - move_with_ball_success_count++; - if (move_with_ball_success_count >= 20) { - state = BallPlacementState::CLEAR_BALL; - } - } else { - move_with_ball_success_count = 0; - } - break; - } - - case BallPlacementState::CLEAR_BALL: { - command.setTargetPosition( - placement_target + (robot->pose.pos - placement_target).normalized() * 0.6); - command.setMaxVelocity(0.5); - break; - } - } - - std::vector cmd_msgs{command.getMsg()}; - - return {PlannerBase::Status::RUNNING, cmd_msgs}; - } - - auto getSelectedRobots( - uint8_t selectable_robots_num, const std::vector & selectable_robots) - -> std::vector override - { - return this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, [this](const std::shared_ptr & robot) { - // ボールに近いほどスコアが高い - return 100.0 / - std::max(world_model->getSquareDistanceFromRobotToBall({true, robot->id}), 0.01); - }); - } - -private: - BallPlacementState state; - - std::unique_ptr get_ball_contact = nullptr; - - std::unique_ptr move_with_ball = nullptr; - - std::unique_ptr turn_around_point = nullptr; - - Point placement_target = Point(0.0, 0.0); - - int move_with_ball_success_count = 0; -}; - -} // namespace crane - -#endif // CRANE_PLANNER_PLUGINS__TEMPORARY__BALL_PLACEMENT_WITH_SKILL_PLANNER_HPP_ From 5a8515b5b15569516ca0b0feb32f798813076ba9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:27:22 +0900 Subject: [PATCH 02/15] =?UTF-8?q?SkillBase::run=E3=81=AE=E5=BC=95=E6=95=B0?= =?UTF-8?q?=E3=81=8B=E3=82=89command=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/kickoff_attack.hpp | 2 +- .../include/crane_robot_skills/skill_base.hpp | 21 ++++++++++++------- .../src/single_ball_placement.cpp | 4 ++-- crane_simple_ai/src/crane_commander.cpp | 3 ++- .../crane_planner_plugins/marker_planner.hpp | 2 +- .../our_kickoff_planner.hpp | 4 ++-- .../crane_planner_plugins/skill_planner.hpp | 8 +++---- 7 files changed, 26 insertions(+), 18 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp index 9b235c644..e097b85fe 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp @@ -41,7 +41,7 @@ class KickoffAttack : public SkillBase go_over_ball->setParameter("margin", 0.3); command.setMaxVelocity(0.5); } - go_over_ball_status = go_over_ball->run(command, visualizer); + go_over_ball_status = go_over_ball->run(visualizer); return Status::RUNNING; }); addTransition( diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 32d8c84f7..2f932def3 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -95,7 +95,7 @@ class SkillInterface const std::string name; virtual Status run( - RobotCommandWrapper & command, ConsaiVisualizerWrapper::SharedPtr visualizer, + ConsaiVisualizerWrapper::SharedPtr visualizer, std::optional> parameters_opt = std::nullopt) = 0; @@ -158,13 +158,18 @@ class SkillBase : public SkillInterface SkillBase( const std::string & name, uint8_t id, const std::shared_ptr & world_model, - StatesType init_state) + StatesType init_state, const std::shared_ptr & robot_command = nullptr) : SkillInterface(name, id, world_model), state_machine(init_state) { + if (robot_command) { + command = robot_command; + } else { + command = std::make_shared(id, world_model); + } } Status run( - RobotCommandWrapper & command, ConsaiVisualizerWrapper::SharedPtr visualizer, + ConsaiVisualizerWrapper::SharedPtr visualizer, std::optional> parameters_opt = std::nullopt) override { @@ -173,12 +178,12 @@ class SkillBase : public SkillInterface } state_machine.update(); - command.latest_msg.current_pose.x = robot->pose.pos.x(); - command.latest_msg.current_pose.y = robot->pose.pos.y(); - command.latest_msg.current_pose.theta = robot->pose.theta; + command->latest_msg.current_pose.x = robot->pose.pos.x(); + command->latest_msg.current_pose.y = robot->pose.pos.y(); + command->latest_msg.current_pose.theta = robot->pose.theta; return state_functions[state_machine.getCurrentState()]( - world_model, robot, command, visualizer); + world_model, robot, *command, visualizer); } void addStateFunction(const StatesType & state, StateFunctionType function) @@ -216,6 +221,8 @@ class SkillBase : public SkillInterface // operator<< がAのprivateメンバにアクセスできるようにfriend宣言 friend std::ostream & operator<<(std::ostream & os, const SkillBase & skill_base); + + std::shared_ptr command = nullptr; }; } // namespace crane::skills diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 7709c789d..6068a7f18 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -29,7 +29,7 @@ SingleBallPlacement::SingleBallPlacement( go_over_ball->setParameter("margin", 0.4); } - skill_status = go_over_ball->run(command, visualizer); + skill_status = go_over_ball->run(visualizer); return Status::RUNNING; }); @@ -48,7 +48,7 @@ SingleBallPlacement::SingleBallPlacement( get_ball_contact = std::make_shared(robot->id, world_model); } - skill_status = get_ball_contact->run(command, visualizer); + skill_status = get_ball_contact->run(visualizer); return Status::RUNNING; }); diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp index 28724c197..aaf7b9507 100644 --- a/crane_simple_ai/src/crane_commander.cpp +++ b/crane_simple_ai/src/crane_commander.cpp @@ -140,7 +140,8 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U skills::Status task_result; try { - task_result = task.skill->run(*ros_node->commander, ros_node->visualizer, task.parameters); + task_result = task.skill->run(ros_node->visualizer, task.parameters); + ros_node->latest_msg = task.skill->getRobotCommand(); std::stringstream ss; task.skill->print(ss); ui->logTextBrowser->append(QString::fromStdString(ss.str())); diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp index 5a5c1dcb3..6cc7b6313 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp @@ -41,8 +41,8 @@ class MarkerPlanner : public PlannerBase for (auto & [id, value] : skill_map) { auto & [command, skill] = value; - skill->run(*command, visualizer); robot_commands.emplace_back(command->getMsg()); + skill->run(visualizer); } return {PlannerBase::Status::RUNNING, robot_commands}; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp index 22d74d3ef..25ffcf947 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp @@ -46,8 +46,8 @@ class OurKickOffPlanner : public PlannerBase { std::vector robot_commands; - kickoff_attack->run(*attacker_command, visualizer); - kickoff_support->run(*supporter_command, visualizer); + kickoff_attack->run(visualizer); + kickoff_support->run(visualizer); robot_commands.emplace_back(attacker_command->getMsg()); robot_commands.emplace_back(supporter_command->getMsg()); diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp index 88232bc96..cbfa4b96c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp @@ -80,8 +80,8 @@ class GoalieSkillPlanner : public PlannerBase return {PlannerBase ::Status ::RUNNING, {}}; } else { std ::vector robot_commands; - auto status = skill->run(*robot_command_wrapper, visualizer); - return {static_cast(status), {robot_command_wrapper->getMsg()}}; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; } } auto getSelectedRobots( @@ -119,8 +119,8 @@ class BallPlacementSkillPlanner : public PlannerBase skill->setParameter("placement_y", target->y()); } std ::vector robot_commands; - auto status = skill->run(*robot_command_wrapper, visualizer); - return {static_cast(status), {robot_command_wrapper->getMsg()}}; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; } } From 4cc6a3bad8a36180dd9ec3b40deb5b63bb502471 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:28:27 +0900 Subject: [PATCH 03/15] =?UTF-8?q?SkillBase::run=E3=81=AE=E5=BC=95=E6=95=B0?= =?UTF-8?q?=E3=81=8B=E3=82=89command=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 6068a7f18..be414b19c 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -69,7 +69,7 @@ SingleBallPlacement::SingleBallPlacement( move_with_ball->setParameter("target_y", getParameter("placement_y")); } - skill_status = move_with_ball->run(command, visualizer); + skill_status = move_with_ball->run(visualizer); return Status::RUNNING; }); @@ -87,7 +87,7 @@ SingleBallPlacement::SingleBallPlacement( if (not sleep) { sleep = std::make_shared(robot->id, world_model); } - skill_status = sleep->run(command, visualizer); + skill_status = sleep->run(visualizer); return Status::RUNNING; }); @@ -105,7 +105,7 @@ SingleBallPlacement::SingleBallPlacement( sleep = std::make_shared(robot->id, world_model); sleep->setParameter("duration", 0.5); } - skill_status = sleep->run(command, visualizer); + skill_status = sleep->run(visualizer); return Status::RUNNING; }); @@ -131,7 +131,7 @@ SingleBallPlacement::SingleBallPlacement( set_target_position->setParameter("y", leave_pos.y()); set_target_position->setParameter("reach_threshold", 0.05); - return set_target_position->run(command, visualizer); + return set_target_position->run(visualizer); }); } } // namespace crane::skills From bae1ec72a39714e5eb2356ae70ccb8e4f0a019e2 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:30:50 +0900 Subject: [PATCH 04/15] =?UTF-8?q?SkillBase=E3=82=AF=E3=83=A9=E3=82=B9?= =?UTF-8?q?=E3=81=8B=E3=82=89=E3=81=AERobotCommand=E5=87=BA=E5=8A=9B?= =?UTF-8?q?=E3=82=A4=E3=83=B3=E3=82=BF=E3=83=95=E3=82=A7=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=82=92=E6=95=B4=E5=82=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/skill_base.hpp | 6 ++++++ .../include/crane_planner_plugins/marker_planner.hpp | 2 +- .../include/crane_planner_plugins/our_kickoff_planner.hpp | 4 ++-- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 2f932def3..5c892e101 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -107,6 +107,8 @@ class SkillInterface void setParameter(const std::string & key, const std::string & value) { parameters[key] = value; } + virtual crane_msgs::msg::RobotCommand getRobotCommand() = 0; + template auto getParameter(const std::string & key) const { @@ -186,6 +188,10 @@ class SkillBase : public SkillInterface world_model, robot, *command, visualizer); } + crane_msgs::msg::RobotCommand getRobotCommand() override { return command->getMsg(); } + + // std::shared_ptr commander() const { return command; } + void addStateFunction(const StatesType & state, StateFunctionType function) { if (state_functions.find(state) != state_functions.end()) { diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp index 6cc7b6313..75695b41b 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp @@ -41,8 +41,8 @@ class MarkerPlanner : public PlannerBase for (auto & [id, value] : skill_map) { auto & [command, skill] = value; - robot_commands.emplace_back(command->getMsg()); skill->run(visualizer); + robot_commands.emplace_back(skill->getRobotCommand()); } return {PlannerBase::Status::RUNNING, robot_commands}; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp index 25ffcf947..e93cc4328 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp @@ -49,8 +49,8 @@ class OurKickOffPlanner : public PlannerBase kickoff_attack->run(visualizer); kickoff_support->run(visualizer); - robot_commands.emplace_back(attacker_command->getMsg()); - robot_commands.emplace_back(supporter_command->getMsg()); + robot_commands.emplace_back(kickoff_attack->getRobotCommand()); + robot_commands.emplace_back(kickoff_support->getRobotCommand()); // いい感じにSUCCESSも返す return {PlannerBase::Status::RUNNING, robot_commands}; From 629f53e7a3430ccbd956716a8e47958d9615d905 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:32:16 +0900 Subject: [PATCH 05/15] =?UTF-8?q?SimpleAI=E5=86=85=E3=81=A7=E3=81=AF?= =?UTF-8?q?=E7=89=B9=E5=AE=9A=E3=81=AEcommander=E3=82=92=E6=8C=81=E3=81=9F?= =?UTF-8?q?=E3=81=9A=E3=80=81ID=E3=81=A0=E3=81=91=E3=81=A7=E7=AE=A1?= =?UTF-8?q?=E7=90=86=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_simple_ai/include/crane_commander.hpp | 5 +++-- crane_simple_ai/src/crane_commander.cpp | 7 ++----- .../include/crane_planner_plugins/marker_planner.hpp | 3 +-- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index 02e335537..b4da157ff 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -119,14 +119,15 @@ class ROSNode : public rclcpp::Node void changeID(uint8_t id) { - commander = std::make_shared(id, world_model); + std::make_shared(robot_id, world_model)->stopHere(); + robot_id = id; } ~ROSNode() {} crane::WorldModelWrapper::SharedPtr world_model; - crane::RobotCommandWrapper::SharedPtr commander; + uint8_t robot_id = 0; rclcpp::TimerBase::SharedPtr timer; diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp index aaf7b9507..c1649dd41 100644 --- a/crane_simple_ai/src/crane_commander.cpp +++ b/crane_simple_ai/src/crane_commander.cpp @@ -99,7 +99,7 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U feedback.error_info.push_back(2); feedback.error_info.push_back(3); for (const auto & robot_feedback : robot_feedback_array.feedback) { - if (robot_feedback.robot_id == ros_node->commander->getMsg().robot_id) { + if (robot_feedback.robot_id == ros_node->robot_id) { feedback = robot_feedback; break; } @@ -133,8 +133,7 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U } else { auto & task = task_queue_execution.front(); if (task.skill == nullptr) { - task.skill = skill_generators[task.name]( - ros_node->commander->getMsg().robot_id, ros_node->world_model); + task.skill = skill_generators[task.name](ros_node->robot_id, ros_node->world_model); task.start_time = std::chrono::steady_clock::now(); } @@ -256,9 +255,7 @@ void CraneCommander::setupROS2() void CraneCommander::on_robotIDSpinBox_valueChanged(int arg1) { ui->logTextBrowser->append(QString::fromStdString("ID changed to " + std::to_string(arg1))); - ros_node->commander->stopHere(); ros_node->changeID(arg1); - ros_node->commander->stopHere(); } // コマンドが変わったらテーブルにデフォルト値を入れる diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp index 75695b41b..b32a31f4d 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp @@ -39,8 +39,7 @@ class MarkerPlanner : public PlannerBase { std::vector robot_commands; - for (auto & [id, value] : skill_map) { - auto & [command, skill] = value; + for (auto & [id, skill] : skill_map) { skill->run(visualizer); robot_commands.emplace_back(skill->getRobotCommand()); } From acf79d10ce36c4c37eb70863a87b02103ce69821 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:33:01 +0900 Subject: [PATCH 06/15] =?UTF-8?q?SimpleAI=E5=86=85=E3=81=A7=E3=81=AE?= =?UTF-8?q?=E6=9C=80=E6=96=B0=E3=83=A1=E3=83=83=E3=82=BB=E3=83=BC=E3=82=B8?= =?UTF-8?q?=E7=AE=A1=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_simple_ai/include/crane_commander.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index b4da157ff..5b26ae474 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -112,7 +112,7 @@ class ROSNode : public rclcpp::Node crane_msgs::msg::RobotCommands msg; msg.header = world_model->getMsg().header; msg.is_yellow = world_model->isYellow(); - msg.robot_commands.push_back(commander->getMsg()); + msg.robot_commands.push_back(latest_msg); publisher_robot_commands->publish(msg); }); } @@ -131,6 +131,8 @@ class ROSNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer; + crane_msgs::msg::RobotCommand latest_msg; + rclcpp::Publisher::SharedPtr publisher_robot_commands; rclcpp::Subscription::SharedPtr subscription_robot_feedback; From 277f4d095a01b3faf3001fe59d06235bdc169e08 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:34:04 +0900 Subject: [PATCH 07/15] =?UTF-8?q?=E3=81=8A=E6=8E=83=E9=99=A4=EF=BC=88?= =?UTF-8?q?=E3=81=99=E3=81=A3=E3=81=8D=E3=82=8A=E3=80=9C=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_simple_ai/include/crane_commander.hpp | 1 - .../crane_planner_plugins/marker_planner.hpp | 13 ++++--------- .../crane_planner_plugins/our_kickoff_planner.hpp | 6 ------ .../include/crane_planner_plugins/skill_planner.hpp | 13 ++----------- 4 files changed, 6 insertions(+), 27 deletions(-) diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index 5b26ae474..59529597c 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -98,7 +98,6 @@ class ROSNode : public rclcpp::Node ROSNode() : Node("crane_commander") { world_model = std::make_shared(*this); - commander = std::make_shared(0, world_model); visualizer = std::make_shared(*this, "simple_ai"); publisher_robot_commands = create_publisher("/control_targets", 10); diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp index b32a31f4d..b55b47d6f 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp @@ -86,12 +86,9 @@ class MarkerPlanner : public PlannerBase selected_robots.push_back(selectable_robots[min_index]); skill_map.emplace( selectable_robots[min_index], - std::make_pair( - std::make_shared(selectable_robots[min_index], world_model), - std::make_shared(selectable_robots[min_index], world_model))); - skill_map[selectable_robots[min_index]].second->setParameter( - "marking_robot_id", enemy_robot->id); - skill_map[selectable_robots[min_index]].second->setParameter("mark_distance", 0.5); + std::make_shared(selectable_robots[min_index], world_model)); + skill_map[selectable_robots[min_index]]->setParameter("marking_robot_id", enemy_robot->id); + skill_map[selectable_robots[min_index]]->setParameter("mark_distance", 0.5); } return selected_robots; @@ -101,9 +98,7 @@ class MarkerPlanner : public PlannerBase // key: ID of our robot in charge, value: ID of the enemy marked robot std::unordered_map marking_target_map; - std::unordered_map< - uint8_t, std::pair, std::shared_ptr>> - skill_map; + std::unordered_map> skill_map; }; } // namespace crane diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp index e93cc4328..66234722c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp @@ -28,12 +28,8 @@ class OurKickOffPlanner : public PlannerBase private: std::shared_ptr kickoff_attack; - std::shared_ptr attacker_command; - std::shared_ptr kickoff_support; - std::shared_ptr supporter_command; - public: COMPOSITION_PUBLIC explicit OurKickOffPlanner( WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) @@ -83,9 +79,7 @@ class OurKickOffPlanner : public PlannerBase }); kickoff_attack = std::make_shared(*best_attacker, world_model); - attacker_command = std::make_shared(*best_attacker, world_model); kickoff_support = std::make_shared(*best_supporter, world_model); - supporter_command = std::make_shared(*best_supporter, world_model); return {*best_attacker, *best_supporter}; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp index cbfa4b96c..58772e9d0 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp @@ -65,8 +65,6 @@ class GoalieSkillPlanner : public PlannerBase public: std ::shared_ptr skill = nullptr; - std ::shared_ptr robot_command_wrapper = nullptr; - COMPOSITION_PUBLIC explicit GoalieSkillPlanner( WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) : PlannerBase("Goalie", world_model, visualizer) @@ -76,7 +74,7 @@ class GoalieSkillPlanner : public PlannerBase std ::pair> calculateRobotCommand( const std ::vector & robots) override { - if (not skill or not robot_command_wrapper) { + if (not skill) { return {PlannerBase ::Status ::RUNNING, {}}; } else { std ::vector robot_commands; @@ -89,8 +87,6 @@ class GoalieSkillPlanner : public PlannerBase -> std ::vector override { skill = std ::make_shared(world_model->getOurGoalieId(), world_model); - robot_command_wrapper = - std ::make_shared(world_model->getOurGoalieId(), world_model); return {world_model->getOurGoalieId()}; } }; @@ -100,8 +96,6 @@ class BallPlacementSkillPlanner : public PlannerBase public: std ::shared_ptr skill = nullptr; - std ::shared_ptr robot_command_wrapper = nullptr; - COMPOSITION_PUBLIC explicit BallPlacementSkillPlanner( WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) : PlannerBase("BallPlacement", world_model, visualizer) @@ -111,7 +105,7 @@ class BallPlacementSkillPlanner : public PlannerBase std ::pair> calculateRobotCommand( const std ::vector & robots) override { - if (not skill or not robot_command_wrapper) { + if (not skill) { return {PlannerBase ::Status ::RUNNING, {}}; } else { if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { @@ -140,9 +134,6 @@ class BallPlacementSkillPlanner : public PlannerBase skill->setParameter("placement_x", target->x()); skill->setParameter("placement_y", target->y()); } - - robot_command_wrapper = - std ::make_shared(selected_robots.front(), world_model); return {selected_robots.front()}; } }; From f15f72ff76767d1417b08ca53871fab5fc991663 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:48:37 +0900 Subject: [PATCH 08/15] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E3=82=B3?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/go_over_ball.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/crane_robot_skills/src/go_over_ball.cpp b/crane_robot_skills/src/go_over_ball.cpp index 206c54540..aa5a6cb6c 100644 --- a/crane_robot_skills/src/go_over_ball.cpp +++ b/crane_robot_skills/src/go_over_ball.cpp @@ -45,9 +45,6 @@ GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wo } }(); - std::cout << "final_distance: " << final_distance << std::endl; - std::cout << "intermediate_distance: " << intermediate_distance << std::endl; - if (intermediate_distance < final_distance && not has_passed_intermediate_target) { command.setTargetPosition(intermediate_point); if (intermediate_distance < getParameter("reach_threshold")) { From d606cf6697816965b0cdef6de4053f4c59dc17c9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:52:21 +0900 Subject: [PATCH 09/15] =?UTF-8?q?StateFunctionType=E3=82=92=E3=82=B9?= =?UTF-8?q?=E3=83=83=E3=82=AD=E3=83=AA=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/skill_base.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 5c892e101..0ddc22dc6 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -154,9 +154,7 @@ template class SkillBase : public SkillInterface { public: - using StateFunctionType = std::function &, const std::shared_ptr &, - crane::RobotCommandWrapper &, ConsaiVisualizerWrapper::SharedPtr)>; + using StateFunctionType = std::function; SkillBase( const std::string & name, uint8_t id, const std::shared_ptr & world_model, @@ -184,8 +182,7 @@ class SkillBase : public SkillInterface command->latest_msg.current_pose.y = robot->pose.pos.y(); command->latest_msg.current_pose.theta = robot->pose.theta; - return state_functions[state_machine.getCurrentState()]( - world_model, robot, *command, visualizer); + return state_functions[state_machine.getCurrentState()](visualizer); } crane_msgs::msg::RobotCommand getRobotCommand() override { return command->getMsg(); } From c022e383636d890e1b21b2f3891df4340455b06c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 23:11:12 +0900 Subject: [PATCH 10/15] =?UTF-8?q?world=5Fmodel=E3=81=AE=E5=90=8D=E5=89=8D?= =?UTF-8?q?=E3=82=B7=E3=83=A3=E3=83=89=E3=83=BC=E3=82=A4=E3=83=B3=E3=82=B0?= =?UTF-8?q?=E3=82=92=E9=81=BF=E3=81=91=E3=82=8B=E3=81=9F=E3=82=81=E3=80=81?= =?UTF-8?q?=E4=B8=80=E9=83=A8=E3=81=AE=E5=BC=95=E6=95=B0=E3=82=92wm?= =?UTF-8?q?=E3=81=AB=E3=81=97=E3=81=9F=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/skill_base.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 0ddc22dc6..47729903d 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -88,8 +88,8 @@ class SkillInterface { public: SkillInterface( - const std::string & name, uint8_t id, const std::shared_ptr & world_model) - : name(name), world_model(world_model), robot(world_model->getOurRobot(id)) + const std::string & name, uint8_t id, const std::shared_ptr & wm) + : name(name), world_model(wm), robot(world_model->getOurRobot(id)) { } const std::string name; @@ -157,14 +157,14 @@ class SkillBase : public SkillInterface using StateFunctionType = std::function; SkillBase( - const std::string & name, uint8_t id, const std::shared_ptr & world_model, + const std::string & name, uint8_t id, const std::shared_ptr & wm, StatesType init_state, const std::shared_ptr & robot_command = nullptr) - : SkillInterface(name, id, world_model), state_machine(init_state) + : SkillInterface(name, id, wm), state_machine(init_state) { if (robot_command) { command = robot_command; } else { - command = std::make_shared(id, world_model); + command = std::make_shared(id, wm); } } From 4f542e5efe7012f39de53e85e356411feb7cea24 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 23:17:13 +0900 Subject: [PATCH 11/15] =?UTF-8?q?world=5Fmodel=E3=81=AE=E5=90=8D=E5=89=8D?= =?UTF-8?q?=E3=82=B7=E3=83=A3=E3=83=89=E3=83=BC=E3=82=A4=E3=83=B3=E3=82=B0?= =?UTF-8?q?=E3=82=92=E9=81=BF=E3=81=91=E3=82=8B=E3=81=9F=E3=82=81=E3=80=81?= =?UTF-8?q?=E4=B8=80=E9=83=A8=E3=81=AE=E5=BC=95=E6=95=B0=E3=82=92wm?= =?UTF-8?q?=E3=81=AB=E3=81=97=E3=81=9F=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/get_ball_contact.hpp | 2 +- .../crane_robot_skills/go_over_ball.hpp | 2 +- .../include/crane_robot_skills/goalie.hpp | 2 +- .../include/crane_robot_skills/idle.hpp | 4 +- .../crane_robot_skills/kickoff_attack.hpp | 5 +- .../crane_robot_skills/kickoff_support.hpp | 4 +- .../include/crane_robot_skills/marker.hpp | 4 +- .../crane_robot_skills/move_to_geometry.hpp | 2 +- .../crane_robot_skills/move_with_ball.hpp | 2 +- .../include/crane_robot_skills/receiver.hpp | 2 +- .../robot_command_as_skill.hpp | 12 +-- .../crane_robot_skills/simple_attacker.hpp | 2 +- .../single_ball_placement.hpp | 2 +- .../include/crane_robot_skills/sleep.hpp | 4 +- .../crane_robot_skills/turn_around_point.hpp | 4 +- crane_robot_skills/src/get_ball_contact.cpp | 4 +- crane_robot_skills/src/go_over_ball.cpp | 4 +- crane_robot_skills/src/goalie.cpp | 4 +- crane_robot_skills/src/move_with_ball.cpp | 4 +- crane_robot_skills/src/receiver.cpp | 4 +- .../src/robot_command_as_skill.cpp | 80 +++++++++---------- crane_robot_skills/src/simple_attacker.cpp | 4 +- .../src/single_ball_placement.cpp | 5 +- 23 files changed, 78 insertions(+), 84 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp index a3c9d3e0d..83964ebac 100644 --- a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp +++ b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp @@ -16,7 +16,7 @@ namespace crane::skills class GetBallContact : public SkillBase<> { public: - explicit GetBallContact(uint8_t id, const std::shared_ptr & world_model); + explicit GetBallContact(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & out) const override { diff --git a/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp b/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp index 76bf612d8..90f28cfc7 100644 --- a/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp @@ -18,7 +18,7 @@ namespace crane::skills class GoOverBall : public SkillBase<> { public: - explicit GoOverBall(uint8_t id, const std::shared_ptr & world_model); + explicit GoOverBall(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & out) const override { diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp index b6d9afbde..160ccee75 100644 --- a/crane_robot_skills/include/crane_robot_skills/goalie.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goalie.hpp @@ -18,7 +18,7 @@ namespace crane::skills class Goalie : public SkillBase<> { public: - explicit Goalie(uint8_t id, const std::shared_ptr & world_model); + explicit Goalie(uint8_t id, const std::shared_ptr & wm); void emitBallFromPenaltyArea(crane::RobotCommandWrapper & target) { diff --git a/crane_robot_skills/include/crane_robot_skills/idle.hpp b/crane_robot_skills/include/crane_robot_skills/idle.hpp index 83bccf2cd..27df2311d 100644 --- a/crane_robot_skills/include/crane_robot_skills/idle.hpp +++ b/crane_robot_skills/include/crane_robot_skills/idle.hpp @@ -16,8 +16,8 @@ namespace crane::skills class Idle : public SkillBase<> { public: - explicit Idle(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("Idle", id, world_model, DefaultStates::DEFAULT) + explicit Idle(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("Idle", id, wm, DefaultStates::DEFAULT) { setParameter("stop_by_position", true); addStateFunction( diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp index e097b85fe..363ecc64e 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp @@ -21,9 +21,8 @@ enum class KickoffAttackState { class KickoffAttack : public SkillBase { public: - explicit KickoffAttack(uint8_t id, const std::shared_ptr & world_model) - : SkillBase( - "KickoffAttack", id, world_model, KickoffAttackState::PREPARE_KICKOFF) + explicit KickoffAttack(uint8_t id, const std::shared_ptr & wm) + : SkillBase("KickoffAttack", id, wm, KickoffAttackState::PREPARE_KICKOFF) { setParameter("target_x", 0.0f); setParameter("target_y", 1.0f); diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp index 8bcbdf137..959509b22 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp @@ -16,8 +16,8 @@ namespace crane::skills class KickoffSupport : public SkillBase<> { public: - explicit KickoffSupport(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("KickoffSupport", id, world_model, DefaultStates::DEFAULT) + explicit KickoffSupport(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("KickoffSupport", id, wm, DefaultStates::DEFAULT) { setParameter("target_x", 0.0f); setParameter("target_y", 1.0f); diff --git a/crane_robot_skills/include/crane_robot_skills/marker.hpp b/crane_robot_skills/include/crane_robot_skills/marker.hpp index dd3a9bfeb..18691d17a 100644 --- a/crane_robot_skills/include/crane_robot_skills/marker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/marker.hpp @@ -21,8 +21,8 @@ namespace crane::skills class Marker : public SkillBase<> { public: - explicit Marker(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("Marker", id, world_model, DefaultStates::DEFAULT) + explicit Marker(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("Marker", id, wm, DefaultStates::DEFAULT) { setParameter("marking_robot_id", 0); setParameter("mark_distance", 0.5); diff --git a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp index 3c94b88ca..dbe112d6e 100644 --- a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp +++ b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp @@ -21,7 +21,7 @@ class MoveToGeometry : public SkillBase<> public: explicit MoveToGeometry( uint8_t id, Geometry geometry, const std::shared_ptr & world_model) - : SkillBase<>("MoveToGeometry", id, world_model, DefaultStates::DEFAULT), geometry(geometry) + : SkillBase<>("MoveToGeometry", id, wm, DefaultStates::DEFAULT), geometry(geometry) { setParameter("reach_threshold", 0.1); addStateFunction( diff --git a/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp b/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp index 0554f0ecf..a987a8b9a 100644 --- a/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp @@ -26,7 +26,7 @@ enum class MoveWithBallStates { SUCCESS, RUNNING, FAILURE }; class MoveWithBall : public SkillBase<> { public: - explicit MoveWithBall(uint8_t id, const std::shared_ptr & world_model); + explicit MoveWithBall(uint8_t id, const std::shared_ptr & wm); Point getTargetPoint(const Point & target_pos) { diff --git a/crane_robot_skills/include/crane_robot_skills/receiver.hpp b/crane_robot_skills/include/crane_robot_skills/receiver.hpp index 720108a4c..116c23612 100644 --- a/crane_robot_skills/include/crane_robot_skills/receiver.hpp +++ b/crane_robot_skills/include/crane_robot_skills/receiver.hpp @@ -40,7 +40,7 @@ class Receiver : public SkillBase<> double score; }; - explicit Receiver(uint8_t id, const std::shared_ptr & world_model); + explicit Receiver(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { os << "[Receiver]"; } diff --git a/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp b/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp index d3565567f..287bd479e 100644 --- a/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp +++ b/crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp @@ -14,12 +14,12 @@ namespace crane::skills { -#define DEFINE_SKILL_COMMAND(name) \ - class Cmd##name : public SkillBase<> \ - { \ - public: \ - explicit Cmd##name(uint8_t id, const std::shared_ptr & world_model); \ - void print(std::ostream & os) const override; \ +#define DEFINE_SKILL_COMMAND(name) \ + class Cmd##name : public SkillBase<> \ + { \ + public: \ + explicit Cmd##name(uint8_t id, const std::shared_ptr & wm); \ + void print(std::ostream & os) const override; \ } DEFINE_SKILL_COMMAND(KickWithChip); diff --git a/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp b/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp index 846fadce2..5298af496 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp @@ -18,7 +18,7 @@ namespace crane::skills class SimpleAttacker : public SkillBase<> { public: - explicit SimpleAttacker(uint8_t id, const std::shared_ptr & world_model); + explicit SimpleAttacker(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { diff --git a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp index 802b13fd7..3c1200783 100644 --- a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp +++ b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp @@ -44,7 +44,7 @@ class SingleBallPlacement : public SkillBase Status skill_status = Status::RUNNING; public: - explicit SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model); + explicit SingleBallPlacement(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { diff --git a/crane_robot_skills/include/crane_robot_skills/sleep.hpp b/crane_robot_skills/include/crane_robot_skills/sleep.hpp index 74ad5906b..9734da0be 100644 --- a/crane_robot_skills/include/crane_robot_skills/sleep.hpp +++ b/crane_robot_skills/include/crane_robot_skills/sleep.hpp @@ -16,8 +16,8 @@ namespace crane::skills class Sleep : public SkillBase<> { public: - explicit Sleep(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("Sleep", id, world_model, DefaultStates::DEFAULT) + explicit Sleep(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("Sleep", id, wm, DefaultStates::DEFAULT) { setParameter("duration", 0.0); addStateFunction( diff --git a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp index 12c5e5689..497148138 100644 --- a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp +++ b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp @@ -21,8 +21,8 @@ namespace crane::skills class TurnAroundPoint : public SkillBase<> { public: - explicit TurnAroundPoint(uint8_t id, const std::shared_ptr & world_model) - : SkillBase<>("TurnAroundPoint", id, world_model, DefaultStates::DEFAULT) + explicit TurnAroundPoint(uint8_t id, const std::shared_ptr & wm) + : SkillBase<>("TurnAroundPoint", id, wm, DefaultStates::DEFAULT) { setParameter("target_x", 0.0); setParameter("target_y", 0.0); diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp index 384e32c75..5d2f3cb23 100644 --- a/crane_robot_skills/src/get_ball_contact.cpp +++ b/crane_robot_skills/src/get_ball_contact.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -GetBallContact::GetBallContact(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("GetBallContact", id, world_model, DefaultStates::DEFAULT) +GetBallContact::GetBallContact(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("GetBallContact", id, wm, DefaultStates::DEFAULT) { setParameter("min_contact_duration", 0.5); setParameter("dribble_power", 0.5); diff --git a/crane_robot_skills/src/go_over_ball.cpp b/crane_robot_skills/src/go_over_ball.cpp index aa5a6cb6c..c5a0b3aab 100644 --- a/crane_robot_skills/src/go_over_ball.cpp +++ b/crane_robot_skills/src/go_over_ball.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("GoOverBall", id, world_model, DefaultStates::DEFAULT) +GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("GoOverBall", id, wm, DefaultStates::DEFAULT) { setParameter("next_target_x", 0.0); setParameter("next_target_y", 0.0); diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 98f6b27aa..b78b0d666 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -Goalie::Goalie(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("Goalie", id, world_model, DefaultStates::DEFAULT) +Goalie::Goalie(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("Goalie", id, wm, DefaultStates::DEFAULT) { setParameter("run_inplay", true); addStateFunction( diff --git a/crane_robot_skills/src/move_with_ball.cpp b/crane_robot_skills/src/move_with_ball.cpp index 7db119e69..e613768ae 100644 --- a/crane_robot_skills/src/move_with_ball.cpp +++ b/crane_robot_skills/src/move_with_ball.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("MoveWithBall", id, world_model, DefaultStates::DEFAULT) +MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("MoveWithBall", id, wm, DefaultStates::DEFAULT) { setParameter("target_x", 0.0); setParameter("target_y", 0.0); diff --git a/crane_robot_skills/src/receiver.cpp b/crane_robot_skills/src/receiver.cpp index 68dd2ad76..422b0e3a8 100644 --- a/crane_robot_skills/src/receiver.cpp +++ b/crane_robot_skills/src/receiver.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -Receiver::Receiver(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("Receiver", id, world_model, DefaultStates::DEFAULT) +Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("Receiver", id, wm, DefaultStates::DEFAULT) { setParameter("passer_id", 0); setParameter("receive_x", 0.0); diff --git a/crane_robot_skills/src/robot_command_as_skill.cpp b/crane_robot_skills/src/robot_command_as_skill.cpp index 1d5f174ad..5b4c96b49 100644 --- a/crane_robot_skills/src/robot_command_as_skill.cpp +++ b/crane_robot_skills/src/robot_command_as_skill.cpp @@ -23,10 +23,12 @@ namespace crane::skills return Status::SUCCESS; \ }); \ } \ + Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & wm) \ + : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ void Cmd##name::print(std::ostream & os) const {} -CmdKickWithChip::CmdKickWithChip(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdKickWithChip", id, world_model, DefaultStates::DEFAULT) +CmdKickWithChip::CmdKickWithChip(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdKickWithChip", id, wm, DefaultStates::DEFAULT) { setParameter("power", 0.5); addStateFunction( @@ -46,8 +48,8 @@ void CmdKickWithChip::print(std::ostream & os) const ; } -CmdKickStraight::CmdKickStraight(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdKickStraight", id, world_model, DefaultStates::DEFAULT) +CmdKickStraight::CmdKickStraight(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdKickStraight", id, wm, DefaultStates::DEFAULT) { setParameter("power", 0.5); addStateFunction( @@ -67,8 +69,8 @@ void CmdKickStraight::print(std::ostream & os) const ; } -CmdDribble::CmdDribble(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdDribble", id, world_model, DefaultStates::DEFAULT) +CmdDribble::CmdDribble(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdDribble", id, wm, DefaultStates::DEFAULT) { setParameter("power", 0.5); addStateFunction( @@ -88,8 +90,8 @@ void CmdDribble::print(std::ostream & os) const ; } -CmdSetVelocity::CmdSetVelocity(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetVelocity", id, world_model, DefaultStates::DEFAULT) +CmdSetVelocity::CmdSetVelocity(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetVelocity", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); @@ -110,8 +112,8 @@ void CmdSetVelocity::print(std::ostream & os) const } CmdSetTargetPosition::CmdSetTargetPosition( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetTargetPosition", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetTargetPosition", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); @@ -144,8 +146,8 @@ void CmdSetTargetPosition::print(std::ostream & os) const } CmdSetDribblerTargetPosition::CmdSetDribblerTargetPosition( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetDribblerTargetPosition", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetDribblerTargetPosition", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); @@ -178,9 +180,8 @@ void CmdSetDribblerTargetPosition::print(std::ostream & os) const .norm(); } -CmdSetTargetTheta::CmdSetTargetTheta( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetTargetTheta", id, world_model, DefaultStates::DEFAULT) +CmdSetTargetTheta::CmdSetTargetTheta(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetTargetTheta", id, wm, DefaultStates::DEFAULT) { setParameter("theta", 0.0); addStateFunction( @@ -199,8 +200,8 @@ void CmdSetTargetTheta::print(std::ostream & os) const os << "[CmdSetTargetTheta] theta: " << getParameter("theta"); } -CmdStopHere::CmdStopHere(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdStopHere", id, world_model, DefaultStates::DEFAULT) +CmdStopHere::CmdStopHere(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdStopHere", id, wm, DefaultStates::DEFAULT) { addStateFunction( DefaultStates::DEFAULT, @@ -227,9 +228,8 @@ ONE_FRAME_IMPLEMENTATION(SetGoalieDefault, setGoalieDefault()) ONE_FRAME_IMPLEMENTATION(EnableBallCenteringControl, enableBallCenteringControl()) ONE_FRAME_IMPLEMENTATION(EnableLocalGoalie, enableLocalGoalie()) -CmdSetMaxVelocity::CmdSetMaxVelocity( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetMaxVelocity", id, world_model, DefaultStates::DEFAULT) +CmdSetMaxVelocity::CmdSetMaxVelocity(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetMaxVelocity", id, wm, DefaultStates::DEFAULT) { setParameter("max_velocity", 0.5); addStateFunction( @@ -249,8 +249,8 @@ void CmdSetMaxVelocity::print(std::ostream & os) const } CmdSetMaxAcceleration::CmdSetMaxAcceleration( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetMaxAcceleration", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetMaxAcceleration", id, wm, DefaultStates::DEFAULT) { setParameter("max_acceleration", 0.5); addStateFunction( @@ -269,8 +269,8 @@ void CmdSetMaxAcceleration::print(std::ostream & os) const os << "[CmdSetMaxAcceleration] max_acceleration: " << getParameter("max_acceleration"); } -CmdSetMaxOmega::CmdSetMaxOmega(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetMaxOmega", id, world_model, DefaultStates::DEFAULT) +CmdSetMaxOmega::CmdSetMaxOmega(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetMaxOmega", id, wm, DefaultStates::DEFAULT) { setParameter("max_omega", 0.5); addStateFunction( @@ -290,8 +290,8 @@ void CmdSetMaxOmega::print(std::ostream & os) const } CmdSetTerminalVelocity::CmdSetTerminalVelocity( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdSetTerminalVelocity", id, world_model, DefaultStates::DEFAULT) + uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdSetTerminalVelocity", id, wm, DefaultStates::DEFAULT) { setParameter("terminal_velocity", 0.5); addStateFunction( @@ -310,9 +310,8 @@ void CmdSetTerminalVelocity::print(std::ostream & os) const os << "[CmdSetTerminalVelocity] terminal_velocity: " << getParameter("terminal_velocity"); } -CmdEnableStopFlag::CmdEnableStopFlag( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdEnableStopFlag ", id, world_model, DefaultStates::DEFAULT) +CmdEnableStopFlag::CmdEnableStopFlag(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdEnableStopFlag ", id, wm, DefaultStates::DEFAULT) { addStateFunction( DefaultStates::DEFAULT, @@ -327,9 +326,8 @@ CmdEnableStopFlag::CmdEnableStopFlag( void CmdEnableStopFlag::print(std::ostream & os) const { os << "[CmdEnableStopFlag]"; } -CmdDisableStopFlag::CmdDisableStopFlag( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdDisableStopFlag", id, world_model, DefaultStates::DEFAULT) +CmdDisableStopFlag::CmdDisableStopFlag(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdDisableStopFlag", id, wm, DefaultStates::DEFAULT) { addStateFunction( DefaultStates::DEFAULT, @@ -344,9 +342,8 @@ CmdDisableStopFlag::CmdDisableStopFlag( void CmdDisableStopFlag::print(std::ostream & os) const { os << "[CmdDisableStopFlag]"; } -CmdLiftUpDribbler::CmdLiftUpDribbler( - uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLiftUpDribbler", id, world_model, DefaultStates::DEFAULT) +CmdLiftUpDribbler::CmdLiftUpDribbler(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLiftUpDribbler", id, wm, DefaultStates::DEFAULT) { setParameter("enable", true); addStateFunction( @@ -365,8 +362,8 @@ void CmdLiftUpDribbler::print(std::ostream & os) const os << "[CmdLiftUpDribbler] enable: " << getParameter("enable"); } -CmdLookAt::CmdLookAt(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLookAt", id, world_model, DefaultStates::DEFAULT) +CmdLookAt::CmdLookAt(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLookAt", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); @@ -387,8 +384,8 @@ void CmdLookAt::print(std::ostream & os) const os << "[CmdLookAt] x: " << getParameter("x") << " y: " << getParameter("y"); } -CmdLookAtBall::CmdLookAtBall(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLookAtBall", id, world_model, DefaultStates::DEFAULT) +CmdLookAtBall::CmdLookAtBall(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLookAtBall", id, wm, DefaultStates::DEFAULT) { addStateFunction( DefaultStates::DEFAULT, @@ -403,9 +400,8 @@ CmdLookAtBall::CmdLookAtBall(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("CmdLookAtBallFrom", id, world_model, DefaultStates::DEFAULT) +CmdLookAtBallFrom::CmdLookAtBallFrom(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("CmdLookAtBallFrom", id, wm, DefaultStates::DEFAULT) { setParameter("x", 0.0); setParameter("y", 0.0); diff --git a/crane_robot_skills/src/simple_attacker.cpp b/crane_robot_skills/src/simple_attacker.cpp index a830f2f69..fd6d9af46 100644 --- a/crane_robot_skills/src/simple_attacker.cpp +++ b/crane_robot_skills/src/simple_attacker.cpp @@ -8,8 +8,8 @@ namespace crane::skills { -SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptr & world_model) -: SkillBase<>("SimpleAttacker", id, world_model, DefaultStates::DEFAULT) +SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("SimpleAttacker", id, wm, DefaultStates::DEFAULT) { addStateFunction( DefaultStates::DEFAULT, diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index be414b19c..36f5275cb 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -9,10 +9,9 @@ namespace crane::skills { -SingleBallPlacement::SingleBallPlacement( - uint8_t id, const std::shared_ptr & world_model) +SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & wm) : SkillBase( - "SingleBallPlacement", id, world_model, SingleBallPlacementStates::GO_OVER_BALL) + "SingleBallPlacement", id, wm, SingleBallPlacementStates::GO_OVER_BALL) { setParameter("placement_x", 0.); setParameter("placement_y", 0.); From fdadf1059dab4bdd246845ad4175fc69e743b0ab Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 23:17:53 +0900 Subject: [PATCH 12/15] =?UTF-8?q?world=5Fmodel=E3=81=AE=E5=90=8D=E5=89=8D?= =?UTF-8?q?=E3=82=B7=E3=83=A3=E3=83=89=E3=83=BC=E3=82=A4=E3=83=B3=E3=82=B0?= =?UTF-8?q?=E3=82=92=E9=81=BF=E3=81=91=E3=82=8B=E3=81=9F=E3=82=81=E3=80=81?= =?UTF-8?q?=E4=B8=80=E9=83=A8=E3=81=AE=E5=BC=95=E6=95=B0=E3=82=92wm?= =?UTF-8?q?=E3=81=AB=E3=81=97=E3=81=9F=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/robot_command_as_skill.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/robot_command_as_skill.cpp b/crane_robot_skills/src/robot_command_as_skill.cpp index 5b4c96b49..3d16f3984 100644 --- a/crane_robot_skills/src/robot_command_as_skill.cpp +++ b/crane_robot_skills/src/robot_command_as_skill.cpp @@ -10,8 +10,6 @@ namespace crane::skills { #define ONE_FRAME_IMPLEMENTATION(name, method) \ - Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & world_model) \ - : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ { \ addStateFunction( \ DefaultStates::DEFAULT, \ @@ -25,6 +23,8 @@ namespace crane::skills } \ Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & wm) \ : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ + Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & wm) \ + : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ void Cmd##name::print(std::ostream & os) const {} CmdKickWithChip::CmdKickWithChip(uint8_t id, const std::shared_ptr & wm) From e543a680efbd7836a82fc1816ac99f338a58cd8a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 23:18:01 +0900 Subject: [PATCH 13/15] =?UTF-8?q?world=5Fmodel=E3=81=AE=E5=90=8D=E5=89=8D?= =?UTF-8?q?=E3=82=B7=E3=83=A3=E3=83=89=E3=83=BC=E3=82=A4=E3=83=B3=E3=82=B0?= =?UTF-8?q?=E3=82=92=E9=81=BF=E3=81=91=E3=82=8B=E3=81=9F=E3=82=81=E3=80=81?= =?UTF-8?q?=E4=B8=80=E9=83=A8=E3=81=AE=E5=BC=95=E6=95=B0=E3=82=92wm?= =?UTF-8?q?=E3=81=AB=E3=81=97=E3=81=9F=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/robot_command_as_skill.cpp | 22 +++++++------------ 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/crane_robot_skills/src/robot_command_as_skill.cpp b/crane_robot_skills/src/robot_command_as_skill.cpp index 3d16f3984..732799e24 100644 --- a/crane_robot_skills/src/robot_command_as_skill.cpp +++ b/crane_robot_skills/src/robot_command_as_skill.cpp @@ -9,22 +9,16 @@ namespace crane::skills { -#define ONE_FRAME_IMPLEMENTATION(name, method) \ - { \ - addStateFunction( \ - DefaultStates::DEFAULT, \ - [this]( \ - const std::shared_ptr & world_model, \ - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, \ - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { \ - command.method; \ - return Status::SUCCESS; \ - }); \ - } \ - Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & wm) \ - : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ +#define ONE_FRAME_IMPLEMENTATION(name, method) \ Cmd##name::Cmd##name(uint8_t id, const std::shared_ptr & wm) \ : SkillBase<>("Cmd" #name, id, world_model, DefaultStates::DEFAULT) \ + { \ + addStateFunction( \ + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { \ + command->method; \ + return Status::SUCCESS; \ + }); \ + } \ void Cmd##name::print(std::ostream & os) const {} CmdKickWithChip::CmdKickWithChip(uint8_t id, const std::shared_ptr & wm) From 7a3ed57840cff6ef39047acad329ff118b6440b7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 23:19:10 +0900 Subject: [PATCH 14/15] =?UTF-8?q?=E5=90=84=E3=82=B9=E3=82=AD=E3=83=AB?= =?UTF-8?q?=E3=81=A7=E3=82=82=E6=96=B0=E3=81=97=E3=81=84=E9=96=A2=E6=95=B0?= =?UTF-8?q?=E3=81=AE=E5=BD=A2=E3=81=AB=E5=AF=BE=E5=BF=9C=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/goalie.hpp | 39 ++--- .../include/crane_robot_skills/idle.hpp | 10 +- .../crane_robot_skills/kickoff_attack.hpp | 21 +-- .../crane_robot_skills/kickoff_support.hpp | 10 +- .../include/crane_robot_skills/marker.hpp | 8 +- .../crane_robot_skills/move_to_geometry.hpp | 6 +- .../include/crane_robot_skills/sleep.hpp | 6 +- .../crane_robot_skills/turn_around_point.hpp | 14 +- crane_robot_skills/src/get_ball_contact.cpp | 12 +- crane_robot_skills/src/go_over_ball.cpp | 12 +- crane_robot_skills/src/goalie.cpp | 8 +- crane_robot_skills/src/move_with_ball.cpp | 20 +-- crane_robot_skills/src/receiver.cpp | 22 ++- .../src/robot_command_as_skill.cpp | 144 +++++------------- crane_robot_skills/src/simple_attacker.cpp | 22 ++- .../src/single_ball_placement.cpp | 30 +--- 16 files changed, 120 insertions(+), 264 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp index 160ccee75..82d2c0633 100644 --- a/crane_robot_skills/include/crane_robot_skills/goalie.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goalie.hpp @@ -20,11 +20,11 @@ class Goalie : public SkillBase<> public: explicit Goalie(uint8_t id, const std::shared_ptr & wm); - void emitBallFromPenaltyArea(crane::RobotCommandWrapper & target) + void emitBallFromPenaltyArea(crane::RobotCommandWrapper::SharedPtr & command) { auto ball = world_model->ball.pos; // パスできるロボットのリストアップ - auto passable_robot_list = world_model->ours.getAvailableRobots(target.robot->id); + auto passable_robot_list = world_model->ours.getAvailableRobots(command->robot->id); passable_robot_list.erase( std::remove_if( passable_robot_list.begin(), passable_robot_list.end(), @@ -52,25 +52,26 @@ class Goalie : public SkillBase<> Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f; double angle_ball_to_target = getAngle(pass_target - ball); - double dot = (world_model->ball.pos - target.robot->pose.pos) + double dot = (world_model->ball.pos - command->robot->pose.pos) .normalized() .dot((pass_target - world_model->ball.pos).normalized()); // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 if ( - dot < 0.95 || std::abs(getAngleDiff(angle_ball_to_target, target.robot->pose.theta)) > 0.05) { - target.setTargetPosition(intermediate_point); - target.enableCollisionAvoidance(); + dot < 0.95 || + std::abs(getAngleDiff(angle_ball_to_target, command->robot->pose.theta)) > 0.05) { + command->setTargetPosition(intermediate_point); + command->enableCollisionAvoidance(); } else { - target.setTargetPosition(world_model->ball.pos); - target.kickWithChip(0.4).disableCollisionAvoidance(); - target.enableCollisionAvoidance(); - target.disableBallAvoidance(); + command->setTargetPosition(world_model->ball.pos); + command->kickWithChip(0.4).disableCollisionAvoidance(); + command->enableCollisionAvoidance(); + command->disableBallAvoidance(); } - target.setTargetTheta(getAngle(pass_target - ball)); - target.disableGoalAreaAvoidance(); + command->setTargetTheta(getAngle(pass_target - ball)); + command->disableGoalAreaAvoidance(); } - void inplay(crane::RobotCommandWrapper & target, bool enable_emit = true) + void inplay(crane::RobotCommandWrapper::SharedPtr & command, bool enable_emit = true) { auto goals = world_model->getOurGoalPosts(); auto ball = world_model->ball.pos; @@ -83,14 +84,14 @@ class Goalie : public SkillBase<> // シュートブロック phase = "シュートブロック"; ClosestPoint result; - bg::closest_point(ball_line, target.robot->pose.pos, result); - target.setTargetPosition(result.closest_point); - target.setTargetTheta(getAngle(-world_model->ball.vel)); + bg::closest_point(ball_line, command->robot->pose.pos, result); + command->setTargetPosition(result.closest_point); + command->setTargetTheta(getAngle(-world_model->ball.vel)); } else { if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball) && enable_emit) { // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す phase = "ボール排出"; - emitBallFromPenaltyArea(target); + emitBallFromPenaltyArea(command); } else { phase = ""; const double BLOCK_DIST = 0.15; @@ -103,8 +104,8 @@ class Goalie : public SkillBase<> phase = "ボールを待ち受ける"; Point goal_center = world_model->getOurGoalCenter(); goal_center << goals.first.x(), 0.0f; - target.setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST); - target.lookAtBall(); + command->setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST); + command->lookAtBall(); } } } diff --git a/crane_robot_skills/include/crane_robot_skills/idle.hpp b/crane_robot_skills/include/crane_robot_skills/idle.hpp index 27df2311d..d18dd765b 100644 --- a/crane_robot_skills/include/crane_robot_skills/idle.hpp +++ b/crane_robot_skills/include/crane_robot_skills/idle.hpp @@ -21,16 +21,12 @@ class Idle : public SkillBase<> { setParameter("stop_by_position", true); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { // TODO(HansRobo): モーターをOFFにするようにしたほうがバッテリーに優しいかも if (getParameter("stop_by_position")) { - command.stopHere(); + command->stopHere(); } else { - command.setVelocity(0., 0.); + command->setVelocity(0., 0.); } return Status::RUNNING; }); diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp index 363ecc64e..f0afcfc59 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp @@ -29,16 +29,13 @@ class KickoffAttack : public SkillBase setParameter("kick_power", 0.5); addStateFunction( KickoffAttackState::PREPARE_KICKOFF, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not go_over_ball) { go_over_ball = std::make_shared(robot->id, world_model); go_over_ball->setParameter("next_target_x", getParameter("target_x")); go_over_ball->setParameter("next_target_y", getParameter("target_y")); go_over_ball->setParameter("margin", 0.3); - command.setMaxVelocity(0.5); + command->setMaxVelocity(0.5); } go_over_ball_status = go_over_ball->run(visualizer); return Status::RUNNING; @@ -48,15 +45,11 @@ class KickoffAttack : public SkillBase [this]() -> bool { return go_over_ball_status == Status::SUCCESS; }); addStateFunction( - KickoffAttackState::KICKOFF, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxVelocity(0.5); - command.kickStraight(getParameter("kick_power")); - command.setTargetPosition(world_model->ball.pos); - command.setTerminalVelocity(0.5); + KickoffAttackState::KICKOFF, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxVelocity(0.5); + command->kickStraight(getParameter("kick_power")); + command->setTargetPosition(world_model->ball.pos); + command->setTerminalVelocity(0.5); if (world_model->ball.vel.norm() > 0.3) { return Status::SUCCESS; } else { diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp index 959509b22..686b3f17d 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp @@ -22,14 +22,10 @@ class KickoffSupport : public SkillBase<> setParameter("target_x", 0.0f); setParameter("target_y", 1.0f); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target(getParameter("target_x"), getParameter("target_y")); - command.setTargetPosition(target); - command.lookAtBallFrom(target); + command->setTargetPosition(target); + command->lookAtBallFrom(target); return Status::RUNNING; }); } diff --git a/crane_robot_skills/include/crane_robot_skills/marker.hpp b/crane_robot_skills/include/crane_robot_skills/marker.hpp index 18691d17a..7f0b47e67 100644 --- a/crane_robot_skills/include/crane_robot_skills/marker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/marker.hpp @@ -28,11 +28,7 @@ class Marker : public SkillBase<> setParameter("mark_distance", 0.5); setParameter("mark_mode", "save_goal"); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { auto marked_robot = world_model->getTheirRobot(getParameter("marking_robot_id")); auto enemy_pos = marked_robot->pose.pos; @@ -48,7 +44,7 @@ class Marker : public SkillBase<> } else { throw std::runtime_error("unknown mark mode"); } - command.setTargetPosition(marking_point, target_theta); + command->setTargetPosition(marking_point, target_theta); return Status::RUNNING; }); } diff --git a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp index dbe112d6e..45dc91e64 100644 --- a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp +++ b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp @@ -25,11 +25,7 @@ class MoveToGeometry : public SkillBase<> { setParameter("reach_threshold", 0.1); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if ((robot->pose.pos - getTargetPoint()).norm() < getParameter("reach_threshold")) { return Status::SUCCESS; } else { diff --git a/crane_robot_skills/include/crane_robot_skills/sleep.hpp b/crane_robot_skills/include/crane_robot_skills/sleep.hpp index 9734da0be..073418fe6 100644 --- a/crane_robot_skills/include/crane_robot_skills/sleep.hpp +++ b/crane_robot_skills/include/crane_robot_skills/sleep.hpp @@ -21,11 +21,7 @@ class Sleep : public SkillBase<> { setParameter("duration", 0.0); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not is_started) { start_time = std::chrono::steady_clock::now(); is_started = true; diff --git a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp index 497148138..589d34711 100644 --- a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp +++ b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp @@ -32,11 +32,7 @@ class TurnAroundPoint : public SkillBase<> setParameter("max_velocity", 0.5); setParameter("max_turn_omega", M_PI_4); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target_point(getParameter("target_x"), getParameter("target_y")); double target_angle = getParameter("target_angle"); if (target_distance < 0.0) { @@ -53,7 +49,7 @@ class TurnAroundPoint : public SkillBase<> } if (std::abs(getAngleDiff(getAngle(robot->pose.pos - target_point), target_angle)) < 0.1) { - command.stopHere(); + command->stopHere(); return Status::SUCCESS; } else { // 円弧を描いて移動する @@ -71,14 +67,14 @@ class TurnAroundPoint : public SkillBase<> (dr * getParameter("dr_p_gain"))) + getNormVec(current_angle + std::copysign(M_PI_2, angle_diff)) * std::min(max_velocity, std::abs(angle_diff * 0.6)); - command.setVelocity(velocity); + command->setVelocity(velocity); // current_target_angle += std::copysign(max_turn_omega / 30.0f, angle_diff); - // command.setTargetPosition( + // command->setTargetPosition( // target_point + getNormVec(current_target_angle) * target_distance); // 中心点の方を向く - command.setTargetTheta(normalizeAngle(current_angle + M_PI)); + command->setTargetTheta(normalizeAngle(current_angle + M_PI)); return Status::RUNNING; } }); diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp index 5d2f3cb23..1eee31205 100644 --- a/crane_robot_skills/src/get_ball_contact.cpp +++ b/crane_robot_skills/src/get_ball_contact.cpp @@ -14,11 +14,7 @@ GetBallContact::GetBallContact(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { // 規定時間以上接していたらOK std::cout << "ContactDuration: " << std::chrono::duration_cast( @@ -36,9 +32,9 @@ GetBallContact::GetBallContact(uint8_t id, const std::shared_ptrball.pos); - command.setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); - command.dribble(getParameter("dribble_power")); + command->setDribblerTargetPosition(world_model->ball.pos); + command->setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); + command->dribble(getParameter("dribble_power")); return Status::RUNNING; } }); diff --git a/crane_robot_skills/src/go_over_ball.cpp b/crane_robot_skills/src/go_over_ball.cpp index c5a0b3aab..7ddf62340 100644 --- a/crane_robot_skills/src/go_over_ball.cpp +++ b/crane_robot_skills/src/go_over_ball.cpp @@ -16,11 +16,7 @@ GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wm setParameter("margin", 0.5); setParameter("reach_threshold", 0.05); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not has_started) { Point next_target{ getParameter("next_target_x"), getParameter("next_target_y")}; @@ -32,7 +28,7 @@ GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wm has_started = true; } - command.lookAtBallFrom(final_target_pos); + command->lookAtBallFrom(final_target_pos); auto final_distance = (robot->pose.pos - final_target_pos).norm(); auto [intermediate_distance, intermediate_point] = [&]() { @@ -46,13 +42,13 @@ GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wm }(); if (intermediate_distance < final_distance && not has_passed_intermediate_target) { - command.setTargetPosition(intermediate_point); + command->setTargetPosition(intermediate_point); if (intermediate_distance < getParameter("reach_threshold")) { std::cout << "Reached intermediate target" << std::endl; has_passed_intermediate_target = true; } } else { - command.setTargetPosition(final_target_pos); + command->setTargetPosition(final_target_pos); } if (final_distance < getParameter("reach_threshold")) { diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index b78b0d666..84b2dcf21 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -13,11 +13,7 @@ Goalie::Goalie(uint8_t id, const std::shared_ptr & wm) { setParameter("run_inplay", true); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { auto situation = world_model->play_situation.getSituationCommandID(); if (getParameter("run_inplay")) { situation = crane_msgs::msg::PlaySituation::INPLAY; @@ -25,7 +21,7 @@ Goalie::Goalie(uint8_t id, const std::shared_ptr & wm) switch (situation) { case crane_msgs::msg::PlaySituation::HALT: phase = "HALT, stop here"; - command.stopHere(); + command->stopHere(); break; case crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION: [[fallthrough]]; diff --git a/crane_robot_skills/src/move_with_ball.cpp b/crane_robot_skills/src/move_with_ball.cpp index e613768ae..fd742829f 100644 --- a/crane_robot_skills/src/move_with_ball.cpp +++ b/crane_robot_skills/src/move_with_ball.cpp @@ -26,16 +26,12 @@ MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr setParameter("dribble_target_horizon", 0.2); setParameter("ball_stabilizing_time", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxVelocity(0.5); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxVelocity(0.5); auto target_pos = parseTargetPoint(); - command.setDribblerTargetPosition(target_pos); + command->setDribblerTargetPosition(target_pos); target_theta = getAngle(target_pos - robot->pose.pos); - command.setTargetTheta(target_theta); + command->setTargetTheta(target_theta); // 開始時にボールに接していることが前提にある if (not robot->ball_contact.findPastContact(getParameter("ball_lost_timeout"))) { // ボールが離れたら失敗 @@ -48,17 +44,17 @@ MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr if ( getElapsedSec(*ball_stabilizing_start_time) > getParameter("ball_stabilizing_time")) { - command.dribble(0.0); + command->dribble(0.0); return Status::SUCCESS; } else { return Status::RUNNING; } } else { phase = "目標位置に向かっています"; - command.setTargetPosition(getTargetPoint(target_pos)); + command->setTargetPosition(getTargetPoint(target_pos)); // 目標姿勢の角度ではなく、目標位置の方向を向いて進む - command.setTargetTheta(target_theta); - command.dribble(getParameter("dribble_power")); + command->setTargetTheta(target_theta); + command->dribble(getParameter("dribble_power")); return Status::RUNNING; } }); diff --git a/crane_robot_skills/src/receiver.cpp b/crane_robot_skills/src/receiver.cpp index 422b0e3a8..f45cc882d 100644 --- a/crane_robot_skills/src/receiver.cpp +++ b/crane_robot_skills/src/receiver.cpp @@ -17,17 +17,13 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) setParameter("ball_vel_threshold", 0.5); setParameter("kicker_power", 0.7); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - auto dpps_points = getDPPSPoints(world_model->ball.pos, 0.25, 16); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + auto dpps_points = getDPPSPoints(this->world_model->ball.pos, 0.25, 16); // モード判断 // こちらへ向かう速度成分 float ball_vel = world_model->ball.vel.dot((robot->pose.pos - world_model->ball.pos).normalized()); - command.kickStraight(getParameter("kicker_power")); + command->kickStraight(getParameter("kicker_power")); if (ball_vel > getParameter("ball_vel_threshold")) { Segment ball_line( world_model->ball.pos, @@ -39,7 +35,7 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) // シュートをブロックしない // TODO(HansRobo): これでは延長線上に相手ゴールのあるパスが全くできなくなるので要修正 if (bg::intersects(ball_line, goal_line)) { - command.stopHere(); + command->stopHere(); } else { // ボールの進路上に移動 ClosestPoint result; @@ -51,10 +47,10 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) auto to_goal = getNormVec(goal_angle); auto to_ball = (world_model->ball.pos - result.closest_point).normalized(); double intermediate_angle = getAngle(2 * to_goal + to_ball); - command.setTargetTheta(intermediate_angle); + command->setTargetTheta(intermediate_angle); // キッカーの中心のためのオフセット - command.setTargetPosition( + command->setTargetPosition( result.closest_point - (2 * to_goal + to_ball).normalized() * 0.06); } } else { @@ -87,16 +83,16 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) best_position = dpps_point; } } - command.setTargetPosition(best_position); + command->setTargetPosition(best_position); // target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); } // ゴールとボールの中間方向を向く - Point target_pos{command.latest_msg.target_x.front(), command.latest_msg.target_y.front()}; + Point target_pos{command->latest_msg.target_x.front(), command->latest_msg.target_y.front()}; auto goal_angle = getLargestGoalAngleFromPosition(target_pos); auto to_goal = getNormVec(goal_angle); auto to_ball = (world_model->ball.pos - target_pos).normalized(); - command.setTargetTheta(getAngle(to_goal + to_ball)); + command->setTargetTheta(getAngle(to_goal + to_ball)); return Status::RUNNING; }); diff --git a/crane_robot_skills/src/robot_command_as_skill.cpp b/crane_robot_skills/src/robot_command_as_skill.cpp index 732799e24..e5582c941 100644 --- a/crane_robot_skills/src/robot_command_as_skill.cpp +++ b/crane_robot_skills/src/robot_command_as_skill.cpp @@ -26,12 +26,8 @@ CmdKickWithChip::CmdKickWithChip(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.kickWithChip(getParameter("power")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->kickWithChip(getParameter("power")); return Status::SUCCESS; }); } @@ -47,12 +43,8 @@ CmdKickStraight::CmdKickStraight(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.kickStraight(getParameter("power")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->kickStraight(getParameter("power")); return Status::SUCCESS; }); } @@ -68,12 +60,8 @@ CmdDribble::CmdDribble(uint8_t id, const std::shared_ptr & wm { setParameter("power", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.dribble(getParameter("power")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->dribble(getParameter("power")); return Status::SUCCESS; }); } @@ -90,12 +78,8 @@ CmdSetVelocity::CmdSetVelocity(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setVelocity(getParameter("x"), getParameter("y")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setVelocity(getParameter("x"), getParameter("y")); return Status::SUCCESS; }); } @@ -114,13 +98,9 @@ CmdSetTargetPosition::CmdSetTargetPosition( setParameter("reach_threshold", 0.1); setParameter("exit_immediately", false); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.setTargetPosition(target); + command->setTargetPosition(target); if (getParameter("exit_immediately")) { return Status::SUCCESS; } else { @@ -148,13 +128,9 @@ CmdSetDribblerTargetPosition::CmdSetDribblerTargetPosition( setParameter("reach_threshold", 0.1); setParameter("exit_immediately", false); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.setDribblerTargetPosition(target); + command->setDribblerTargetPosition(target); if (getParameter("exit_immediately")) { return Status::SUCCESS; } else { @@ -179,12 +155,8 @@ CmdSetTargetTheta::CmdSetTargetTheta(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setTargetTheta(getParameter("theta")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setTargetTheta(getParameter("theta")); return Status::SUCCESS; }); } @@ -198,12 +170,8 @@ CmdStopHere::CmdStopHere(uint8_t id, const std::shared_ptr & : SkillBase<>("CmdStopHere", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.stopHere(); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->stopHere(); return Status::SUCCESS; }); } @@ -227,12 +195,8 @@ CmdSetMaxVelocity::CmdSetMaxVelocity(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxVelocity(getParameter("max_velocity")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxVelocity(getParameter("max_velocity")); return Status::SUCCESS; }); } @@ -248,12 +212,8 @@ CmdSetMaxAcceleration::CmdSetMaxAcceleration( { setParameter("max_acceleration", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxAcceleration(getParameter("max_acceleration")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxAcceleration(getParameter("max_acceleration")); return Status::SUCCESS; }); } @@ -268,12 +228,8 @@ CmdSetMaxOmega::CmdSetMaxOmega(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setMaxOmega(getParameter("max_omega")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setMaxOmega(getParameter("max_omega")); return Status::SUCCESS; }); } @@ -289,12 +245,8 @@ CmdSetTerminalVelocity::CmdSetTerminalVelocity( { setParameter("terminal_velocity", 0.5); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.setTerminalVelocity(getParameter("terminal_velocity")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->setTerminalVelocity(getParameter("terminal_velocity")); return Status::SUCCESS; }); } @@ -308,12 +260,8 @@ CmdEnableStopFlag::CmdEnableStopFlag(uint8_t id, const std::shared_ptr("CmdEnableStopFlag ", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.stopEmergency(true); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->stopEmergency(true); return Status::SUCCESS; }); } @@ -324,12 +272,8 @@ CmdDisableStopFlag::CmdDisableStopFlag(uint8_t id, const std::shared_ptr("CmdDisableStopFlag", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.stopEmergency(false); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->stopEmergency(false); return Status::SUCCESS; }); } @@ -341,12 +285,8 @@ CmdLiftUpDribbler::CmdLiftUpDribbler(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.liftUpDribbler(getParameter("enable")); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->liftUpDribbler(getParameter("enable")); return Status::SUCCESS; }); } @@ -362,13 +302,9 @@ CmdLookAt::CmdLookAt(uint8_t id, const std::shared_ptr & wm) setParameter("x", 0.0); setParameter("y", 0.0); addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.lookAt(target); + command->lookAt(target); return Status::SUCCESS; }); } @@ -382,12 +318,8 @@ CmdLookAtBall::CmdLookAtBall(uint8_t id, const std::shared_ptr("CmdLookAtBall", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { - command.lookAtBall(); + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->lookAtBall(); return Status::SUCCESS; }); } @@ -400,13 +332,9 @@ CmdLookAtBallFrom::CmdLookAtBallFrom(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { Point target{getParameter("x"), getParameter("y")}; - command.lookAtBallFrom(target); + command->lookAtBallFrom(target); return Status::SUCCESS; }); } diff --git a/crane_robot_skills/src/simple_attacker.cpp b/crane_robot_skills/src/simple_attacker.cpp index fd6d9af46..14f517695 100644 --- a/crane_robot_skills/src/simple_attacker.cpp +++ b/crane_robot_skills/src/simple_attacker.cpp @@ -12,11 +12,7 @@ SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptr("SimpleAttacker", id, wm, DefaultStates::DEFAULT) { addStateFunction( - DefaultStates::DEFAULT, - [this]( - const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { auto [best_target, goal_angle_width] = getBestShootTargetWithWidth(); // シュートの隙がないときは仲間へパス @@ -38,22 +34,22 @@ SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptrball.pos); // ボールと敵ゴールの延長線上にいない && 角度があってないときは，中間ポイントを経由 if (dot < 0.95 || std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.05) { - command.setTargetPosition(intermediate_point); - command.enableCollisionAvoidance(); + command->setTargetPosition(intermediate_point); + command->enableCollisionAvoidance(); } else { - command.setTargetPosition(world_model->ball.pos); - command.kickStraight(0.7).disableCollisionAvoidance(); - command.enableCollisionAvoidance(); - command.disableBallAvoidance(); + command->setTargetPosition(world_model->ball.pos); + command->kickStraight(0.7).disableCollisionAvoidance(); + command->enableCollisionAvoidance(); + command->disableBallAvoidance(); } - command.setTargetTheta(getAngle(best_target - world_model->ball.pos)); + command->setTargetTheta(getAngle(best_target - 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) or is_in_defense) { - command.stopHere(); + command->stopHere(); } return Status::RUNNING; }); diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 36f5275cb..62b9e124f 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -17,10 +17,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not go_over_ball) { go_over_ball = std::make_shared(robot->id, world_model); go_over_ball->setParameter("next_target_x", getParameter("placement_x")); @@ -39,10 +36,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not get_ball_contact) { get_ball_contact = std::make_shared(robot->id, world_model); } @@ -58,10 +52,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not move_with_ball) { move_with_ball = std::make_shared(robot->id, world_model); move_with_ball->setParameter("target_x", getParameter("placement_x")); @@ -79,10 +70,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not sleep) { sleep = std::make_shared(robot->id, world_model); } @@ -96,10 +84,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not sleep) { sleep = std::make_shared(robot->id, world_model); sleep->setParameter("duration", 0.5); @@ -114,10 +99,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr & world_model, - const std::shared_ptr & robot, crane::RobotCommandWrapper & command, - ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { if (not set_target_position) { set_target_position = std::make_shared(robot->id, world_model); } From 374844ab435abc76430438386eb1d65b3677d906 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 23:34:33 +0900 Subject: [PATCH 15/15] =?UTF-8?q?=E5=85=A5=E3=82=8C=E5=AD=90=E3=82=B9?= =?UTF-8?q?=E3=82=AD=E3=83=AB=E3=81=AE=E3=82=B3=E3=83=9E=E3=83=B3=E3=83=89?= =?UTF-8?q?=E3=81=AE=E6=89=B1=E3=81=84=E3=81=AB=E6=B3=A8=E6=84=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/kickoff_attack.hpp | 3 +++ .../include/crane_robot_skills/skill_base.hpp | 5 +++++ crane_robot_skills/src/single_ball_placement.cpp | 6 ++++++ 3 files changed, 14 insertions(+) diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp index f0afcfc59..3c1fe2617 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp @@ -30,8 +30,10 @@ class KickoffAttack : public SkillBase addStateFunction( KickoffAttackState::PREPARE_KICKOFF, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + std::cout << "KickoffAttackState::PREPARE_KICKOFF" << std::endl; if (not go_over_ball) { go_over_ball = std::make_shared(robot->id, world_model); + go_over_ball->setCommander(command); go_over_ball->setParameter("next_target_x", getParameter("target_x")); go_over_ball->setParameter("next_target_y", getParameter("target_y")); go_over_ball->setParameter("margin", 0.3); @@ -46,6 +48,7 @@ class KickoffAttack : public SkillBase addStateFunction( KickoffAttackState::KICKOFF, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + std::cout << "KickoffAttackState::KICKOFF" << std::endl; command->setMaxVelocity(0.5); command->kickStraight(getParameter("kick_power")); command->setTargetPosition(world_model->ball.pos); diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 47729903d..c71dafba3 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -168,6 +168,11 @@ class SkillBase : public SkillInterface } } + void setCommander(const std::shared_ptr & commander) + { + this->command = commander; + } + Status run( ConsaiVisualizerWrapper::SharedPtr visualizer, std::optional> parameters_opt = diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 62b9e124f..1e9e446ec 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -20,6 +20,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr Status { if (not go_over_ball) { go_over_ball = std::make_shared(robot->id, world_model); + go_over_ball->setCommander(command); go_over_ball->setParameter("next_target_x", getParameter("placement_x")); go_over_ball->setParameter("next_target_y", getParameter("placement_y")); go_over_ball->setParameter("margin", 0.4); @@ -39,6 +40,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr Status { if (not get_ball_contact) { get_ball_contact = std::make_shared(robot->id, world_model); + get_ball_contact->setCommander(command); } skill_status = get_ball_contact->run(visualizer); @@ -55,6 +57,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr Status { if (not move_with_ball) { move_with_ball = std::make_shared(robot->id, world_model); + move_with_ball->setCommander(command); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); } @@ -73,6 +76,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr Status { if (not sleep) { sleep = std::make_shared(robot->id, world_model); + sleep->setCommander(command); } skill_status = sleep->run(visualizer); return Status::RUNNING; @@ -87,6 +91,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr Status { if (not sleep) { sleep = std::make_shared(robot->id, world_model); + sleep->setCommander(command); sleep->setParameter("duration", 0.5); } skill_status = sleep->run(visualizer); @@ -102,6 +107,7 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptr Status { if (not set_target_position) { set_target_position = std::make_shared(robot->id, world_model); + set_target_position->setCommander(command); } // メモ：().normalized() * 0.6したらなぜかゼロベクトルが出来上がってしまう Vector2 diff = (robot->pose.pos - world_model->ball.pos);