| 13 | |
| 14 | namespace roborts_decision{ |
| 15 | class EscapeBehavior { |
| 16 | public: |
| 17 | EscapeBehavior(ChassisExecutor* &chassis_executor, |
| 18 | Blackboard* &blackboard, |
| 19 | const std::string & proto_file_path) : chassis_executor_(chassis_executor), |
| 20 | blackboard_(blackboard) { |
| 21 | |
| 22 | // init whirl velocity |
| 23 | whirl_vel_.accel.linear.x = 0; |
| 24 | whirl_vel_.accel.linear.x = 0; |
| 25 | whirl_vel_.accel.linear.y = 0; |
| 26 | whirl_vel_.accel.linear.z = 0; |
| 27 | |
| 28 | whirl_vel_.accel.angular.x = 0; |
| 29 | whirl_vel_.accel.angular.y = 0; |
| 30 | whirl_vel_.accel.angular.z = 0; |
| 31 | |
| 32 | if (!LoadParam(proto_file_path)) { |
| 33 | ROS_ERROR("%s can't open file", __FUNCTION__); |
| 34 | } |
| 35 | |
| 36 | } |
| 37 | |
| 38 | void Run() { |
| 39 | |
| 40 | auto executor_state = Update(); |
| 41 | |
| 42 | if (executor_state != BehaviorState::RUNNING) { |
| 43 | |
| 44 | if (blackboard_->IsEnemyDetected()) { |
| 45 | |
| 46 | geometry_msgs::PoseStamped enemy; |
| 47 | enemy = blackboard_->GetEnemy(); |
| 48 | float goal_yaw, goal_x, goal_y; |
| 49 | unsigned int goal_cell_x, goal_cell_y; |
| 50 | unsigned int enemy_cell_x, enemy_cell_y; |
| 51 | |
| 52 | std::random_device rd; |
| 53 | std::mt19937 gen(rd()); |
| 54 | |
| 55 | auto robot_map_pose = blackboard_->GetRobotMapPose(); |
| 56 | float x_min, x_max; |
| 57 | if (enemy.pose.position.x < left_x_limit_) { |
| 58 | x_min = right_random_min_x_; |
| 59 | x_max = right_random_max_x_; |
| 60 | |
| 61 | } else if (enemy.pose.position.x > right_x_limit_) { |
| 62 | x_min = left_random_min_x_; |
| 63 | x_max = left_random_max_x_; |
| 64 | |
| 65 | } else { |
| 66 | if ((robot_x_limit_ - robot_map_pose.pose.position.x) >= 0) { |
| 67 | x_min = left_random_min_x_; |
| 68 | x_max = left_random_max_x_; |
| 69 | |
| 70 | } else { |
| 71 | x_min = right_random_min_x_; |
| 72 | x_max = right_random_max_x_; |
nothing calls this directly
no outgoing calls
no test coverage detected