Skip to content

Commit d6a6307

Browse files
HansRoboHansRobo
authored
Merge pull request #157 from ibis-ssl/refactor/command_in_skillbase
SkillBase::runからcommandをなくすのだ
2 parents f02bb14 + 374844a commit d6a6307

31 files changed

+279
-619
lines changed

‎crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ namespace crane::skills
1616
class GetBallContact : public SkillBase<>
1717
{
1818
public:
19-
explicit GetBallContact(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
19+
explicit GetBallContact(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
2020

2121
void print(std::ostream & out) const override
2222
{

‎crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ namespace crane::skills
1818
class GoOverBall : public SkillBase<>
1919
{
2020
public:
21-
explicit GoOverBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
21+
explicit GoOverBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
2222

2323
void print(std::ostream & out) const override
2424
{

‎crane_robot_skills/include/crane_robot_skills/goalie.hpp

+21-20
Original file line numberDiff line numberDiff line change
@@ -18,13 +18,13 @@ namespace crane::skills
1818
class Goalie : public SkillBase<>
1919
{
2020
public:
21-
explicit Goalie(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
21+
explicit Goalie(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
2222

23-
void emitBallFromPenaltyArea(crane::RobotCommandWrapper & target)
23+
void emitBallFromPenaltyArea(crane::RobotCommandWrapper::SharedPtr & command)
2424
{
2525
auto ball = world_model->ball.pos;
2626
// パスできるロボットのリストアップ
27-
auto passable_robot_list = world_model->ours.getAvailableRobots(target.robot->id);
27+
auto passable_robot_list = world_model->ours.getAvailableRobots(command->robot->id);
2828
passable_robot_list.erase(
2929
std::remove_if(
3030
passable_robot_list.begin(), passable_robot_list.end(),
@@ -52,25 +52,26 @@ class Goalie : public SkillBase<>
5252

5353
Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f;
5454
double angle_ball_to_target = getAngle(pass_target - ball);
55-
double dot = (world_model->ball.pos - target.robot->pose.pos)
55+
double dot = (world_model->ball.pos - command->robot->pose.pos)
5656
.normalized()
5757
.dot((pass_target - world_model->ball.pos).normalized());
5858
// ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由
5959
if (
60-
dot < 0.95 || std::abs(getAngleDiff(angle_ball_to_target, target.robot->pose.theta)) > 0.05) {
61-
target.setTargetPosition(intermediate_point);
62-
target.enableCollisionAvoidance();
60+
dot < 0.95 ||
61+
std::abs(getAngleDiff(angle_ball_to_target, command->robot->pose.theta)) > 0.05) {
62+
command->setTargetPosition(intermediate_point);
63+
command->enableCollisionAvoidance();
6364
} else {
64-
target.setTargetPosition(world_model->ball.pos);
65-
target.kickWithChip(0.4).disableCollisionAvoidance();
66-
target.enableCollisionAvoidance();
67-
target.disableBallAvoidance();
65+
command->setTargetPosition(world_model->ball.pos);
66+
command->kickWithChip(0.4).disableCollisionAvoidance();
67+
command->enableCollisionAvoidance();
68+
command->disableBallAvoidance();
6869
}
69-
target.setTargetTheta(getAngle(pass_target - ball));
70-
target.disableGoalAreaAvoidance();
70+
command->setTargetTheta(getAngle(pass_target - ball));
71+
command->disableGoalAreaAvoidance();
7172
}
7273

73-
void inplay(crane::RobotCommandWrapper & target, bool enable_emit = true)
74+
void inplay(crane::RobotCommandWrapper::SharedPtr & command, bool enable_emit = true)
7475
{
7576
auto goals = world_model->getOurGoalPosts();
7677
auto ball = world_model->ball.pos;
@@ -83,14 +84,14 @@ class Goalie : public SkillBase<>
8384
// シュートブロック
8485
phase = "シュートブロック";
8586
ClosestPoint result;
86-
bg::closest_point(ball_line, target.robot->pose.pos, result);
87-
target.setTargetPosition(result.closest_point);
88-
target.setTargetTheta(getAngle(-world_model->ball.vel));
87+
bg::closest_point(ball_line, command->robot->pose.pos, result);
88+
command->setTargetPosition(result.closest_point);
89+
command->setTargetTheta(getAngle(-world_model->ball.vel));
8990
} else {
9091
if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball) && enable_emit) {
9192
// ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す
9293
phase = "ボール排出";
93-
emitBallFromPenaltyArea(target);
94+
emitBallFromPenaltyArea(command);
9495
} else {
9596
phase = "";
9697
const double BLOCK_DIST = 0.15;
@@ -103,8 +104,8 @@ class Goalie : public SkillBase<>
103104
phase = "ボールを待ち受ける";
104105
Point goal_center = world_model->getOurGoalCenter();
105106
goal_center << goals.first.x(), 0.0f;
106-
target.setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST);
107-
target.lookAtBall();
107+
command->setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST);
108+
command->lookAtBall();
108109
}
109110
}
110111
}

‎crane_robot_skills/include/crane_robot_skills/idle.hpp

+5-9
Original file line numberDiff line numberDiff line change
@@ -16,21 +16,17 @@ namespace crane::skills
1616
class Idle : public SkillBase<>
1717
{
1818
public:
19-
explicit Idle(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
20-
: SkillBase<>("Idle", id, world_model, DefaultStates::DEFAULT)
19+
explicit Idle(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
20+
: SkillBase<>("Idle", id, wm, DefaultStates::DEFAULT)
2121
{
2222
setParameter("stop_by_position", true);
2323
addStateFunction(
24-
DefaultStates::DEFAULT,
25-
[this](
26-
const std::shared_ptr<WorldModelWrapper> & world_model,
27-
const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
28-
ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
24+
DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
2925
// TODO(HansRobo): モーターをOFFにするようにしたほうがバッテリーに優しいかも
3026
if (getParameter<bool>("stop_by_position")) {
31-
command.stopHere();
27+
command->stopHere();
3228
} else {
33-
command.setVelocity(0., 0.);
29+
command->setVelocity(0., 0.);
3430
}
3531
return Status::RUNNING;
3632
});

‎crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp

+13-18
Original file line numberDiff line numberDiff line change
@@ -21,43 +21,38 @@ enum class KickoffAttackState {
2121
class KickoffAttack : public SkillBase<KickoffAttackState>
2222
{
2323
public:
24-
explicit KickoffAttack(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
25-
: SkillBase<KickoffAttackState>(
26-
"KickoffAttack", id, world_model, KickoffAttackState::PREPARE_KICKOFF)
24+
explicit KickoffAttack(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
25+
: SkillBase<KickoffAttackState>("KickoffAttack", id, wm, KickoffAttackState::PREPARE_KICKOFF)
2726
{
2827
setParameter("target_x", 0.0f);
2928
setParameter("target_y", 1.0f);
3029
setParameter("kick_power", 0.5);
3130
addStateFunction(
3231
KickoffAttackState::PREPARE_KICKOFF,
33-
[this](
34-
const std::shared_ptr<WorldModelWrapper> & world_model,
35-
const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
36-
ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
32+
[this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
33+
std::cout << "KickoffAttackState::PREPARE_KICKOFF" << std::endl;
3734
if (not go_over_ball) {
3835
go_over_ball = std::make_shared<GoOverBall>(robot->id, world_model);
36+
go_over_ball->setCommander(command);
3937
go_over_ball->setParameter("next_target_x", getParameter<double>("target_x"));
4038
go_over_ball->setParameter("next_target_y", getParameter<double>("target_y"));
4139
go_over_ball->setParameter("margin", 0.3);
42-
command.setMaxVelocity(0.5);
40+
command->setMaxVelocity(0.5);
4341
}
44-
go_over_ball_status = go_over_ball->run(command, visualizer);
42+
go_over_ball_status = go_over_ball->run(visualizer);
4543
return Status::RUNNING;
4644
});
4745
addTransition(
4846
KickoffAttackState::PREPARE_KICKOFF, KickoffAttackState::KICKOFF,
4947
[this]() -> bool { return go_over_ball_status == Status::SUCCESS; });
5048

5149
addStateFunction(
52-
KickoffAttackState::KICKOFF,
53-
[this](
54-
const std::shared_ptr<WorldModelWrapper> & world_model,
55-
const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
56-
ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
57-
command.setMaxVelocity(0.5);
58-
command.kickStraight(getParameter<double>("kick_power"));
59-
command.setTargetPosition(world_model->ball.pos);
60-
command.setTerminalVelocity(0.5);
50+
KickoffAttackState::KICKOFF, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
51+
std::cout << "KickoffAttackState::KICKOFF" << std::endl;
52+
command->setMaxVelocity(0.5);
53+
command->kickStraight(getParameter<double>("kick_power"));
54+
command->setTargetPosition(world_model->ball.pos);
55+
command->setTerminalVelocity(0.5);
6156
if (world_model->ball.vel.norm() > 0.3) {
6257
return Status::SUCCESS;
6358
} else {

‎crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp

+5-9
Original file line numberDiff line numberDiff line change
@@ -16,20 +16,16 @@ namespace crane::skills
1616
class KickoffSupport : public SkillBase<>
1717
{
1818
public:
19-
explicit KickoffSupport(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
20-
: SkillBase<>("KickoffSupport", id, world_model, DefaultStates::DEFAULT)
19+
explicit KickoffSupport(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
20+
: SkillBase<>("KickoffSupport", id, wm, DefaultStates::DEFAULT)
2121
{
2222
setParameter("target_x", 0.0f);
2323
setParameter("target_y", 1.0f);
2424
addStateFunction(
25-
DefaultStates::DEFAULT,
26-
[this](
27-
const std::shared_ptr<WorldModelWrapper> & world_model,
28-
const std::shared_ptr<RobotInfo> & robot, RobotCommandWrapper & command,
29-
ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
25+
DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
3026
Point target(getParameter<double>("target_x"), getParameter<double>("target_y"));
31-
command.setTargetPosition(target);
32-
command.lookAtBallFrom(target);
27+
command->setTargetPosition(target);
28+
command->lookAtBallFrom(target);
3329
return Status::RUNNING;
3430
});
3531
}

‎crane_robot_skills/include/crane_robot_skills/marker.hpp

+4-8
Original file line numberDiff line numberDiff line change
@@ -21,18 +21,14 @@ namespace crane::skills
2121
class Marker : public SkillBase<>
2222
{
2323
public:
24-
explicit Marker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
25-
: SkillBase<>("Marker", id, world_model, DefaultStates::DEFAULT)
24+
explicit Marker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
25+
: SkillBase<>("Marker", id, wm, DefaultStates::DEFAULT)
2626
{
2727
setParameter("marking_robot_id", 0);
2828
setParameter("mark_distance", 0.5);
2929
setParameter("mark_mode", "save_goal");
3030
addStateFunction(
31-
DefaultStates::DEFAULT,
32-
[this](
33-
const std::shared_ptr<WorldModelWrapper> & world_model,
34-
const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
35-
ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
31+
DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
3632
auto marked_robot = world_model->getTheirRobot(getParameter<int>("marking_robot_id"));
3733
auto enemy_pos = marked_robot->pose.pos;
3834

@@ -48,7 +44,7 @@ class Marker : public SkillBase<>
4844
} else {
4945
throw std::runtime_error("unknown mark mode");
5046
}
51-
command.setTargetPosition(marking_point, target_theta);
47+
command->setTargetPosition(marking_point, target_theta);
5248
return Status::RUNNING;
5349
});
5450
}

‎crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp

+2-6
Original file line numberDiff line numberDiff line change
@@ -21,15 +21,11 @@ class MoveToGeometry : public SkillBase<>
2121
public:
2222
explicit MoveToGeometry(
2323
uint8_t id, Geometry geometry, const std::shared_ptr<WorldModelWrapper> & world_model)
24-
: SkillBase<>("MoveToGeometry", id, world_model, DefaultStates::DEFAULT), geometry(geometry)
24+
: SkillBase<>("MoveToGeometry", id, wm, DefaultStates::DEFAULT), geometry(geometry)
2525
{
2626
setParameter("reach_threshold", 0.1);
2727
addStateFunction(
28-
DefaultStates::DEFAULT,
29-
[this](
30-
const std::shared_ptr<WorldModelWrapper> & world_model,
31-
const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
32-
ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
28+
DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
3329
if ((robot->pose.pos - getTargetPoint()).norm() < getParameter<double>("reach_threshold")) {
3430
return Status::SUCCESS;
3531
} else {

‎crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ enum class MoveWithBallStates { SUCCESS, RUNNING, FAILURE };
2626
class MoveWithBall : public SkillBase<>
2727
{
2828
public:
29-
explicit MoveWithBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
29+
explicit MoveWithBall(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
3030

3131
Point getTargetPoint(const Point & target_pos)
3232
{

‎crane_robot_skills/include/crane_robot_skills/receiver.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ class Receiver : public SkillBase<>
4040
double score;
4141
};
4242

43-
explicit Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
43+
explicit Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
4444

4545
void print(std::ostream & os) const override { os << "[Receiver]"; }
4646

‎crane_robot_skills/include/crane_robot_skills/robot_command_as_skill.hpp

+6-6
Original file line numberDiff line numberDiff line change
@@ -14,12 +14,12 @@
1414
namespace crane::skills
1515
{
1616

17-
#define DEFINE_SKILL_COMMAND(name) \
18-
class Cmd##name : public SkillBase<> \
19-
{ \
20-
public: \
21-
explicit Cmd##name(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model); \
22-
void print(std::ostream & os) const override; \
17+
#define DEFINE_SKILL_COMMAND(name) \
18+
class Cmd##name : public SkillBase<> \
19+
{ \
20+
public: \
21+
explicit Cmd##name(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm); \
22+
void print(std::ostream & os) const override; \
2323
}
2424

2525
DEFINE_SKILL_COMMAND(KickWithChip);

‎crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ namespace crane::skills
1818
class SimpleAttacker : public SkillBase<>
1919
{
2020
public:
21-
explicit SimpleAttacker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
21+
explicit SimpleAttacker(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
2222

2323
void print(std::ostream & os) const override
2424
{

‎crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ class SingleBallPlacement : public SkillBase<SingleBallPlacementStates>
4444
Status skill_status = Status::RUNNING;
4545

4646
public:
47-
explicit SingleBallPlacement(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model);
47+
explicit SingleBallPlacement(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
4848

4949
void print(std::ostream & os) const override
5050
{

0 commit comments

Comments
 (0)