AutoCar  v1.0.0
logical_core.h
1 #ifndef LOGICALCORE_H
2 #define LOGICALCORE_H
3 #include <move_base_msgs/MoveBaseAction.h>
4 #include <geometry_msgs/PoseWithCovarianceStamped.h>
5 #include <actionlib/client/simple_action_client.h>
6 #include <vector>
7 #include <opencv2/opencv.hpp>
8 #include <string>
9 #include <algorithm>
10 #include "mission_mode.h"
11 #include "point_mode.h"
12 #include "patrol_mode.h"
13 #include "shooting_mode.h"
14 #include <ros/ros.h>
15 #include <tf/tf.h>
16 #include <tf/transform_listener.h>
17 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
18 
19 enum CarState
20 {
21  OBTAINED_GOAL_STATE,
22  BUSY_STATE,
23  IDLE_STATE
24 };
25 
27 {
28 public:
29  friend class PointMode;
30  friend class PatrolMode;
31  friend class ShootingMode;
32  LogicalCore(tf::TransformListener &tf);
33  void pos_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pos);
34  void getCarPos(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pos);
39  void getCameraGoal(const move_base_msgs::MoveBaseGoal::ConstPtr &msg);
44  void fillPatrolList();
45  double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
46  geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg);
50  void stateMachine();
51 private:
52  //We should init the patrol_list first.
53  std::vector<move_base_msgs::MoveBaseGoal> patrol_list_;
54  MoveBaseClient ac_;
55  tf::TransformListener &tf_;
56  move_base_msgs::MoveBaseGoal goal_;
57  geometry_msgs::PoseStamped old_enemy_global_goal_;
58  geometry_msgs::PoseStamped enemy_goal_;
59  geometry_msgs::PoseStamped enemy_global_goal_;
60  geometry_msgs::PoseStamped car_position;
61  ros::Subscriber send_goal_srv_;
62  ros::Subscriber sub_current_pos;
63  CarState car_state_;
64  MissionMode *p_mission_mode_;
65  PointMode *p_point_mode_;
66  PatrolMode *p_patrol_mode_;
67  ShootingMode *p_shooting_mode_;
68 };
69 
70 #endif // LOGICALCORE_H
void stateMachine()
stateMachine the logical core of autocar.
Definition: point_mode.h:6
Definition: logical_core.h:26
Definition: patrol_mode.h:7
There are three missions of the autocar: point state, patrol state, shooting state. and the autocar also have three state: OBTAINED_GOAL_STATE, BUSY_STATE, IDLE_STATE.
Definition: mission_mode.h:9
void fillPatrolList()
You can set some points in file ./param_manager/autocar_params.xml first, this function will load the...
void getCameraGoal(const move_base_msgs::MoveBaseGoal::ConstPtr &msg)
The callback function of the camera_goal publishing by armor_detect node.
Definition: shooting_mode.h:6