Decision Module
1.0
ICRA2020 AI Challenge Northwestern Polytechnical University Aoxiang Team Strategy Code
|
#include <blackboard.h>
Public Types | |
typedef std::shared_ptr< Blackboard > | Ptr |
Public Member Functions | |
Blackboard (std::shared_ptr< MyRobot > &p_myrobot1, std::shared_ptr< MyRobot > &p_myrobot2, const ros::NodeHandle &nh=ros::NodeHandle("~")) | |
Construct Blackboard using two MyRobot. More... | |
virtual | ~Blackboard () |
Blackboard (const Blackboard &)=delete | |
Blackboard (Blackboard &&)=delete | |
const std::shared_ptr< MyRobot > & | GetMyRobot1 () |
Get number 1 of MyRobot. More... | |
const std::shared_ptr< MyRobot > & | GetMyRobot2 () |
Get number 2 of MyRobot. More... | |
EnemyRobot & | GetEnemyRobot1 () |
Get number 1 of EnemyRobot. More... | |
EnemyRobot & | GetEnemyRobot2 () |
Get number 2 of EnemyRobot. More... | |
const EnemyRobot & | GetAnotherRobot (const EnemyRobot &enemy_robot) |
Given an EnemyRobot, returns its companion. More... | |
const std::shared_ptr< MyRobot > & | GetAnotherRobot (const std::shared_ptr< MyRobot > &p_my_robot) const |
Given an MyRobot, returns its companion. More... | |
void | Update () |
Update all the information. Need to be called on every frame. More... | |
MyColor | GetMyColor () const |
Get the color of MyRobot. More... | |
RobotBehavior | GetCommonMoveType (const std::shared_ptr< MyRobot > &my_robot) |
Whether the non-defensive run (spinning run) is obstacle avoidance run or run at a certain angle. More... | |
std::vector< EnemyRobot > | GetDangerousOpp (const std::shared_ptr< MyRobot > &p_my_robot) |
Get EnemyRobot s that can attack us directly. More... | |
roborts_common::Point2D | GetAttackRunningPoint (const std::shared_ptr< MyRobot > &p_my_robot, const roborts_common::Point2D &another_partner_point) |
Get the point to run when attacking. More... | |
roborts_common::Point2D | GetSpecificRunPoint (const std::vector< PointValue > &running_interval, std::shared_ptr< MyRobot > p_my_robot, const roborts_common::Point2D &another_point) |
Get the point to run to, used by GetAttackRunningPoint. More... | |
const EnemyRobot & | GetEnemyRobotToAttack (const std::shared_ptr< MyRobot > &p_my_robot) |
Use analytic hierarchy process to obtain robots that need to attack. More... | |
const EnemyRobot & | GetCloserEnemyRobot (const std::shared_ptr< MyRobot > &p_my_robot) |
Get the EnemyRobot that closest to our robots. More... | |
double | GetKeepDirectionSafeDistance () const |
Get max safe distance. More... | |
double | GetKeepDirectionEffectWithDistance (double distance, double angle=45) const |
Obtain the damage percentage of a certain distance from the enemy and a certain deflection angle relative to the enemy (the angle formed by the line between the normal direction and the center of the robot) More... | |
double | GetShootSpeed (const std::shared_ptr< MyRobot > &p_my_robot, const roborts_common::Point2D &enemy_pose) |
const EnemyRobot & | GetPrepareShootOpp (const std::shared_ptr< MyRobot > &my_robot) |
roborts_common::Point2D | GetDefenseRunningPoint (const std::shared_ptr< MyRobot > &p_my_robot, roborts_common::Point2D another_point=roborts_common::Point2D(0, 0)) |
void | UpdateDefenseAllPotentialField (std::vector< std::vector< PointValue >> &security_matrix) |
void | UpdateAreaOfPotentialField (std::vector< std::vector< PointValue >> &security_matrix, const roborts_common::Polygon2D &area, double value) const |
double | GetDefensePotentialFieldSafeValue () const |
bool | CanCooperatePartner (const roborts_common::Point2D &tar_point, const roborts_common::Point2D &partner_tar_point, const std::shared_ptr< MyRobot > &me, const std::shared_ptr< MyRobot > &partner) const |
void | FindSafePoint (PointValue &safe_point, const std::shared_ptr< MyRobot > &p_my_robot, const std::vector< std::vector< PointValue >> &security_matrix, roborts_common::Point2D another_point) |
double | GetFortPotentialField (double distance, double direction) |
double | GetPlacePotentialField (double distance) |
double | GetWallPotentialField (double distance) |
void | UpdatePotentialField (std::vector< std::vector< PointValue >> &potential_field, const roborts_common::Polygon2D &object, const std::function< double(const roborts_common::Point2D &, const roborts_common::Polygon2D &)> &fun) |
std::vector< std::vector< PointValue > > | DiscretizeMap (double discrete_unit) const |
bool | IsCollide (std::shared_ptr< MyRobot > p_my_robot) const |
std::vector< std::vector< PointValue > > | UpdatePotentialFieldInAttack (const std::shared_ptr< MyRobot > &p_my_robot, const EnemyRobot &enemy_to_attack) |
double | CalculateEnemyToAttackPotentialField (const roborts_common::Point2D &pose_point, const roborts_common::Polygon2D &pose_polygon) |
double | CalculateEnemyAnotherPotentialField (const roborts_common::Point2D &pose_point, const roborts_common::Polygon2D &pose_polygon) |
std::vector< PointValue > | CalculateSuitRunPoint (const std::vector< std::vector< PointValue >> &potential_field) |
double | GetDistanceWithObstacles (const roborts_common::Point2D &pose, const roborts_common::Polygon2D &polygon_2_d) |
double | GetPointToEntityDistance (const roborts_common::Point2D &point_2_d, const roborts_common::Polygon2D &polygon_2_d) const |
std::vector< roborts_common::Polygon2D > | GetHomePolygon () |
double | PredictedDistance (const roborts_common::Point2D &_point, const std::shared_ptr< MyRobot > &p_my_robot) |
roborts_common::Polygon2D | GetHomePolygon (const std::shared_ptr< MyRobot > &p_my_robot) const |
std::vector< roborts_common::Polygon2D > | GetEnemyPolygon () const |
roborts_common::Polygon2D | GetEnemyPolygon (const EnemyRobot &enemy_robot) const |
void | WriteFile (std::string file_name, std::vector< std::vector< PointValue >> &map, roborts_common::Point2D &most_suitable_point) |
Static Public Member Functions | |
static std::shared_ptr< Blackboard > | GetBlackboard () |
Create or get the single instance of Blackboard. More... | |
Private Member Functions | |
void | OutpostDetectedCallback (const roborts_msgs::OutpostDetected::ConstPtr &msg) |
void | MyColorCallback (const roborts_msgs::RobotStatus::ConstPtr &msg) |
void | GameSurvivorCallback (const roborts_msgs::GameSurvivor::ConstPtr &msg) |
void | NextDecisionCallBackDeprecated (const std_msgs::Int32::ConstPtr &msg) |
std::string | LoadValue (const std::string &value_name, const std::string &target_name) |
std::string | FindValueInFile (const std::string &value_name, const std::string &file_name) |
bool | UpdataValue (const std::string &value_name, const std::string &file_name, const std::string &value) |
bool | AddValueInFile (const std::string &value_name, const std::string &value, const std::string &file_path) |
bool | ChangeValueInFile (const std::string &value_name, const std::string &value, const std::string &file_path) |
Private Attributes | |
ros::NodeHandle | nh_ |
ros::Subscriber | outpose_camera_sub_ |
ros::Subscriber | color_sub_ |
ros::Subscriber | game_survivor_sub_ |
ros::Publisher | is_collide_pub_ |
MyColor | my_color_ |
EnemyRobot | enemy_robot_1_ |
EnemyRobot | enemy_robot_2_ |
std::shared_ptr< Field > | field_ |
std::shared_ptr< MyRobot > | p_my_robot1_ |
std::shared_ptr< MyRobot > | p_my_robot2_ |
PredictionSystem | prediction_system_ |
double | defense_safe_value_ = 100.0 |
Static Private Attributes | |
static std::shared_ptr< Blackboard > | p_blackboard_ = nullptr |
Contains all information needed by the strategy The class implements single instance pattern, the instance is obtained by Blackboard::GetBlackboard().
typedef std::shared_ptr<Blackboard> roborts_decision::Blackboard::Ptr |
|
explicit |
Construct Blackboard using two MyRobot.
|
virtualdefault |
|
delete |
|
delete |
|
private |
在目标文件添加键值对(不检查是否字段存在)
value_name | |
value | |
file_path |
double Blackboard::CalculateEnemyAnotherPotentialField | ( | const roborts_common::Point2D & | pose_point, |
const roborts_common::Polygon2D & | pose_polygon | ||
) |
计算敌方另一个机器人(相对于被进攻的机器人)机器人的势场,根据距离确定, 直接取倒数即可
pose_point,位置点 | |
pose_polygon | ,机器人矩形 |
double Blackboard::CalculateEnemyToAttackPotentialField | ( | const roborts_common::Point2D & | pose_point, |
const roborts_common::Polygon2D & | pose_polygon | ||
) |
计算敌方被进攻的机器人的势场,根据距离确定, 并结合障碍物位置
pose_point,位置点 | |
pose_polygon | ,机器人矩形 |
std::vector< PointValue > Blackboard::CalculateSuitRunPoint | ( | const std::vector< std::vector< PointValue >> & | potential_field | ) |
bool Blackboard::CanCooperatePartner | ( | const roborts_common::Point2D & | tar_point, |
const roborts_common::Point2D & | partner_tar_point, | ||
const std::shared_ptr< MyRobot > & | me, | ||
const std::shared_ptr< MyRobot > & | partner | ||
) | const |
判断是否可以配合同伴(原则法) 不损害同伴:1. 跑点与当前位置的连线不相交;2. 跑点不重合(距离小于两个车长); 尽力帮助同伴:1. 响应同伴的信号(有余力才响应);
tar_point | |
partner_tar_point | |
partner |
|
private |
更新目标文件目标字段的值(若有重复,全部更新;若无,也不检查)
value_name | |
value | |
file_path |
std::vector< std::vector< PointValue > > Blackboard::DiscretizeMap | ( | double | discrete_unit | ) | const |
void Blackboard::FindSafePoint | ( | PointValue & | safe_point, |
const std::shared_ptr< MyRobot > & | p_my_robot, | ||
const std::vector< std::vector< PointValue >> & | security_matrix, | ||
roborts_common::Point2D | another_point | ||
) |
|
private |
在指定文件中查找目标字段
value_name | |
file_name |
|
private |
const EnemyRobot & Blackboard::GetAnotherRobot | ( | const EnemyRobot & | enemy_robot | ) |
Given an EnemyRobot, returns its companion.
const std::shared_ptr< MyRobot > & Blackboard::GetAnotherRobot | ( | const std::shared_ptr< MyRobot > & | p_my_robot | ) | const |
Given an MyRobot, returns its companion.
roborts_common::Point2D Blackboard::GetAttackRunningPoint | ( | const std::shared_ptr< MyRobot > & | p_my_robot, |
const roborts_common::Point2D & | another_partner_point | ||
) |
Get the point to run when attacking.
p_my_robot | The robot attacking |
another_partner_point | The partner of the attacking robot |
计算我方机器人进攻跑位点
0. 获取进攻的对象
|
static |
Create or get the single instance of Blackboard.
const EnemyRobot & Blackboard::GetCloserEnemyRobot | ( | const std::shared_ptr< MyRobot > & | p_my_robot | ) |
Get the EnemyRobot that closest to our robots.
RobotBehavior Blackboard::GetCommonMoveType | ( | const std::shared_ptr< MyRobot > & | my_robot | ) |
Whether the non-defensive run (spinning run) is obstacle avoidance run or run at a certain angle.
my_robot | std::shared_ptr to MyRobot |
std::vector< EnemyRobot > Blackboard::GetDangerousOpp | ( | const std::shared_ptr< MyRobot > & | p_my_robot | ) |
Get EnemyRobot
s that can attack us directly.
std::vector
of dangerous EnemyRobot double Blackboard::GetDefensePotentialFieldSafeValue | ( | ) | const |
roborts_common::Point2D Blackboard::GetDefenseRunningPoint | ( | const std::shared_ptr< MyRobot > & | p_my_robot, |
roborts_common::Point2D | another_point = roborts_common::Point2D(0, 0) |
||
) |
不考虑同伴 根据两个敌方位置、自己位置、障碍物位置进行躲藏保障安全 统一:成倍扩张寻找范围,直到寻找到一个满足安全系数的位置 寻找算法:将地图离散(80mm单位离散),通过势场算法将获得各离散点的安全度,找到一个范围内满足安全性要求的最安全的位置。无满足的,进行范围扩散 障碍物问题:给障碍物添加势场影响,加范围,防止离墙过近
考虑同伴(原则法) 不损害同伴、完成自己的目的,尽力帮助同伴 不损害同伴:1. 跑点与当前位置的连线不相交;2. 跑点不重合(距离小于两个车长); 完成自己的目的:1. 进攻、防御的目的不会因为同伴而改变(若重合,选其他点); 尽力帮助同伴:1. 响应同伴的信号(有余力才响应);
p_my_robot | |
another_point |
double Blackboard::GetDistanceWithObstacles | ( | const roborts_common::Point2D & | pose, |
const roborts_common::Polygon2D & | polygon_2_d | ||
) |
std::vector< roborts_common::Polygon2D > Blackboard::GetEnemyPolygon | ( | ) | const |
roborts_common::Polygon2D Blackboard::GetEnemyPolygon | ( | const EnemyRobot & | enemy_robot | ) | const |
EnemyRobot & Blackboard::GetEnemyRobot1 | ( | ) |
Get number 1 of EnemyRobot.
EnemyRobot & Blackboard::GetEnemyRobot2 | ( | ) |
Get number 2 of EnemyRobot.
const EnemyRobot & Blackboard::GetEnemyRobotToAttack | ( | const std::shared_ptr< MyRobot > & | p_my_robot | ) |
Use analytic hierarchy process to obtain robots that need to attack.
double Blackboard::GetFortPotentialField | ( | double | distance, |
double | direction | ||
) |
std::vector< roborts_common::Polygon2D > Blackboard::GetHomePolygon | ( | ) |
roborts_common::Polygon2D Blackboard::GetHomePolygon | ( | const std::shared_ptr< MyRobot > & | p_my_robot | ) | const |
double Blackboard::GetKeepDirectionEffectWithDistance | ( | double | distance, |
double | angle = 45 |
||
) | const |
Obtain the damage percentage of a certain distance from the enemy and a certain deflection angle relative to the enemy (the angle formed by the line between the normal direction and the center of the robot)
double Blackboard::GetKeepDirectionSafeDistance | ( | ) | const |
Get max safe distance.
double Blackboard::GetPlacePotentialField | ( | double | distance | ) |
double Blackboard::GetPointToEntityDistance | ( | const roborts_common::Point2D & | point_2_d, |
const roborts_common::Polygon2D & | polygon_2_d | ||
) | const |
const EnemyRobot & Blackboard::GetPrepareShootOpp | ( | const std::shared_ptr< MyRobot > & | my_robot | ) |
double Blackboard::GetShootSpeed | ( | const std::shared_ptr< MyRobot > & | p_my_robot, |
const roborts_common::Point2D & | enemy_pose | ||
) |
roborts_common::Point2D Blackboard::GetSpecificRunPoint | ( | const std::vector< PointValue > & | running_interval, |
std::shared_ptr< MyRobot > | p_my_robot, | ||
const roborts_common::Point2D & | another_point | ||
) |
Get the point to run to, used by GetAttackRunningPoint.
running_interval | 所有符合条件的跑位点 |
p_my_robot | 我方执行此函数的机器人机器人 |
another_point | 我方另一个机器人的跑位点 |
获取最终的跑位点,结合到我方机器人的距离,与障碍物的距离和另一个机器人的距离
double Blackboard::GetWallPotentialField | ( | double | distance | ) |
bool Blackboard::IsCollide | ( | std::shared_ptr< MyRobot > | p_my_robot | ) | const |
|
private |
加载目标文件的某个值,若找不到,返回空字符串 若目标是目录:在默认文件查找 若目标是文件:在该文件查找
value_name | |
target_name |
|
private |
|
private |
|
private |
double Blackboard::PredictedDistance | ( | const roborts_common::Point2D & | _point, |
const std::shared_ptr< MyRobot > & | p_my_robot | ||
) |
|
private |
查找目标路径的所有文件,将数据更新到对应文件中,在单个文件中不允许重名
value_name | |
file_name |
void Blackboard::Update | ( | ) |
Update all the information. Need to be called on every frame.
void Blackboard::UpdateAreaOfPotentialField | ( | std::vector< std::vector< PointValue >> & | security_matrix, |
const roborts_common::Polygon2D & | area, | ||
double | value | ||
) | const |
void Blackboard::UpdateDefenseAllPotentialField | ( | std::vector< std::vector< PointValue >> & | security_matrix | ) |
void Blackboard::UpdatePotentialField | ( | std::vector< std::vector< PointValue >> & | potential_field, |
const roborts_common::Polygon2D & | object, | ||
const std::function< double(const roborts_common::Point2D &, const roborts_common::Polygon2D &)> & | fun | ||
) |
std::vector< std::vector< PointValue > > Blackboard::UpdatePotentialFieldInAttack | ( | const std::shared_ptr< MyRobot > & | p_my_robot, |
const EnemyRobot & | enemy_to_attack | ||
) |
更新势场
void Blackboard::WriteFile | ( | std::string | file_name, |
std::vector< std::vector< PointValue >> & | map, | ||
roborts_common::Point2D & | most_suitable_point | ||
) |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
staticprivate |
|
private |
|
private |
|
private |