chassis_executor.h

Go to the documentation of this file.

#ifndef ROBORTS_DECISION_CHASSIS_EXECUTOR_H
#define ROBORTS_DECISION_CHASSIS_EXECUTOR_H
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <nav_msgs/Odometry.h>
 
#include "roborts_msgs/GlobalPlannerAction.h"
#include "roborts_msgs/LocalPlannerAction.h"
#include "roborts_msgs/PIDControllerTowardAngularAction.h"
#include "roborts_msgs/TwistAccel.h"
#include "geometry_msgs/Twist.h"
 
#include "../behavior_tree/behavior_state.h"
#include "pid_controller/pid_controller.h"
 
#include <thread>
 
namespace roborts_decision {
/***
 * @brief Chassis Executor to execute different abstracted task for chassis module
 */
class ChassisExecutor {
 
  typedef actionlib::SimpleActionClient<roborts_msgs::GlobalPlannerAction> GlobalActionClient;
  typedef actionlib::SimpleActionClient<roborts_msgs::LocalPlannerAction> LocalActionClient;
  typedef actionlib::SimpleActionClient<roborts_msgs::PIDControllerTowardAngularAction> PIDControllerClient;
 
 public:
  enum class ExcutionMode {
    IDLE_MODE,            
    GOAL_USE_PLANNER_MODE,            
    GOAL_FROM_ODOM_MODE,  
    SPEED_MODE,           
    SPEED_WITH_ACCEL_MODE 
  };
 
  enum class GoalMode {
    GOAL_MODE_USE_GOLBAL_LOCAL_PLANNER,
    GOAL_MODE_USE_ODOM_DATA
  };
  explicit ChassisExecutor(const ros::NodeHandle &nh = ros::NodeHandle("~"));
  ~ChassisExecutor() = default;
  void Execute(const geometry_msgs::PoseStamped &goal);
  void Execute(const geometry_msgs::PoseStamped &goal, GoalMode _goal_mode);
  void Execute(const geometry_msgs::Twist &twist);
  void Execute(const roborts_msgs::TwistAccel &twist_accel);
  BehaviorState Update();
  void Cancel();
 
  uint32_t GetErrorCode() const;
 
 private:
 
  ros::NodeHandle nh_;
  /***
   * @brief Global planner actionlib feedback callback function to send the global planner path to local planner
   * @param global_planner_feedback  Global planner actionlib feedback, which mainly consists of global planner path output
   */
  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);
  ExcutionMode execution_mode_;
  BehaviorState execution_state_;
 
  actionlib::SimpleActionClient<roborts_msgs::GlobalPlannerAction> global_planner_client_;
  actionlib::SimpleActionClient<roborts_msgs::LocalPlannerAction> local_planner_client_;
  actionlib::SimpleActionClient<roborts_msgs::PIDControllerTowardAngularAction> pid_controller_client_;
  roborts_msgs::GlobalPlannerGoal global_planner_goal_;
  roborts_msgs::LocalPlannerGoal local_planner_goal_;
  roborts_msgs::PIDControllerTowardAngularGoal pid_controller_toward_angular_goal_;
 
  ros::Publisher cmd_vel_pub_;
  geometry_msgs::Twist zero_twist_;
 
  ros::Publisher cmd_vel_acc_pub_;
  roborts_msgs::TwistAccel zero_twist_accel_;
 
  uint32_t error_code_;
 
  // std::thread update_thread_;
//  bool LoadParam(const std::string &proto_file_path);
//  double chassis_v2p_pid_kp;
//  double chassis_v2p_pid_ki;
//  double chassis_v2p_pid_kd;
//  bool chassis_v2p_pid_has_threshold;
//  double chassis_v2p_pid_threshold;
 
};
}
 
#endif //ROBORTS_DECISION_CHASSIS_EXECUTOR_H