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

#include <chassis_executor.h>

Public Types

enum  ExcutionMode {
  ExcutionMode::IDLE_MODE, ExcutionMode::GOAL_USE_PLANNER_MODE, ExcutionMode::GOAL_FROM_ODOM_MODE, ExcutionMode::SPEED_MODE,
  ExcutionMode::SPEED_WITH_ACCEL_MODE
}
 Chassis execution mode for different tasks. More...
 
enum  GoalMode { GoalMode::GOAL_MODE_USE_GOLBAL_LOCAL_PLANNER, GoalMode::GOAL_MODE_USE_ODOM_DATA }
 

Public Member Functions

 ChassisExecutor (const ros::NodeHandle &nh=ros::NodeHandle("~"))
 Constructor of ChassisExecutor. More...
 
 ~ChassisExecutor ()=default
 
void Execute (const geometry_msgs::PoseStamped &goal)
 Execute the goal-targeted task using global and local planner with actionlib. More...
 
void Execute (const geometry_msgs::PoseStamped &goal, GoalMode _goal_mode)
 
void Execute (const geometry_msgs::Twist &twist)
 Execute the velocity task with publisher. More...
 
void Execute (const roborts_msgs::TwistAccel &twist_accel)
 Execute the velocity with acceleration task with publisher. More...
 
BehaviorState Update ()
 Update the current chassis executor state. More...
 
void Cancel ()
 Cancel the current task and deal with the mode transition. More...
 
uint32_t GetErrorCode () const
 

Private Types

typedef actionlib::SimpleActionClient< roborts_msgs::GlobalPlannerAction > GlobalActionClient
 
typedef actionlib::SimpleActionClient< roborts_msgs::LocalPlannerAction > LocalActionClient
 
typedef actionlib::SimpleActionClient< roborts_msgs::PIDControllerTowardAngularAction > PIDControllerClient
 

Private Member Functions

void GlobalPlannerFeedbackCallback (const roborts_msgs::GlobalPlannerFeedbackConstPtr &global_planner_feedback)
 
void GlobalPlannerDoneCallback (const actionlib::SimpleClientGoalState &state, const roborts_msgs::GlobalPlannerResultConstPtr &global_planner_result)
 
void PIDControllerFeedbackCallback (const roborts_msgs::PIDControllerTowardAngularFeedbackConstPtr &pid_controller_toward_angular_feedback)
 

Private Attributes

ros::NodeHandle nh_
 
ExcutionMode execution_mode_
 execution mode of the executor More...
 
BehaviorState execution_state_
 execution state of the executor (same with behavior state) More...
 
actionlib::SimpleActionClient< roborts_msgs::GlobalPlannerAction > global_planner_client_
 global planner actionlib client More...
 
actionlib::SimpleActionClient< roborts_msgs::LocalPlannerAction > local_planner_client_
 local planner actionlib client More...
 
actionlib::SimpleActionClient< roborts_msgs::PIDControllerTowardAngularAction > pid_controller_client_
 pid controller actionlib client More...
 
roborts_msgs::GlobalPlannerGoal global_planner_goal_
 global planner actionlib goal More...
 
roborts_msgs::LocalPlannerGoal local_planner_goal_
 local planner actionlib goal More...
 
roborts_msgs::PIDControllerTowardAngularGoal pid_controller_toward_angular_goal_
 pid controller actionlib goal More...
 
ros::Publisher cmd_vel_pub_
 velocity control publisher in ROS More...
 
geometry_msgs::Twist zero_twist_
 zero twist in form of ROS geometry_msgs::Twist More...
 
ros::Publisher cmd_vel_acc_pub_
 velocity with accel publisher in ROS More...
 
roborts_msgs::TwistAccel zero_twist_accel_
 zero twist with acceleration in form of ROS roborts_msgs::TwistAccel More...
 
uint32_t error_code_
 

Member Typedef Documentation

◆ GlobalActionClient

typedef actionlib::SimpleActionClient<roborts_msgs::GlobalPlannerAction> roborts_decision::ChassisExecutor::GlobalActionClient
private

◆ LocalActionClient

typedef actionlib::SimpleActionClient<roborts_msgs::LocalPlannerAction> roborts_decision::ChassisExecutor::LocalActionClient
private

◆ PIDControllerClient

typedef actionlib::SimpleActionClient<roborts_msgs::PIDControllerTowardAngularAction> roborts_decision::ChassisExecutor::PIDControllerClient
private

Member Enumeration Documentation

◆ ExcutionMode

Chassis execution mode for different tasks.

Enumerator
IDLE_MODE 

Default idle mode with no task.

GOAL_USE_PLANNER_MODE 

Goal-targeted task mode using global and local planner.

GOAL_FROM_ODOM_MODE 

Goal-targeted task mode using odom data.

SPEED_MODE 

Velocity task mode.

SPEED_WITH_ACCEL_MODE 

Velocity with acceleration task mode.

◆ GoalMode

Enumerator
GOAL_MODE_USE_GOLBAL_LOCAL_PLANNER 
GOAL_MODE_USE_ODOM_DATA 

Constructor & Destructor Documentation

◆ ChassisExecutor()

roborts_decision::ChassisExecutor::ChassisExecutor ( const ros::NodeHandle &  nh = ros::NodeHandle("~"))
explicit

Constructor of ChassisExecutor.

◆ ~ChassisExecutor()

roborts_decision::ChassisExecutor::~ChassisExecutor ( )
default

Member Function Documentation

◆ Cancel()

void roborts_decision::ChassisExecutor::Cancel ( )

Cancel the current task and deal with the mode transition.

◆ Execute() [1/4]

void roborts_decision::ChassisExecutor::Execute ( const geometry_msgs::PoseStamped &  goal)

Execute the goal-targeted task using global and local planner with actionlib.

Parameters
goalGiven target goal

◆ Execute() [2/4]

void roborts_decision::ChassisExecutor::Execute ( const geometry_msgs::PoseStamped &  goal,
GoalMode  _goal_mode 
)
Parameters
goalGiven target goal
_goal_modeGiven goal mode

◆ Execute() [3/4]

void roborts_decision::ChassisExecutor::Execute ( const geometry_msgs::Twist &  twist)

Execute the velocity task with publisher.

Parameters
twistGiven velocity

◆ Execute() [4/4]

void roborts_decision::ChassisExecutor::Execute ( const roborts_msgs::TwistAccel &  twist_accel)

Execute the velocity with acceleration task with publisher.

Parameters
twist_accelGiven velocity with acceleration

◆ GetErrorCode()

uint32_t roborts_decision::ChassisExecutor::GetErrorCode ( ) const

◆ GlobalPlannerDoneCallback()

void roborts_decision::ChassisExecutor::GlobalPlannerDoneCallback ( const actionlib::SimpleClientGoalState &  state,
const roborts_msgs::GlobalPlannerResultConstPtr &  global_planner_result 
)
private

◆ GlobalPlannerFeedbackCallback()

void roborts_decision::ChassisExecutor::GlobalPlannerFeedbackCallback ( const roborts_msgs::GlobalPlannerFeedbackConstPtr &  global_planner_feedback)
private

◆ PIDControllerFeedbackCallback()

void roborts_decision::ChassisExecutor::PIDControllerFeedbackCallback ( const roborts_msgs::PIDControllerTowardAngularFeedbackConstPtr &  pid_controller_toward_angular_feedback)
private

◆ Update()

BehaviorState roborts_decision::ChassisExecutor::Update ( )

Update the current chassis executor state.

Returns
Current chassis executor state(same with behavior state)

Member Data Documentation

◆ cmd_vel_acc_pub_

ros::Publisher roborts_decision::ChassisExecutor::cmd_vel_acc_pub_
private

velocity with accel publisher in ROS

◆ cmd_vel_pub_

ros::Publisher roborts_decision::ChassisExecutor::cmd_vel_pub_
private

velocity control publisher in ROS

◆ error_code_

uint32_t roborts_decision::ChassisExecutor::error_code_
private

◆ execution_mode_

ExcutionMode roborts_decision::ChassisExecutor::execution_mode_
private

execution mode of the executor

◆ execution_state_

BehaviorState roborts_decision::ChassisExecutor::execution_state_
private

execution state of the executor (same with behavior state)

◆ global_planner_client_

actionlib::SimpleActionClient<roborts_msgs::GlobalPlannerAction> roborts_decision::ChassisExecutor::global_planner_client_
private

global planner actionlib client

◆ global_planner_goal_

roborts_msgs::GlobalPlannerGoal roborts_decision::ChassisExecutor::global_planner_goal_
private

global planner actionlib goal

◆ local_planner_client_

actionlib::SimpleActionClient<roborts_msgs::LocalPlannerAction> roborts_decision::ChassisExecutor::local_planner_client_
private

local planner actionlib client

◆ local_planner_goal_

roborts_msgs::LocalPlannerGoal roborts_decision::ChassisExecutor::local_planner_goal_
private

local planner actionlib goal

◆ nh_

ros::NodeHandle roborts_decision::ChassisExecutor::nh_
private

◆ pid_controller_client_

actionlib::SimpleActionClient<roborts_msgs::PIDControllerTowardAngularAction> roborts_decision::ChassisExecutor::pid_controller_client_
private

pid controller actionlib client

◆ pid_controller_toward_angular_goal_

roborts_msgs::PIDControllerTowardAngularGoal roborts_decision::ChassisExecutor::pid_controller_toward_angular_goal_
private

pid controller actionlib goal

◆ zero_twist_

geometry_msgs::Twist roborts_decision::ChassisExecutor::zero_twist_
private

zero twist in form of ROS geometry_msgs::Twist

◆ zero_twist_accel_

roborts_msgs::TwistAccel roborts_decision::ChassisExecutor::zero_twist_accel_
private

zero twist with acceleration in form of ROS roborts_msgs::TwistAccel


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