offbroad

最新推荐文章于 2024-04-09 17:58:53 发布

学习小卡车

最新推荐文章于 2024-04-09 17:58:53 发布

阅读量230

点赞数

分类专栏： ROS机器人学习文章标签：自动驾驶

本文链接：https://blog.csdn.net/m0_37769093/article/details/117375084

版权

ROS机器人学习专栏收录该内容

8 篇文章 2 订阅

订阅专栏

//action文件夹
class IControlFunction {
    virtual ~IControlFunction() {};
    virtual void Oninit(const int config) = 0;
    virtual void GetData() = 0;
    virtual void DoProgress() = 0;
    virtual void SetFunctionOutPut() = 0;}
//工厂基函数，初始化、获取数据、做处理、输出

class IMsgRosManager {}
//消息管理，飞机、船、UUV的控制和设置他们的状态

class Ivehicle {}
//uav全局位置、状态、局部位置

class MultiUAVFormation : public IControlFunction{}
class MultiUSVFormation : public IControlFunction{}
class USV_Avoidance : public IControlFunction {}

//VehicleControl文件夹
class IVehicleControl {
public:
        virtual ~IVehicleControl() {};
        virtual void onInit() = 0;
        virtual void DoProgress() = 0;
        virtual void chooseLeader() = 0;
        virtual void getData() = 0;
        virtual void setVehicleCtrlData() = 0;};
//初始化、做处理、选择领航者、获取数据、设置飞行控制数据

class MultiBoatControl : public IVehicleControl {}
class MultiDroneControl : public IVehicleControl {}
class MultiUUVControl : public IVehicleControl {}

//Calculate文件
class Calculate {}
//一些角度弧度转换，四元数转欧拉角(quaternion_rpy)

//Cinc文件
//一些通用参数和系数配置

//DataMan文件  < ------ >  数据交互，非常重要
class DataMan {}

//FlightManager文件
class FlightManager : public IFlightDataCallback{
    void OnInitConfig(IMsgRosManager* msg_manager);
    void DoProgress();
    void GetData();
    static FlightManager* getInstance();
    void AddVehicleControlProgress(IVehicleControlFactory *IVehicleControl);
    void AddFunctionProgress(IFunctionFactory *factory);
    void FunctionStateUpdate(IFunctionFactory *factory);
    void ActionMotionPlanUpdate(IFunctionFactory *factory);
    void OnFlightDataUpdate(FDATA_TYPE data_type) override;}

//multi_offbroad文件
class MultiOffboard : public IMsgRosManager{}
//都是一些MAVROS消息

//PathCreator.hpp
class PathCreator {
public:
    void onInit(IMsgRosManager *msg_manager, const bool is_uav_follow);
    void initMotionPlan();
    void uav_add_way_points(vector<geometry_msgs::PoseStamped> &uav_way_points);
    void usv_add_way_points(vector<geometry_msgs::PoseStamped> &usv_way_points);
    void CreatFunction();
    void CreatVehicle();
    void CreateUAVFormationInit(const int config);
    void CreateUSVFormationInit(const int config);
    static PathCreator* geInstance();}

//util.h
void util_set_id (const char *name, int number, const char *prog);
void util_log(const char *fmt, ...);
void util_add_remote (int sock, struct sockaddr_in sa);
void util_daemonize ();
char *util_string_trim (const char *buf, int size);
string_list_t util_string_split (const char *haystack, const char *needle, int at_most);
void util_string_list_free (string_list_t *list);
char *util_string_replace_once (const char *string, const char *original, const char *replace);
void *util_realloc (void *ptr, int size);
char *util_strndup (const char *str, int size);
void util_free (void *ptr);
void util_free_all (void);
void util_display_resource (void);
void util_SetLogFile(char logfile[256]);

3USV

//usvs_control.hpp
class usvs_control : public IMsgRosManager{
    static  usvs_control* getInstance();
    void OnInit() override ;
    void PublishDronePosControl(const multi_vehicle &multi_vehicles) override ;
    void PublishBoatPosControl(const multi_vehicle &multi_vehicles) override ;
    void PublishUUVPosControl(const multi_vehicle &multi_vehicles) override;
    void SetUAVState(mavros_msgs::SetMode &m_mode) override ;
    void SetUSVState(mavros_msgs::SetMode &arm_command, int usv_id) override ;
    void SetUSVAvoData(const bool usv1_usv2_crash, const bool usv1_usv3_crash, const bool usv2_usv3_crash) override ;
    void getData();
    void doProgress();}

PCL

class IMap {
public:
    virtual void onInit() = 0;
    virtual bool isStateValid(const Eigen::Vector3f &PosENU) = 0;
    virtual void updateOctomap(const octomap_msgs::Octomap &msg) = 0;
    virtual void setSafeRaduis(const float &raduis) = 0;};

class PCLROSMessageManager {
public:
    void OnInit(ros::NodeHandle &nh);
    void cloudHandler(const sensor_msgs::PointCloud2::ConstPtr &m);
    void getOctomap(octomap_msgs::Octomap &octomap);
    void setVehicleMessage(const M_Drone& usv);
private:
    ros::Subscriber lidar_point_sub_;
    ros::Publisher octomap_pub_, transformed_cloud_pub_, ground_removal_pub_;
    M_Drone usv_;
    octomap_msgs::Octomap octomap_;
    octomap::OcTree *tree_ = new octomap::OcTree(0.5);
    //发布八叉树
    void PubOctomap(octomap::OcTree *tree, const ros::Publisher &pub);
    //半径消除
    void radiusRemoval(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                       const pcl::PointCloud<pcl::PointXYZ>::Ptr &output_cloud,
                       float radius, int min_neighbors,
                       pcl::IndicesConstPtr &cloud_filtered_indices);
    //地面消除
    void groundRemove(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                      const pcl::PointCloud<pcl::PointXYZ>::Ptr &output_cloud,
                      const pcl::IndicesConstPtr &cloud_filtered_indices);
    Eigen::Isometry3f get_transformation_matrix();
    void PubPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &o_cloud, const ros::Publisher &pub, string frame = "map");
};

class OctoMap  : public IMap {
public:
    void onInit() override;
    void updateOctomap(const octomap_msgs::Octomap &msg) override;
    bool isStateValid(const Eigen::Vector3f &PosENU) override ;
    void setSafeRaduis(const float &raduis) override;
    typedef unique_ptr<OctoMap> Ptr;
private:
    float safety_radius_;
    std::shared_ptr<fcl::CollisionGeometry> Quadcopter_;
    std::shared_ptr<fcl::CollisionGeometry> tree_obj_;};

motion_plan

//bspline
//bspline.hpp
namespace fast_planner {
// An implementation of non-uniform B-spline with different dimensions
// It also represents uniform B-spline which is a special case of non-uniform
class NonUniformBspline {
private:
  // control points for B-spline with different dimensions.
  // Each row represents one single control point
  // The dimension is determined by column number
  // e.g. B-spline with N points in 3D space -> Nx3 matrix
  Eigen::MatrixXd control_points_;
  int             p_, n_, m_;  // p degree, n+1 control points, m = n+p+1
  Eigen::VectorXd u_;          // knots vector
  double          interval_;   // knot span \delta t
  Eigen::MatrixXd getDerivativeControlPoints();
  double limit_vel_, limit_acc_, limit_ratio_;  // physical limits and time adjustment ratio
public:
  // initialize as an uniform B-spline
  void setUniformBspline(const Eigen::MatrixXd& points, const int& order, const double& interval);
  // get / set basic bspline info
  void                                   setKnot(const Eigen::VectorXd& knot);
  Eigen::VectorXd                        getKnot();
  Eigen::MatrixXd                        getControlPoint();
  double                                 getInterval();
  void                                   getTimeSpan(double& um, double& um_p);
  pair<Eigen::VectorXd, Eigen::VectorXd> getHeadTailPts();
  // compute position / derivative
  Eigen::VectorXd   evaluateDeBoor(const double& u);   // use u \in [up, u_mp]
  Eigen::VectorXd   evaluateDeBoorT(const double& t);  // use t \in [0, duration]
  NonUniformBspline getDerivative();
  // 3D B-spline interpolation of points in point_set, with boundary vel&acc
  // constraints
  // input : (K+2) points with boundary vel/acc; ts
  // output: (K+6) control_pts
  static void parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
                                    const vector<Eigen::Vector3d>& start_end_derivative,
                                    Eigen::MatrixXd&               ctrl_pts);
  /* check feasibility, adjust time */
  void   setPhysicalLimits(const double& vel, const double& acc);
  bool   checkFeasibility(bool show = false);
  double checkRatio();
  void   lengthenTime(const double& ratio);
  bool   reallocateTime(bool show = false);
  /* for performance evaluation */
  double getTimeSum();
  double getLength(const double& res = 0.01);
  double getJerk();
  void   getMeanAndMaxVel(double& mean_v, double& max_v);
  void   getMeanAndMaxAcc(double& mean_a, double& max_a);
  void recomputeInit();
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}

//bspline_opt
//bspline_optimizer.h
// Gradient and elasitc band optimization

// Input: a signed distance field and a sequence of points
// Output: the optimized sequence of points
// The format of points: N x 3 matrix, each row is a point
namespace fast_planner {
class BsplineOptimizer {
public:
  static const int SMOOTHNESS;
  static const int DISTANCE;
  static const int FEASIBILITY;
  static const int ENDPOINT;
  static const int GUIDE;
  static const int WAYPOINTS;
  static const int YAW_SMOOTHNESS;
  static const int DIST_CENTER;

  static const int GUIDE_PHASE;
  static const int NORMAL_PHASE;
  static const int SAFE;
  /* main API */
  void            setEnvironment(const Sp<IMap>& env);
    void setSpeedLimit(const double max_vel, const double max_acc);
    void            setParam(ros::NodeHandle& nh);
  Eigen::MatrixXd BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
                                      const int& cost_function, int max_num_id, int max_time_id);
  void setTargetPoint(const Eigen::Vector3d &target_pos, const Eigen::Vector3d &start_point);
  /* helper function */

  // required inputs
  void setControlPoints(const Eigen::MatrixXd& points);
  void setBsplineInterval(const double& ts);
  void setCostFunction(const int& cost_function);
  void setTerminateCond(const int& max_num_id, const int& max_time_id);
  // optional inputs
  void setGuidePath(const vector<Eigen::Vector3d>& guide_pt);
  void setWaypoints(const vector<Eigen::Vector3d>& waypts,
                    const vector<int>&             waypt_idx);  // N-2 constraints at most
  void optimize();
  Eigen::MatrixXd         getControlPoints();
  vector<Eigen::Vector3d> matrixToVectors(const Eigen::MatrixXd& ctrl_pts);
private:
  EDTEnvironment::Ptr edt_environment_;
  Sp<IMap> map_;
  // main input
  Eigen::MatrixXd control_points_;     // B-spline control points, N x dim
  double          bspline_interval_;   // B-spline knot span
  Eigen::Vector3d end_pt_;             // end of the trajectory
  int             dim_;                // dimension of the B-spline
                                       //
  vector<Eigen::Vector3d> guide_pts_;  // geometric guiding path points, N-6
  vector<Eigen::Vector3d> waypoints_;  // waypts constraints
  vector<int>             waypt_idx_;  // waypts constraints index
                                       //
  int    max_num_id_, max_time_id_;    // stopping criteria
  int    cost_function_;               // used to determine objective function
  bool   dynamic_;                     // moving obstacles ?
  double start_time_;                  // global time for moving obstacles
  /* optimization parameters */
  int    order_;                  // bspline degree
  double lambda1_;                // jerk smoothness weight
  double lambda2_;                // distance weight
  double lambda3_;                // feasibility weight
  double lambda4_;                // end point weight
  double lambda5_;                // guide cost weight
  double lambda6_;                // visibility cost weight
  double lambda7_;                // waypoints cost weight
  double lambda8_;                // acc smoothness
  double lambda9_;                // control points to center line distance weight
                                  //
  double dist0_;                  // safe distance
  double dist_min_;                  // safe distance
  double dist_max_;                  // safe distance
  double max_vel_, max_acc_;      // dynamic limits
  double visib_min_;              // threshold of visibility
  double wnl_;                    //
  double dlmin_;                  //
                                  //
  int    algorithm1_;             // optimization algorithms for quadratic cost
  int    algorithm2_;             // optimization algorithms for general cost
  int    max_iteration_num_[4];   // stopping criteria that can be used
  double max_iteration_time_[4];  // stopping criteria that can be used
  Eigen::Vector3d target_pos_, start_pos_;
  /* intermediate variables */
  /* buffer for gradient of cost function, to avoid repeated allocation and
   * release of memory */
  vector<Eigen::Vector3d> g_q_;
  vector<Eigen::Vector3d> g_smoothness_;
  vector<Eigen::Vector3d> g_distance_;
  vector<Eigen::Vector3d> g_dist_center_;
  vector<Eigen::Vector3d> g_feasibility_;
  vector<Eigen::Vector3d> g_endpoint_;
  vector<Eigen::Vector3d> g_guide_;
  vector<Eigen::Vector3d> g_waypoints_;

  int                 variable_num_;   // optimization variables
  int                 iter_num_;       // iteration of the solver
  std::vector<double> best_variable_;  //
  double              min_cost_;       //

  vector<Eigen::Vector3d> block_pts_;  // blocking points to compute visibility

  /* cost function */
  /* calculate each part of cost function with control points q as input */

  static double costFunction(const std::vector<double>& x, std::vector<double>& grad, void* func_data);
  void          combineCost(const std::vector<double>& x, vector<double>& grad, double& cost);
  // q contains all control points
  void calcSmoothnessCost(const vector<Eigen::Vector3d>& q, double& cost,
                          vector<Eigen::Vector3d>& gradient);
  void calcDistanceCost(const vector<Eigen::Vector3d>& q, double& cost,
                        vector<Eigen::Vector3d>& gradient);
  void calcDistCenterCost(const vector<Eigen::Vector3d>& q, double& cost,
                        vector<Eigen::Vector3d>& gradient);
  void calcFeasibilityCost(const vector<Eigen::Vector3d>& q, double& cost,
                           vector<Eigen::Vector3d>& gradient);
  void calcEndpointCost(const vector<Eigen::Vector3d>& q, double& cost,
                        vector<Eigen::Vector3d>& gradient);
  void calcGuideCost(const vector<Eigen::Vector3d>& q, double& cost, vector<Eigen::Vector3d>& gradient);
  void calcVisibilityCost(const vector<Eigen::Vector3d>& q, double& cost,
                          vector<Eigen::Vector3d>& gradient);
  void calcWaypointsCost(const vector<Eigen::Vector3d>& q, double& cost,
                         vector<Eigen::Vector3d>& gradient);
  void calcViewCost(const vector<Eigen::Vector3d>& q, double& cost, vector<Eigen::Vector3d>& gradient);
  bool isQuadratic();
  /* for benckmark evaluation only */
public:
  vector<double> vec_cost_;
  vector<double> vec_time_;
  ros::Time      time_start_;
  void getCostCurve(vector<double>& cost, vector<double>& time) {
    cost = vec_cost_;
    time = vec_time_;
  }
  typedef unique_ptr<BsplineOptimizer> Ptr;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}  // namespace fast_planner

mp_core

namespace fast_planner{
    class IPathFinder{
    public:
        virtual bool replan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc, Eigen::Vector3d end_pt, Eigen::Vector3d end_vel) = 0;
        virtual void planYaw(const Eigen::Vector3d& start_yaw) = 0;
        virtual void initPlanModules(const MP_Config &config, Sp<IMap> &map) = 0;
        virtual bool checkTrajCollision(double& distance) = 0;
        virtual LocalTrajData& getLocaldata() = 0;
        virtual MidPlanData& getPlanData() = 0;
        virtual void setGlobalWaypoints(const TVec3 &waypoints, const TVec3 &start_point, const TVec3 &cur_pos) = 0;
        virtual void updateSpeedLimit(const float max_speed,const float max_acc)=0;
    };
}

//FastPathFinder.h
namespace fast_planner {

// Fast Planner Manager
// Key algorithms of mapping and planning are called
    class FastPathFinder : public IPathFinder{
        // SECTION stable
    public:
        /* main planning interface */
        bool replan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc,
                               Eigen::Vector3d end_pt, Eigen::Vector3d end_vel) override ;
        bool planGlobalTraj(const Eigen::Vector3d& start_pos);

        void planYaw(const Eigen::Vector3d& start_yaw) override ;

        void initPlanModules(const MP_Config &config, Sp<IMap> &map) override ;
        void setGlobalWaypoints(const TVec3 &waypoints, const TVec3 &start_point, const TVec3 &cur_pos) override;

        bool checkTrajCollision(double& distance) override ;

        LocalTrajData& getLocaldata() override ;
        MidPlanData& getPlanData() override;

        void updateSpeedLimit(const float max_speed, const float max_acc) override;

        PlanParameters pp_;
        LocalTrajData local_data_;
        MidPlanData plan_data_;
        EDTEnvironment::Ptr edt_environment_;   
    private:
        ros::Subscriber plan_state_sub_;
        /* main planning algorithms & modules */
        SDFMap::Ptr sdf_map_;
        GlobalTrajData global_data_;

        unique_ptr<KinodynamicAstar> kino_path_finder_;
        vector<BsplineOptimizer::Ptr> bspline_optimizers_;
        OctoMap::Ptr octo_map_;
        MP_Config mp_config_;

        void updateTrajInfo();

        // topology guided optimization

        void findCollisionRange(vector<Eigen::Vector3d>& colli_start, vector<Eigen::Vector3d>& colli_end,
                                vector<Eigen::Vector3d>& start_pts, vector<Eigen::Vector3d>& end_pts);

        Eigen::MatrixXd reparamLocalTraj(double start_t, double& dt, double& duration);
        Eigen::MatrixXd reparamLocalTraj(double start_t, double duration, int seg_num, double& dt);

        void selectBestTraj(NonUniformBspline& traj);
        void refineTraj(NonUniformBspline& best_traj, double& time_inc);
        void reparamBspline(NonUniformBspline& bspline, double ratio, Eigen::MatrixXd& ctrl_pts, double& dt,
                            double& time_inc);

        // heading planning
        void calcNextYaw(const double& last_yaw, double& yaw);

        // !SECTION stable

        // SECTION developing

    public:
        typedef unique_ptr<FastPathFinder> Ptr;

        // !SECTION
    };
}  // namespace fast_planner

//MPManager.h
class MPManager{
public:
    MPManager(const MP_Config& config);
    void SetMpEnable(bool is_enable);
    void OnUpdateDroneStatus(const TVec3 &drone_pos, const TVec3 &drone_vel, const TVec3 &drone_acc, const TVec3 &drone_attitude);
    void OnUpdateTargetPos(const TVec3& target_pos);
    void OnUpdateMaxSpeed(float speed_max);
    void OnUpdateDroneHeading(float drone_heading);
    bool GetControlOutput(TVec3& vector_eus);
    void ProcessState();
    void updateMotionPlan(const float dist, const TVec3 &insp_vec,
                          const vector<TVec3> &waypoints);
    void bsplineResult(const offboard::Bspline& msg);
    void OnUpdateOctomap(const octomap_msgs::Octomap &msg);
    void updateEndVel(const TVec3 &end_vel);
private:
    enum MP_EXEC_STATE {
        INIT,
        WAIT_TARGET,
        GEN_NEW_TRAJ,
        REPLAN_TRAJ,
        EXEC_TRAJ,
        REPLAN_NEW
    };
    ros::NodeHandle nh_;
    MP_Config mp_config_;
    Drone_State drone_st_;
    MP_EXEC_STATE mp_state_;
    TVec3 start_pt_,start_vel_,start_acc_, end_pos_, end_vel_;
    TVec3 pre_end_pt_;
    ros::Time start_time_;
    vector<NonUniformBspline> real_traj_;
    vector<NonUniformBspline> plan_traj_;
    double traj_duration_;
    float dist_config_;
    TVec3 insp_vec_ENU_;
    float min_dist_to_line_ = 100000;

    Sp<IPathFinder> path_finder_;
    Sp<MPPublisher> mp_publisher_;
    Sp<IMap> mp_map_;
    //Sp<FastPathFinder> planner_manager_;
    //PlanningVisualization::Ptr visualization_;

    vector<TVec3> traj_cmd_;

    bool receive_traj_;
    bool has_drone_update_;
    TVec3 init_target_pos_;
    int path_find_fail_timer_;

    // private functions
    void CalcDistToCenter(float &dist, const TVec3 &cur_pos, TVec3 start_pos, TVec3 end_pos);
    bool CallKinodynamicReplan();
    void ChangeExecState(MP_EXEC_STATE new_state, string pos_call);
    void initAfterTriggered(const TVec3& target_pos);
    void changeToTurbineFrame(TVec3 &pnt, TVec3 &pos_in_turbine_EUS);
};

class MPPublisher{
public:
    void OnInit(const float state);
    void DrawTrajCommand(const TVec3 &pos, const TVec3 &vec, const Eigen::Vector4d &color, int id);
    void DrawFlightCorridor(TVec3 &start_point, TVec3 &end_point);
    bool updateMPTarget(const float dist, const vector<TVec3> &waypoints);
    void updateDroneData(const TVec3 &drone_pos);
    void displayTrajWithColor(vector<TVec3> &path, double resolution, const Eigen::Vector4d &color, int id);
    void drawGoal(TVec3 goal, double resolution, const Eigen::Vector4d& color, int id = 0);
    void drawGeometricPath(const vector<Eigen::Vector3d>& path);
    void drawBspline(NonUniformBspline& bspline);
private:
    void calcDistToCenter(float &dist, const TVec3 &cur_pos, TVec3 start_pos, TVec3 end_pos);
    ros::Publisher  motionPlanPub_, mp_drone_pos_update_pub_, mp_track_distance_pub_, cmd_vis_pub_, pos_cmd_pub, traj_pub_, drone_vel_sp_pub_, mp_state_pub_, marker_blade_pub_, marker_path_pub_, fitting_path_pub_, flight_corridor_pub_, path_update_pub_; Sp<PlanningVisualization> visualization_;

    float dist_config_;
    bool drone_update_;
    bool KF_init_;
    int mp_state_;
    int traj_pub_time_;

    TVec3 drone_pos_;
    vector<TVec3> wayPoints_;};

学习小卡车

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
offbroad

//action文件夹class IControlFunction { virtual ~IControlFunction() {}; virtual void Oninit(const int config) = 0; virtual void GetData() = 0; virtual void DoProgress() = 0; virtual void SetFunctionOutPut() = 0;}//工厂基函数，初始化、获取数据、做处理、输出c
复制链接

扫一扫