Decision Module
1.0
ICRA2020 AI Challenge Northwestern Polytechnical University Aoxiang Team Strategy Code
|
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"
9 #include "roborts_msgs/GimbalAngle.h"
10 #include "roborts_msgs/GimbalRate.h"
11 #include "roborts_msgs/ShootCmd.h"
12 #include "roborts_msgs/FricWhl.h"
14 #include "../behavior_tree/behavior_state.h"
21 typedef actionlib::SimpleActionClient<roborts_msgs::PIDControllerTowardAngularAction>
PIDControllerClient;
43 explicit GimbalExecutor(
const ros::NodeHandle &nh = ros::NodeHandle(
"~"));
51 void Execute(geometry_msgs::Point32 &target, roborts_msgs::GimbalAngle &executor_gimbal_angle_);
56 void Execute(
const roborts_msgs::GimbalAngle &gimbal_angle);
61 void Execute(
const roborts_msgs::GimbalRate &gimbal_rate);
108 #endif //ROBORTS_DECISION_GIMBAL_EXECUTOR_H
@ GOAL_MODE_USE_GIMBAL_ANGLE
ExcutionMode
Gimbal execution mode for different tasks.
Definition: gimbal_executor.h:29
~GimbalExecutor()=default
@ GOAL_MODE_USE_GIMBAL_RATE
roborts_msgs::PIDControllerTowardAngularGoal pid_controller_toward_angular_goal_
Definition: gimbal_executor.h:102
BehaviorState Update()
Definition: gimbal_executor.cpp:124
ros::NodeHandle nh_
Definition: gimbal_executor.h:81
actionlib::SimpleActionClient< roborts_msgs::PIDControllerTowardAngularAction > PIDControllerClient
Definition: gimbal_executor.h:23
GimbalExecutor(const ros::NodeHandle &nh=ros::NodeHandle("~"))
Constructor of GimbalExecutor.
Definition: gimbal_executor.cpp:3
ExcutionMode excution_mode_
execution mode of the executor
Definition: gimbal_executor.h:84
ros::ServiceClient fric_wheel_client_
Definition: gimbal_executor.h:98
void PIDControllerFeedbackCallback(const roborts_msgs::PIDControllerTowardAngularFeedbackConstPtr &pid_controller_toward_angular_feedback)
Definition: gimbal_executor.cpp:185
void Execute_ShootCmd(const uint16_t &mode_cmd, const uint16_t &num)
Update the current gimbal executor state.
Definition: gimbal_executor.cpp:90
GoalMode
Definition: gimbal_executor.h:36
Definition: gimbal_executor.h:19
BehaviorState execution_state_
execution state of the executor (same with behavior state)
Definition: gimbal_executor.h:86
@ RATE_MODE
Rate task mode.
roborts_msgs::GimbalRate zero_gimbal_rate_
zero gimbal rate in form of ROS roborts_msgs::GimbalRate
Definition: gimbal_executor.h:91
ros::ServiceClient shoot_client_
Definition: gimbal_executor.h:96
Definition: behavior_test_node.h:14
void Execute(const geometry_msgs::PoseStamped &gimbal_angle, GoalMode goal_mode)
Definition: gimbal_executor.cpp:50
@ IDLE_MODE
Default idle mode with no task.
@ ANGLE_MODE
Angle task mode.
ros::Publisher cmd_gimbal_angle_pub_
gimbal angle control publisher in ROS
Definition: gimbal_executor.h:94
void Execute_FricWhl(const bool key, const double speed)
Definition: gimbal_executor.cpp:106
BehaviorState
Behavior state.
Definition: behavior_state.h:11
actionlib::SimpleActionClient< roborts_msgs::PIDControllerTowardAngularAction > pid_controller_client_
pid controller actionlib client
Definition: gimbal_executor.h:101
void Cancel()
Cancel the current task and deal with the mode transition.
Definition: gimbal_executor.cpp:163
ros::Publisher cmd_gimbal_rate_pub_
gimbal rate control publisher in ROS
Definition: gimbal_executor.h:89