Decision Module  1.0
ICRA2020 AI Challenge Northwestern Polytechnical University Aoxiang Team Strategy Code
wait_for_enemy_supply_area.h
Go to the documentation of this file.
1 //
2 // Created by heihei on 2020/4/13.
3 //
4 
5 #ifndef ICRA_FIREFLY_ROBORTS_WAIT_FOR_ENEMY_SUPPLY_AREA_H_
6 #define ICRA_FIREFLY_ROBORTS_WAIT_FOR_ENEMY_SUPPLY_AREA_H_
7 
8 #include "../blackboard/blackboard.h"
9 #include "../blackboard/my_robot.h"
10 #include "../behavior/robot_behaviors.h"
11 
12 namespace roborts_decision {
13 
14 class WaitForEnemySupplyArea {
15 public:
16  WaitForEnemySupplyArea(std::shared_ptr<MyRobot> p_my_robot1,
17  std::shared_ptr<MyRobot> p_my_robot2,
18  std::shared_ptr<Blackboard> p_blackboard,
19  std::shared_ptr<RobotBehaviors> p_robot1_behaviors,
20  std::shared_ptr<RobotBehaviors> p_robot2_behaviors)
21  : p_my_robot1(std::move(p_my_robot1)),
22  p_my_robot2(std::move(p_my_robot2)),
23  p_blackboard_(std::move(p_blackboard)),
24  p_robot1_behaviors(std::move(p_robot1_behaviors)),
25  p_robot2_behaviors(std::move(p_robot2_behaviors)) {
26  if (this->p_my_robot1->GetRobotType() > 10) {
27  this->buff_status_projection_supplier = RED_PROJECTILE_SUPPLIER;
28  this->buff_status_restoration = RED_RESTORATION;
29  } else {
30  this->buff_status_projection_supplier = BLUE_PROJECTILE_SUPPLIER;
31  this->buff_status_restoration = BLUE_RESTORATION;
32  }
33  }
34 
35  bool Run(EnemyBuffZone enemy_zone) {
36  int zone_id = 0;
37  if (enemy_zone == PROJECTILE_SUPPLIER) {
38  zone_id = this->p_blackboard_->isZoneActive(
39  this->buff_status_projection_supplier);
40  if (!zone_id) {
41  ROS_ERROR("The Zone is not active!");
42  return false;
43  } else {
44 
45  auto current_behavior = p_my_robot1->GetCurrentBehavior();
46  if (current_behavior != GOAL) {
47  CancelBehavior(p_my_robot1, p_robot1_behaviors);
48  }
49 
50  current_behavior = p_my_robot2->GetCurrentBehavior();
51  if (current_behavior != GOAL) {
52  CancelBehavior(p_my_robot2, p_robot2_behaviors);
53  }
54 
55  p_my_robot1->SetCurrentBehavior(GOAL);
56  p_my_robot2->SetCurrentBehavior(GOAL);
57  this->p_robot1_behaviors->GetGoalBehavior()->Run(
58  getGoalPose(zone_id).first);
59  this->p_robot2_behaviors->GetGoalBehavior()->Run(
60  getGoalPose(zone_id).second);
61  }
62  } else if (enemy_zone == RESTORATION) {
63  zone_id =
64  this->p_blackboard_->isZoneActive(this->buff_status_restoration);
65  if (!zone_id) {
66  ROS_ERROR("The Zone is not active!");
67  return false;
68  } else {
69 
70  auto current_behavior = p_my_robot1->GetCurrentBehavior();
71  if (current_behavior != GOAL) {
72  CancelBehavior(p_my_robot1, p_robot1_behaviors);
73  }
74 
75  current_behavior = p_my_robot2->GetCurrentBehavior();
76  if (current_behavior != GOAL) {
77  CancelBehavior(p_my_robot2, p_robot2_behaviors);
78  }
79 
80  p_my_robot1->SetCurrentBehavior(GOAL);
81  p_my_robot2->SetCurrentBehavior(GOAL);
82  this->p_robot1_behaviors->GetGoalBehavior()->Run(
83  getGoalPose(zone_id).first);
84  this->p_robot2_behaviors->GetGoalBehavior()->Run(
85  getGoalPose(zone_id).second);
86  }
87  }
88  return true;
89  }
90 
91  void Cancel() {
92  this->p_robot1_behaviors->GetGoalBehavior()->Cancel();
93  this->p_robot2_behaviors->GetGoalBehavior()->Cancel();
94  }
95 
96  static void
97  CancelBehavior(const std::shared_ptr<MyRobot> &myRobot,
98  const std::shared_ptr<RobotBehaviors> &robotBehaviors) {
99  auto current_behavior = myRobot->GetCurrentBehavior();
100  if (current_behavior == ESCAPE) {
101  robotBehaviors->GetEscapeBehavior()->Cancel();
102  } else if (current_behavior == SUPPLY) {
103  robotBehaviors->GetSupplyBehavior()->Cancel();
104  } else if (current_behavior == GOAL) {
105  robotBehaviors->GetGoalBehavior()->Cancel();
106  }
107  }
108 
109 private:
110  std::shared_ptr<MyRobot> p_my_robot1;
111  std::shared_ptr<MyRobot> p_my_robot2;
112  std::shared_ptr<Blackboard> p_blackboard_;
113 
114  std::shared_ptr<RobotBehaviors> p_robot1_behaviors;
115  std::shared_ptr<RobotBehaviors> p_robot2_behaviors;
116 
117  BuffStatus buff_status_restoration;
118  BuffStatus buff_status_projection_supplier;
119 
120  std::pair<geometry_msgs::PoseStamped, geometry_msgs::PoseStamped>
121  getGoalPose(int zone_id) {
122  geometry_msgs::PoseStamped zone_pose_1;
123  geometry_msgs::PoseStamped zone_pose_2;
124 
125  switch (zone_id) {
126  case 1: {
127  zone_pose_1.pose.position.x =
128  this->p_blackboard_->GetBuffZoneStatus().at(1).second.max_x_ + 1;
129  zone_pose_1.pose.position.y =
130  this->p_blackboard_->GetBuffZoneStatus().at(1).second.zone_y_;
131  zone_pose_1.pose.orientation.z = 1;
132 
133  zone_pose_2.pose.position.x =
134  this->p_blackboard_->GetBuffZoneStatus().at(1).second.zone_x_;
135  zone_pose_2.pose.position.y =
136  this->p_blackboard_->GetBuffZoneStatus().at(1).second.min_y_ - 1;
137  zone_pose_2.pose.orientation.z = 0.707;
138  zone_pose_2.pose.orientation.w = 0.707;
139  break;
140  }
141 
142  case 2: {
143  zone_pose_1.pose.position.x =
144  this->p_blackboard_->GetBuffZoneStatus().at(2).second.min_x_ - 1;
145  zone_pose_1.pose.position.y =
146  this->p_blackboard_->GetBuffZoneStatus().at(2).second.zone_y_;
147  zone_pose_1.pose.orientation.w = 1;
148 
149  zone_pose_2.pose.position.x =
150  this->p_blackboard_->GetBuffZoneStatus().at(2).second.max_x_ + 1;
151  zone_pose_2.pose.position.y =
152  this->p_blackboard_->GetBuffZoneStatus().at(2).second.zone_y_;
153  zone_pose_2.pose.orientation.z = 1;
154  break;
155  }
156 
157  case 6: {
158  zone_pose_1.pose.position.x =
159  this->p_blackboard_->GetBuffZoneStatus().at(6).second.min_x_ - 1;
160  zone_pose_1.pose.position.y =
161  this->p_blackboard_->GetBuffZoneStatus().at(6).second.zone_y_;
162  zone_pose_1.pose.orientation.w = 1;
163 
164  zone_pose_2.pose.position.x =
165  this->p_blackboard_->GetBuffZoneStatus().at(6).second.max_x_ + 1;
166  zone_pose_2.pose.position.y =
167  this->p_blackboard_->GetBuffZoneStatus().at(6).second.zone_y_;
168  zone_pose_2.pose.orientation.z = 1;
169  break;
170  }
171 
172  case 3: {
173  zone_pose_1.pose.position.x =
174  this->p_blackboard_->GetBuffZoneStatus().at(3).second.min_x_ - 1;
175  zone_pose_1.pose.position.y =
176  this->p_blackboard_->GetBuffZoneStatus().at(3).second.zone_y_;
177  zone_pose_1.pose.orientation.w = 1;
178 
179  zone_pose_2.pose.position.x =
180  this->p_blackboard_->GetBuffZoneStatus().at(3).second.max_x_ + 1;
181  zone_pose_2.pose.position.y =
182  this->p_blackboard_->GetBuffZoneStatus().at(3).second.zone_y_;
183  zone_pose_2.pose.orientation.z = 1;
184  break;
185  }
186 
187  case 5: {
188  zone_pose_1.pose.position.x =
189  this->p_blackboard_->GetBuffZoneStatus().at(5).second.min_x_ - 1;
190  zone_pose_1.pose.position.y =
191  this->p_blackboard_->GetBuffZoneStatus().at(5).second.zone_y_;
192  zone_pose_1.pose.orientation.w = 1;
193 
194  zone_pose_2.pose.position.x =
195  this->p_blackboard_->GetBuffZoneStatus().at(5).second.max_x_ + 1;
196  zone_pose_2.pose.position.y =
197  this->p_blackboard_->GetBuffZoneStatus().at(5).second.zone_y_;
198  zone_pose_2.pose.orientation.z = 1;
199  break;
200  }
201 
202  case 4: {
203  zone_pose_1.pose.position.x =
204  this->p_blackboard_->GetBuffZoneStatus().at(4).second.min_x_ - 1;
205  zone_pose_1.pose.position.y =
206  this->p_blackboard_->GetBuffZoneStatus().at(4).second.zone_y_;
207  zone_pose_1.pose.orientation.w = 1;
208 
209  zone_pose_2.pose.position.x =
210  this->p_blackboard_->GetBuffZoneStatus().at(4).second.zone_x_;
211  zone_pose_2.pose.position.y =
212  this->p_blackboard_->GetBuffZoneStatus().at(4).second.max_y_ + 1;
213  zone_pose_2.pose.orientation.z = -0.707;
214  zone_pose_2.pose.orientation.w = 0.707;
215  break;
216  }
217 
218  default: {
219  ROS_ERROR("Error Zone id !");
220  break;
221  }
222  }
223 
224  //处理位置
225 
226  return std::pair<geometry_msgs::PoseStamped, geometry_msgs::PoseStamped>(
227  zone_pose_1, zone_pose_2);
228  }
229 };
230 } // namespace roborts_decision
231 
232 #endif // ICRA_FIREFLY_ROBORTS_WAIT_FOR_ENEMY_SUPPLY_AREA_H_
roborts_decision::RED_RESTORATION
@ RED_RESTORATION
Definition: blackboard_common.h:62
roborts_decision::SUPPLY
@ SUPPLY
Definition: blackboard_common.h:51
roborts_decision::WaitForEnemySupplyArea::p_my_robot1
std::shared_ptr< MyRobot > p_my_robot1
Definition: wait_for_enemy_supply_area.h:110
roborts_decision::BLUE_RESTORATION
@ BLUE_RESTORATION
Definition: blackboard_common.h:64
roborts_decision::ESCAPE
@ ESCAPE
Definition: blackboard_common.h:49
roborts_decision::WaitForEnemySupplyArea
Definition: wait_for_enemy_supply_area.h:14
roborts_decision::WaitForEnemySupplyArea::p_robot1_behaviors
std::shared_ptr< RobotBehaviors > p_robot1_behaviors
Definition: wait_for_enemy_supply_area.h:114
roborts_decision::BuffStatus
BuffStatus
Definition: blackboard_common.h:61
roborts_decision::WaitForEnemySupplyArea::p_blackboard_
std::shared_ptr< Blackboard > p_blackboard_
Definition: wait_for_enemy_supply_area.h:112
roborts_decision::WaitForEnemySupplyArea::WaitForEnemySupplyArea
WaitForEnemySupplyArea(std::shared_ptr< MyRobot > p_my_robot1, std::shared_ptr< MyRobot > p_my_robot2, std::shared_ptr< Blackboard > p_blackboard, std::shared_ptr< RobotBehaviors > p_robot1_behaviors, std::shared_ptr< RobotBehaviors > p_robot2_behaviors)
Definition: wait_for_enemy_supply_area.h:16
roborts_decision::RESTORATION
@ RESTORATION
Definition: blackboard_common.h:78
roborts_decision::EnemyBuffZone
EnemyBuffZone
Definition: blackboard_common.h:76
roborts_decision::WaitForEnemySupplyArea::CancelBehavior
static void CancelBehavior(const std::shared_ptr< MyRobot > &myRobot, const std::shared_ptr< RobotBehaviors > &robotBehaviors)
Definition: wait_for_enemy_supply_area.h:97
roborts_decision::WaitForEnemySupplyArea::getGoalPose
std::pair< geometry_msgs::PoseStamped, geometry_msgs::PoseStamped > getGoalPose(int zone_id)
Definition: wait_for_enemy_supply_area.h:121
roborts_decision::RED_PROJECTILE_SUPPLIER
@ RED_PROJECTILE_SUPPLIER
Definition: blackboard_common.h:63
roborts_decision::WaitForEnemySupplyArea::p_robot2_behaviors
std::shared_ptr< RobotBehaviors > p_robot2_behaviors
Definition: wait_for_enemy_supply_area.h:115
roborts_decision::WaitForEnemySupplyArea::buff_status_projection_supplier
BuffStatus buff_status_projection_supplier
Definition: wait_for_enemy_supply_area.h:118
roborts_decision
Definition: behavior_test_node.h:14
roborts_decision::GOAL
@ GOAL
Definition: blackboard_common.h:50
roborts_decision::WaitForEnemySupplyArea::p_my_robot2
std::shared_ptr< MyRobot > p_my_robot2
Definition: wait_for_enemy_supply_area.h:111
roborts_decision::BLUE_PROJECTILE_SUPPLIER
@ BLUE_PROJECTILE_SUPPLIER
Definition: blackboard_common.h:65
roborts_decision::WaitForEnemySupplyArea::buff_status_restoration
BuffStatus buff_status_restoration
Definition: wait_for_enemy_supply_area.h:117
roborts_decision::PROJECTILE_SUPPLIER
@ PROJECTILE_SUPPLIER
Definition: blackboard_common.h:77
roborts_decision::WaitForEnemySupplyArea::Run
bool Run(EnemyBuffZone enemy_zone)
Definition: wait_for_enemy_supply_area.h:35
roborts_decision::WaitForEnemySupplyArea::Cancel
void Cancel()
Definition: wait_for_enemy_supply_area.h:91