【toolschain algorithm cpp ros】cpp工厂模式实现--后续填充具体规划算法，控制器版的已填充了算法接入了仿真器

news2025/2/3 12:47:47

写在前面

现在局势危机，于是想复习一下之前写的设计模式，之前提到，做过一个闭环仿真器（借用ros），见https://blog.csdn.net/weixin_46479223/article/details/134864123我的控制器的建立遵循了工厂模式（多态、工厂、客户订阅）。目前按照之前的设计，把规划器也改写成类似的模式。未来接入我的闭环仿真器械中。

本篇文章只设计框架的搭建，具体算法lattice搭建完毕填充以及其他算法为看时间缓慢的更新吧。

有错误和问题还请大大们指点阿

基本结构

在这里插入图片描述
最外面是我的ros节点（目前刚建立没有接入ros以及控制器用while先测试了）

然后包一个planner的调度器（scheduler：planner.cpp）
传建一个基类然后动态多态的方式实现不同规划器的注册，把根据类型注册的规划器放入到一个哈希map，实现增删。（这里临时固定）。控制器和规划器的这个架构原来的设想是根据不同的场景可以有一个判断器自动实现合适的控制器和规划器的切换（也就是自动驾驶行泊一体啦）

对外接口是run和init和update，基本每行代码都有注释。

参数使用yaml管理

跑起来的样子

之前跑起来的并且接入了我的闭环无敌可进化仿真器的视频。我的git也放了一版。
这里不能贴视频。
仿真器想表现车辆追随一段轨迹，前面是轨迹（预瞄）。后面是我的车实不断更新的位置。（实现了闭环）
在这里插入图片描述
0 1 2 3 4 是假设的控制类型，这里先跑0.

部分重要代码

代码在git上

origin git@github.com:HerrQQ/MPotherSet.git (fetch)
origin git@github.com:HerrQQ/MPotherSet.git (push)
第一版如此
未来不断调整

工厂类

#ifndef _PlannerFactory_H_
#define _PlannerFactory_H_

#include "planner_A.h"
#include "planner_B.h"
#include "planner_C.h"
#include "planner_D.h"

/**
 * @brief PlannerFactory
 * @author kaifeng
 * @date 
*/
#include <memory>
namespace planner { 
class PlannerFactory//factory 
{
public :

    PlannerFactory()=default;
    ~PlannerFactory()=default;
    /**
    * @brief creat planner
    * @param needed planner type
    */
    std::shared_ptr<PlannerBase> fCreateplanner (const PlannerType& type)
    {
        if (type == B) {
            return std::make_shared<planner_B>();
        }
        else if (type == A) {
            return std::make_shared<planner_A>();
        }
        else if (type == C) {
            return std::make_shared<planner_C>();
        }
        else if (type == D) {
            return std::make_shared<planner_D>();
        }
        else {
            return nullptr;
        }
    }

};
}

// class PlannerFactory_D :public PlannerFactory //factory son
// {
// public :

//     PlannerFactory_D()=default;
//     virtual ~PlannerFactory_D() override
//     {
//         LOG(INFO)<<"D plannerBase created"<<std::endl;
//     }
//     virtual planner::plannerBase* fCreateplanner ()override
//     {
//         return new planner::planner_D();
//     }
// };

// class PlannerFactory_A :public PlannerFactory //factory son
// {
// public :

//     PlannerFactory_A()=default;
//     virtual ~PlannerFactory_A() override
//     {
//         LOG(INFO)<<"A plannerBase created"<<std::endl;
//     }
//     virtual planner::plannerBase* fCreateplanner ()override
//     {
//         return new planner::planner_A();
//     }
// };

// class PlannerFactory_B :public PlannerFactory //factory son
// {
// public :

//     PlannerFactory_B()=default;
//     virtual ~PlannerFactory_B() override
//     {
//         LOG(INFO)<<"B plannerBase created"<<std::endl;
//     }
//     virtual planner::plannerBase* fCreateplanner ()override
//     {
//         return new planner::planner_B();
//     }
// };

// class PlannerFactory_B :public PlannerFactory //factory son
// {
// public :

//     PlannerFactory_B()=default;
//     virtual ~PlannerFactory_B() override
//     {
//         LOG(INFO)<<"B plannerBase created"<<std::endl;
//     }
//     virtual planner::plannerBase* fCreateplanner ()override
//     {
//         return new planner::planner_B();
//     }
// };

#endif

基类

#ifndef _PLANNER_H_
#define _PLANNER_H_
#include "structPlanner_t.h"
#include <Eigen/Dense>
#include <vector>
#include <map>
#include <unordered_map>
#include "glog/logging.h"
#include <yaml-cpp/yaml.h>


/**
 * @brief Planner_base
 * @author kaifeng
 * @date 
*/




namespace planner { 

/**
 * @brief base class for all planners.
 */
class PlannerBase//client
{
    public:
  /**
   * @brief constructor
   */
  PlannerBase() = default;

  /**
   * @brief destructor
   */
  virtual ~PlannerBase() = default;

  /**
   * @brief initialize PlannerBase
   * @param config_path path of config file, yaml  
   * @return Status initialization status
   */
  virtual uint8_t initialize(const std::string& config_path) = 0;

  
  /**
   * @brief compute planner command
   * @param PlannerDataFlow saving relativ infos of planning
   * @param locInfo reference of LocInfo instance
   * @param VehicleState reference of VehicleState instance
   * @return reference of Command instance, which is private member
   */
  virtual void run( const Location& locInfo,
                    const VehicleState& VehicleState,
                    PlannerDataFlow& PlannerDataFlow) = 0;

  virtual void noRun(const Location& locInfo,
                    const VehicleState& VehicleState) = 0;

  /**
   * @brief stop planner
   */
  // virtual void stop();

protected:
    double cmd_; 
  //Command cmd_;
  //Debug debug_ptr_;
};

}

#endif

其中一个子class

#ifndef __A_planner_h__
#define __A_planner_h__

#include "planner_base.h"
#include "glog/logging.h"
#include <vector>
#include <map>
#include <cmath>
#include <utility>



/**
 * @brief A
 * @author kaifeng
 * @date 
*/
namespace planner
{
    class planner_A:public PlannerBase{ // product son
        public:
        planner_A()
        {
            LOG(INFO)<<"A in progress"<<std::endl;
        }
        ~planner_A()
        {
            LOG(INFO)<<"A stop"<<std::endl;
        }
       
        uint8_t initialize(const std::string& config_path)override;

        void run( const Location& locInfo,
                            const VehicleState& VehicleState,
                            PlannerDataFlow& PlannerDataFlow) override;

        void noRun(const Location& locInfo,
                            const VehicleState& VehicleState) override;
    
        private:
        /**
         * @brief read configuration
        */
        void readConfig(const std::string& path);

        private:
        // cost parameters
        struct CostParameter {
            float Xxxx{0.f};
            float xXxx{0.f};
            float xxXx{0.f};
            float xxxX{0.f};
        };
        CostParameter* p_parameter_;
        // 

    
    
    
    };

}
#endif