Decision Module  1.0
ICRA2020 AI Challenge Northwestern Polytechnical University Aoxiang Team Strategy Code
roborts_decision::Blackboard Class Reference

#include <blackboard.h>

Public Types

typedef std::shared_ptr< BlackboardPtr
 

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...
 
EnemyRobotGetEnemyRobot1 ()
 Get number 1 of EnemyRobot. More...
 
EnemyRobotGetEnemyRobot2 ()
 Get number 2 of EnemyRobot. More...
 
const EnemyRobotGetAnotherRobot (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< EnemyRobotGetDangerousOpp (const std::shared_ptr< MyRobot > &p_my_robot)
 Get EnemyRobots 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 EnemyRobotGetEnemyRobotToAttack (const std::shared_ptr< MyRobot > &p_my_robot)
 Use analytic hierarchy process to obtain robots that need to attack. More...
 
const EnemyRobotGetCloserEnemyRobot (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 EnemyRobotGetPrepareShootOpp (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< PointValueCalculateSuitRunPoint (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< BlackboardGetBlackboard ()
 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< Fieldfield_
 
std::shared_ptr< MyRobotp_my_robot1_
 
std::shared_ptr< MyRobotp_my_robot2_
 
PredictionSystem prediction_system_
 
double defense_safe_value_ = 100.0
 

Static Private Attributes

static std::shared_ptr< Blackboardp_blackboard_ = nullptr
 

Detailed Description

Contains all information needed by the strategy The class implements single instance pattern, the instance is obtained by Blackboard::GetBlackboard().

Member Typedef Documentation

◆ Ptr

Constructor & Destructor Documentation

◆ Blackboard() [1/3]

Blackboard::Blackboard ( std::shared_ptr< MyRobot > &  p_myrobot1,
std::shared_ptr< MyRobot > &  p_myrobot2,
const ros::NodeHandle &  nh = ros::NodeHandle("~") 
)
explicit

Construct Blackboard using two MyRobot.

◆ ~Blackboard()

Blackboard::~Blackboard ( )
virtualdefault

◆ Blackboard() [2/3]

roborts_decision::Blackboard::Blackboard ( const Blackboard )
delete

◆ Blackboard() [3/3]

roborts_decision::Blackboard::Blackboard ( Blackboard &&  )
delete

Member Function Documentation

◆ AddValueInFile()

bool Blackboard::AddValueInFile ( const std::string &  value_name,
const std::string &  value,
const std::string &  file_path 
)
private

在目标文件添加键值对(不检查是否字段存在)

Parameters
value_name
value
file_path
Returns

◆ CalculateEnemyAnotherPotentialField()

double Blackboard::CalculateEnemyAnotherPotentialField ( const roborts_common::Point2D &  pose_point,
const roborts_common::Polygon2D &  pose_polygon 
)

计算敌方另一个机器人(相对于被进攻的机器人)机器人的势场,根据距离确定, 直接取倒数即可

Parameters
pose_point,位置点
pose_polygon,机器人矩形
Returns
返回获得的value

◆ CalculateEnemyToAttackPotentialField()

double Blackboard::CalculateEnemyToAttackPotentialField ( const roborts_common::Point2D &  pose_point,
const roborts_common::Polygon2D &  pose_polygon 
)

计算敌方被进攻的机器人的势场,根据距离确定, 并结合障碍物位置

Parameters
pose_point,位置点
pose_polygon,机器人矩形
Returns
返回获得的value

◆ CalculateSuitRunPoint()

std::vector< PointValue > Blackboard::CalculateSuitRunPoint ( const std::vector< std::vector< PointValue >> &  potential_field)

◆ CanCooperatePartner()

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. 响应同伴的信号(有余力才响应);

Parameters
tar_point
partner_tar_point
partner
Returns

◆ ChangeValueInFile()

bool Blackboard::ChangeValueInFile ( const std::string &  value_name,
const std::string &  value,
const std::string &  file_path 
)
private

更新目标文件目标字段的值(若有重复,全部更新;若无,也不检查)

Parameters
value_name
value
file_path
Returns

◆ DiscretizeMap()

std::vector< std::vector< PointValue > > Blackboard::DiscretizeMap ( double  discrete_unit) const

◆ FindSafePoint()

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 
)

◆ FindValueInFile()

std::string Blackboard::FindValueInFile ( const std::string &  value_name,
const std::string &  file_name 
)
private

在指定文件中查找目标字段

Parameters
value_name
file_name
Returns
返回目标字段的value(string),若没找到,返回空字符串

◆ GameSurvivorCallback()

void Blackboard::GameSurvivorCallback ( const roborts_msgs::GameSurvivor::ConstPtr &  msg)
private

◆ GetAnotherRobot() [1/2]

const EnemyRobot & Blackboard::GetAnotherRobot ( const EnemyRobot enemy_robot)

Given an EnemyRobot, returns its companion.

◆ GetAnotherRobot() [2/2]

const std::shared_ptr< MyRobot > & Blackboard::GetAnotherRobot ( const std::shared_ptr< MyRobot > &  p_my_robot) const

Given an MyRobot, returns its companion.

◆ GetAttackRunningPoint()

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.

Parameters
p_my_robotThe robot attacking
another_partner_pointThe partner of the attacking robot

计算我方机器人进攻跑位点

  1. 打敌方其中一个机器人,远离另一个 1.1 通过层次分析法计算 1.2 总共考虑了三要素,距离,对敌方机器人状态估计,敌方机器人与我方机器人之间的障碍物个数
  2. 尽可能大的获取敌方视野 2.1 通过我方机器人和敌方机器人的势场以及障碍物的势场进行判断,尝试采用RRT算法,获取跑位点 2.2 敌方机器人势场在半径4m以内为负,,4m以外为正,但是势场无法穿越障碍物, 2.2.1 我方势场为负,但是我方势场可以穿越障碍物,尽可能选择势场绝对值小的地方 2.2.2 考虑我方势场前提:我方机器人状态较好 2.3 首先确定跑位半径,然后获得相应的离散的跑位点 2.4 半径扩大原则:如果跑位点不满足,扩大跑位范围 2.5 就近原则:跑点就近,但是要考虑绕行障碍物所带来的距离损耗 2.6 易防守原则:尽量靠近障碍物
  3. 考虑我方机器人 3.1 不损害同伴,跑位点不能重合,并且适当远离同伴 3.2 完成自己的跑位点 3.3 在同伴发出请求的情况下,尽可能帮助同伴

0. 获取进攻的对象

  1. 获取势场
  2. 确定跑位点区间 直接选择value最小的1000个点
  3. 对跑位点进行评估,选取最优跑位点 3.1 获取距离最近的点 3.2 需要考虑对墙的距离,不能离墙太近 3.3 考虑另一个机器人的跑位点,在我方机器人状态良好的情况下,适当远离

◆ GetBlackboard()

std::shared_ptr< Blackboard > Blackboard::GetBlackboard ( )
static

Create or get the single instance of Blackboard.

◆ GetCloserEnemyRobot()

const EnemyRobot & Blackboard::GetCloserEnemyRobot ( const std::shared_ptr< MyRobot > &  p_my_robot)

Get the EnemyRobot that closest to our robots.

◆ GetCommonMoveType()

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.

Parameters
my_robotstd::shared_ptr to MyRobot

◆ GetDangerousOpp()

std::vector< EnemyRobot > Blackboard::GetDangerousOpp ( const std::shared_ptr< MyRobot > &  p_my_robot)

Get EnemyRobots that can attack us directly.

Returns
a std::vector of dangerous EnemyRobot

◆ GetDefensePotentialFieldSafeValue()

double Blackboard::GetDefensePotentialFieldSafeValue ( ) const

◆ GetDefenseRunningPoint()

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. 响应同伴的信号(有余力才响应);

Parameters
p_my_robot
another_point
Returns

◆ GetDistanceWithObstacles()

double Blackboard::GetDistanceWithObstacles ( const roborts_common::Point2D &  pose,
const roborts_common::Polygon2D &  polygon_2_d 
)

◆ GetEnemyPolygon() [1/2]

std::vector< roborts_common::Polygon2D > Blackboard::GetEnemyPolygon ( ) const

◆ GetEnemyPolygon() [2/2]

roborts_common::Polygon2D Blackboard::GetEnemyPolygon ( const EnemyRobot enemy_robot) const

◆ GetEnemyRobot1()

EnemyRobot & Blackboard::GetEnemyRobot1 ( )

Get number 1 of EnemyRobot.

◆ GetEnemyRobot2()

EnemyRobot & Blackboard::GetEnemyRobot2 ( )

Get number 2 of EnemyRobot.

◆ GetEnemyRobotToAttack()

const EnemyRobot & Blackboard::GetEnemyRobotToAttack ( const std::shared_ptr< MyRobot > &  p_my_robot)

Use analytic hierarchy process to obtain robots that need to attack.

◆ GetFortPotentialField()

double Blackboard::GetFortPotentialField ( double  distance,
double  direction 
)

◆ GetHomePolygon() [1/2]

std::vector< roborts_common::Polygon2D > Blackboard::GetHomePolygon ( )

◆ GetHomePolygon() [2/2]

roborts_common::Polygon2D Blackboard::GetHomePolygon ( const std::shared_ptr< MyRobot > &  p_my_robot) const

◆ GetKeepDirectionEffectWithDistance()

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)

◆ GetKeepDirectionSafeDistance()

double Blackboard::GetKeepDirectionSafeDistance ( ) const

Get max safe distance.

◆ GetMyColor()

MyColor Blackboard::GetMyColor ( ) const

Get the color of MyRobot.

◆ GetMyRobot1()

const std::shared_ptr< MyRobot > & Blackboard::GetMyRobot1 ( )

Get number 1 of MyRobot.

◆ GetMyRobot2()

const std::shared_ptr< MyRobot > & Blackboard::GetMyRobot2 ( )

Get number 2 of MyRobot.

◆ GetPlacePotentialField()

double Blackboard::GetPlacePotentialField ( double  distance)

◆ GetPointToEntityDistance()

double Blackboard::GetPointToEntityDistance ( const roborts_common::Point2D &  point_2_d,
const roborts_common::Polygon2D &  polygon_2_d 
) const

◆ GetPrepareShootOpp()

const EnemyRobot & Blackboard::GetPrepareShootOpp ( const std::shared_ptr< MyRobot > &  my_robot)

◆ GetShootSpeed()

double Blackboard::GetShootSpeed ( const std::shared_ptr< MyRobot > &  p_my_robot,
const roborts_common::Point2D &  enemy_pose 
)

◆ GetSpecificRunPoint()

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.

Parameters
running_interval所有符合条件的跑位点
p_my_robot我方执行此函数的机器人机器人
another_point我方另一个机器人的跑位点
Returns
返回具体跑位点

获取最终的跑位点,结合到我方机器人的距离,与障碍物的距离和另一个机器人的距离

◆ GetWallPotentialField()

double Blackboard::GetWallPotentialField ( double  distance)

◆ IsCollide()

bool Blackboard::IsCollide ( std::shared_ptr< MyRobot p_my_robot) const

◆ LoadValue()

std::string Blackboard::LoadValue ( const std::string &  value_name,
const std::string &  target_name 
)
private

加载目标文件的某个值,若找不到,返回空字符串 若目标是目录:在默认文件查找 若目标是文件:在该文件查找

Parameters
value_name
target_name
Returns
返回value的字符串

◆ MyColorCallback()

void Blackboard::MyColorCallback ( const roborts_msgs::RobotStatus::ConstPtr &  msg)
private

◆ NextDecisionCallBackDeprecated()

void Blackboard::NextDecisionCallBackDeprecated ( const std_msgs::Int32::ConstPtr &  msg)
private

◆ OutpostDetectedCallback()

void Blackboard::OutpostDetectedCallback ( const roborts_msgs::OutpostDetected::ConstPtr &  msg)
private

◆ PredictedDistance()

double Blackboard::PredictedDistance ( const roborts_common::Point2D &  _point,
const std::shared_ptr< MyRobot > &  p_my_robot 
)

◆ UpdataValue()

bool Blackboard::UpdataValue ( const std::string &  value_name,
const std::string &  file_name,
const std::string &  value 
)
private

查找目标路径的所有文件,将数据更新到对应文件中,在单个文件中不允许重名

Parameters
value_name
file_name
Returns
Author
lq

◆ Update()

void Blackboard::Update ( )

Update all the information. Need to be called on every frame.

◆ UpdateAreaOfPotentialField()

void Blackboard::UpdateAreaOfPotentialField ( std::vector< std::vector< PointValue >> &  security_matrix,
const roborts_common::Polygon2D &  area,
double  value 
) const

◆ UpdateDefenseAllPotentialField()

void Blackboard::UpdateDefenseAllPotentialField ( std::vector< std::vector< PointValue >> &  security_matrix)

◆ UpdatePotentialField()

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 
)

◆ UpdatePotentialFieldInAttack()

std::vector< std::vector< PointValue > > Blackboard::UpdatePotentialFieldInAttack ( const std::shared_ptr< MyRobot > &  p_my_robot,
const EnemyRobot enemy_to_attack 
)

更新势场

  1. 所有点全部更新,暂时利用赛场上的个点,势场均为正
  2. 对于敌方需要打击的机器人势场4m以内势场逐渐减小,4m以外逐渐增加,绝对值从1-100
  3. 对于敌方不需要打击的机器人算法同上
  4. 对于我方另一个机器人,算法同上。
  5. 有障碍物阻挡,距离无限远,可以考虑取倒数,即势场影响为0

◆ WriteFile()

void Blackboard::WriteFile ( std::string  file_name,
std::vector< std::vector< PointValue >> &  map,
roborts_common::Point2D &  most_suitable_point 
)

Member Data Documentation

◆ color_sub_

ros::Subscriber roborts_decision::Blackboard::color_sub_
private

◆ defense_safe_value_

double roborts_decision::Blackboard::defense_safe_value_ = 100.0
private

◆ enemy_robot_1_

EnemyRobot roborts_decision::Blackboard::enemy_robot_1_
private

◆ enemy_robot_2_

EnemyRobot roborts_decision::Blackboard::enemy_robot_2_
private

◆ field_

std::shared_ptr<Field> roborts_decision::Blackboard::field_
private

◆ game_survivor_sub_

ros::Subscriber roborts_decision::Blackboard::game_survivor_sub_
private

◆ is_collide_pub_

ros::Publisher roborts_decision::Blackboard::is_collide_pub_
private

◆ my_color_

MyColor roborts_decision::Blackboard::my_color_
private

◆ nh_

ros::NodeHandle roborts_decision::Blackboard::nh_
private

◆ outpose_camera_sub_

ros::Subscriber roborts_decision::Blackboard::outpose_camera_sub_
private

◆ p_blackboard_

std::shared_ptr< Blackboard > Blackboard::p_blackboard_ = nullptr
staticprivate

◆ p_my_robot1_

std::shared_ptr<MyRobot> roborts_decision::Blackboard::p_my_robot1_
private

◆ p_my_robot2_

std::shared_ptr<MyRobot> roborts_decision::Blackboard::p_my_robot2_
private

◆ prediction_system_

PredictionSystem roborts_decision::Blackboard::prediction_system_
private

The documentation for this class was generated from the following files: