Decision Module  1.0
ICRA2020 AI Challenge Northwestern Polytechnical University Aoxiang Team Strategy Code
gimbal_executor.h
Go to the documentation of this file.
1 #ifndef ROBORTS_DECISION_GIMBAL_EXECUTOR_H
2 #define ROBORTS_DECISION_GIMBAL_EXECUTOR_H
3 #include <actionlib/client/simple_action_client.h>
4 #include <geometry_msgs/Point32.h>
5 #include "roborts_msgs/PIDControllerTowardAngularAction.h"
6 #include "gimbal_control.h"
7 #include "ros/ros.h"
8 
9 #include "roborts_msgs/GimbalAngle.h"
10 #include "roborts_msgs/GimbalRate.h"
11 #include "roborts_msgs/ShootCmd.h"
12 #include "roborts_msgs/FricWhl.h"
13 
14 #include "../behavior_tree/behavior_state.h"
15 namespace roborts_decision{
16 /***
17  * @brief Gimbal Executor to execute different abstracted task for gimbal module
18  */
20 
21  typedef actionlib::SimpleActionClient<roborts_msgs::PIDControllerTowardAngularAction> PIDControllerClient;
22 
23  public:
27  enum class ExcutionMode{
28  IDLE_MODE,
30  RATE_MODE,
31  PID_MODE
32  };
33 
34  enum class GoalMode {
38  };
39 
43  explicit GimbalExecutor(const ros::NodeHandle &nh = ros::NodeHandle("~"));
44  ~GimbalExecutor() = default;
45 
46  void Execute(const geometry_msgs::PoseStamped &gimbal_angle, GoalMode goal_mode);
47  /***
48  * @brief Execute the gimbal angle task with publisher
49  * @param gimbal_angle Given gimbal angle
50  */
51  void Execute(geometry_msgs::Point32 &target, roborts_msgs::GimbalAngle &executor_gimbal_angle_);
52  /***
53  * @brief transform the target point to gimbal angle with publisher
54  * @param gimbal_angle Given gimbal angle
55  */
56  void Execute(const roborts_msgs::GimbalAngle &gimbal_angle);
57  /***
58  * @brief Execute the gimbal rate task with publisher
59  * @param gimbal_rate Given gimbal rate
60  */
61  void Execute(const roborts_msgs::GimbalRate &gimbal_rate);
67  void Execute_ShootCmd(const uint16_t &mode_cmd, const uint16_t &num);
68  void Execute_FricWhl(const bool key, const double speed);
69 
74  void Cancel();
75  void PublishMsgs();
76 
77  private:
78 
79  ros::NodeHandle nh_;
80 
85 
87  ros::Publisher cmd_gimbal_rate_pub_;
89  roborts_msgs::GimbalRate zero_gimbal_rate_;
90 
92  ros::Publisher cmd_gimbal_angle_pub_;
93 
94  ros::ServiceClient shoot_client_;
95 
96  ros::ServiceClient fric_wheel_client_;
97 
99  actionlib::SimpleActionClient<roborts_msgs::PIDControllerTowardAngularAction> pid_controller_client_;
100  roborts_msgs::PIDControllerTowardAngularGoal pid_controller_toward_angular_goal_;
101 
102 
103  void PIDControllerFeedbackCallback(const roborts_msgs::PIDControllerTowardAngularFeedbackConstPtr &pid_controller_toward_angular_feedback);
104 };
105 }
106 
107 
108 #endif //ROBORTS_DECISION_GIMBAL_EXECUTOR_H
roborts_decision::GimbalExecutor::GoalMode::GOAL_MODE_USE_GIMBAL_ANGLE
@ GOAL_MODE_USE_GIMBAL_ANGLE
roborts_decision::GimbalExecutor::ExcutionMode::PID_MODE
@ PID_MODE
roborts_decision::GimbalExecutor::ExcutionMode
ExcutionMode
Gimbal execution mode for different tasks.
Definition: gimbal_executor.h:29
roborts_decision::GimbalExecutor::~GimbalExecutor
~GimbalExecutor()=default
roborts_decision::GimbalExecutor::GoalMode::GOAL_MODE_USE_GIMBAL_RATE
@ GOAL_MODE_USE_GIMBAL_RATE
roborts_decision::GimbalExecutor::pid_controller_toward_angular_goal_
roborts_msgs::PIDControllerTowardAngularGoal pid_controller_toward_angular_goal_
Definition: gimbal_executor.h:102
roborts_decision::GimbalExecutor::Update
BehaviorState Update()
Definition: gimbal_executor.cpp:124
roborts_decision::GimbalExecutor::nh_
ros::NodeHandle nh_
Definition: gimbal_executor.h:81
roborts_decision::GimbalExecutor::PIDControllerClient
actionlib::SimpleActionClient< roborts_msgs::PIDControllerTowardAngularAction > PIDControllerClient
Definition: gimbal_executor.h:23
roborts_decision::GimbalExecutor::GimbalExecutor
GimbalExecutor(const ros::NodeHandle &nh=ros::NodeHandle("~"))
Constructor of GimbalExecutor.
Definition: gimbal_executor.cpp:3
roborts_decision::GimbalExecutor::excution_mode_
ExcutionMode excution_mode_
execution mode of the executor
Definition: gimbal_executor.h:84
roborts_decision::GimbalExecutor::fric_wheel_client_
ros::ServiceClient fric_wheel_client_
Definition: gimbal_executor.h:98
roborts_decision::GimbalExecutor::GoalMode::GOAL_MODE_USE_PID
@ GOAL_MODE_USE_PID
roborts_decision::GimbalExecutor::PIDControllerFeedbackCallback
void PIDControllerFeedbackCallback(const roborts_msgs::PIDControllerTowardAngularFeedbackConstPtr &pid_controller_toward_angular_feedback)
Definition: gimbal_executor.cpp:185
roborts_decision::GimbalExecutor::Execute_ShootCmd
void Execute_ShootCmd(const uint16_t &mode_cmd, const uint16_t &num)
Update the current gimbal executor state.
Definition: gimbal_executor.cpp:90
roborts_decision::GimbalExecutor::GoalMode
GoalMode
Definition: gimbal_executor.h:36
roborts_decision::GimbalExecutor
Definition: gimbal_executor.h:19
roborts_decision::GimbalExecutor::execution_state_
BehaviorState execution_state_
execution state of the executor (same with behavior state)
Definition: gimbal_executor.h:86
roborts_decision::GimbalExecutor::PublishMsgs
void PublishMsgs()
roborts_decision::GimbalExecutor::ExcutionMode::RATE_MODE
@ RATE_MODE
Rate task mode.
roborts_decision::GimbalExecutor::zero_gimbal_rate_
roborts_msgs::GimbalRate zero_gimbal_rate_
zero gimbal rate in form of ROS roborts_msgs::GimbalRate
Definition: gimbal_executor.h:91
roborts_decision::GimbalExecutor::shoot_client_
ros::ServiceClient shoot_client_
Definition: gimbal_executor.h:96
roborts_decision
Definition: behavior_test_node.h:14
roborts_decision::GimbalExecutor::Execute
void Execute(const geometry_msgs::PoseStamped &gimbal_angle, GoalMode goal_mode)
Definition: gimbal_executor.cpp:50
roborts_decision::GimbalExecutor::ExcutionMode::IDLE_MODE
@ IDLE_MODE
Default idle mode with no task.
roborts_decision::GimbalExecutor::ExcutionMode::ANGLE_MODE
@ ANGLE_MODE
Angle task mode.
roborts_decision::GimbalExecutor::cmd_gimbal_angle_pub_
ros::Publisher cmd_gimbal_angle_pub_
gimbal angle control publisher in ROS
Definition: gimbal_executor.h:94
roborts_decision::GimbalExecutor::Execute_FricWhl
void Execute_FricWhl(const bool key, const double speed)
Definition: gimbal_executor.cpp:106
roborts_decision::BehaviorState
BehaviorState
Behavior state.
Definition: behavior_state.h:11
roborts_decision::GimbalExecutor::pid_controller_client_
actionlib::SimpleActionClient< roborts_msgs::PIDControllerTowardAngularAction > pid_controller_client_
pid controller actionlib client
Definition: gimbal_executor.h:101
gimbal_control.h
roborts_decision::GimbalExecutor::Cancel
void Cancel()
Cancel the current task and deal with the mode transition.
Definition: gimbal_executor.cpp:163
roborts_decision::GimbalExecutor::cmd_gimbal_rate_pub_
ros::Publisher cmd_gimbal_rate_pub_
gimbal rate control publisher in ROS
Definition: gimbal_executor.h:89