add planning code

dora-rs · May 21, 2024 · 1459eec · 1459eec
1 parent 0ded6dc
commit 1459eec
Show file tree

Hide file tree

Showing 61 changed files with 5,279 additions and 0 deletions.
diff --git a/control/README.md b/control/README.md
diff --git a/control/include/ControlMode.h b/control/include/ControlMode.h
@@ -0,0 +1,18 @@
+#ifndef CONTROLMODE_H
+#define CONTROLMODE_H
+
+#include <iostream>
+#include <cstdint>
+
+struct ControlMode_h
+{
+    uint8_t      start;                     //qidong
+    uint8_t      mode_type;                 //模式类型  00：无， 自动模式：（11：自动生成地图，12：已有地图） 遥控模式：20
+    uint8_t      enable;                    //紧急使能  
+    uint8_t      brake_enable;              //制动使能  0：制动，1：可行驶
+    float    speed_require;             //速度要求
+    float    angle_require;             //角度要求
+};
+
+
+#endif
diff --git a/control/include/Controller.h b/control/include/Controller.h
@@ -0,0 +1,39 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include <iostream>
+#include <cstdint>
+
+struct TrqBreCmd_h
+{
+    uint8_t trq_enable;        //发动机驱动扭矩是否使能
+    uint32_t trq_value_2;       //->>扭矩:所需扭矩值     #2号车使用
+    float trq_value_3;         //->>扭矩:所需扭矩百分比  #3号车使用
+    uint8_t bre_enable;         //刹车是否使能
+    float bre_value;            //刹车值
+    uint8_t ACC_DecToStop;      //ACC请求减速到停止 0x0: no demand; 0x1: demand 车子按自己的减速度平滑停车 不受发送的其他指令控制
+    uint8_t ACC_Driveoff;       //释放汽缸压力，必须与ACC_DecToStop配合使
+};
+
+struct SteeringCmd_h
+{
+    float SteeringAngle;         //->>方向： 发送方向角度值
+};
+
+struct AEBCmd_h
+{
+    uint8_t AEB_enable;
+    uint8_t ACC_DecToStop;
+    float AEB_bre_value;
+};
+
+struct PRNDShiftCmd_h
+{
+    uint8_t APA_TransPRNDShiftEnable;
+    uint8_t APA_TransPRNDShiftReqValid;
+    uint8_t APA_TransPRNDShiftRequest;
+};
+
+
+
+#endif
diff --git a/control/include/LaneLine.h b/control/include/LaneLine.h
@@ -0,0 +1,27 @@
+#ifndef LANELINE_H
+#define LANELINE_H
+
+#include <iostream>
+#include <cstdint>
+#include <vector>
+using namespace std;
+
+struct Point_h
+{
+    double x;
+    double y;
+    double z;
+};
+
+struct LaneLine_h
+{
+    uint32_t id;
+    uint8_t typy;
+    vector<double> x;
+    vector<double> y;
+    vector<double> s;
+    vector<Point_h> points;
+    uint8_t current_lane_num;
+};
+
+#endif
diff --git a/control/include/LaneLineArray.h b/control/include/LaneLineArray.h
@@ -0,0 +1,17 @@
+#ifndef LANELINEARRAY_H
+#define LANELINEARRAY_H
+
+#include <iostream>
+#include <cstdint>
+#include <vector>
+#include "LaneLine.h"
+
+using namespace std;
+
+
+struct LaneLineArray_h
+{
+    vector<LaneLine_h> lines;
+};
+
+#endif
diff --git a/control/include/Localization.h b/control/include/Localization.h
@@ -0,0 +1,33 @@
+#ifndef LOCALIZATION_H
+#define LOCALIZATION_H
+
+#include <iostream>
+#include <cstdint>
+
+struct NaviData_h
+{
+    float pitch;          //俯仰角
+    float roll;           //翻滚角
+    float heading;         //航向角
+
+    double longitude;      //经度
+    double latitude;      //纬度
+    double altitude;      //海拔
+
+    float speed2d;         //车辆速度
+// ------------------------------
+    float Ve;
+
+    float Vn;
+
+    float Vu;
+// --------------------------------
+    int32_t  pose_type;     //定位状态
+    double gpsTime;       
+
+    int32_t INS_Status;       //解算状态
+    float Lat_vari;         //纬度标准差
+    float Lon_vari;         //经度标准差
+};
+
+#endif
diff --git a/control/include/Planning.h b/control/include/Planning.h
@@ -0,0 +1,34 @@
+#ifndef PLANNING_H
+#define PLANNING_H
+
+#include <iostream>
+#include <cstdint>
+#include<vector>
+
+using namespace std;
+
+struct Request_h
+{
+    uint8_t reques_type;      //前进使能
+    float run_speed;        //行车速度
+    float stop_distance;    //前方停止距离
+    float aeb_distance;     //前方AEB停止距离
+};
+
+struct Path_h
+{
+    vector<float> x_ref;
+    vector<float> y_ref;
+};
+
+enum ControlState : uint8_t {
+    FORWARD_ENABLE_H = 0,     //前进使能
+    BACK_ENABLE_H = 1,    //倒车使能
+    STOP_ENABLE_H = 2,    //停车使能
+    AEB_ENABLE_H = 3      //紧急刹车使能
+};
+
+
+
+
+#endif
diff --git a/control/include/VehicleStat.h b/control/include/VehicleStat.h
@@ -0,0 +1,35 @@
+#ifndef VEHICLESTAT_H
+#define VEHICLESTAT_H
+
+#include <iostream>
+#include <cstdint>
+
+struct VehicleStat_h
+{
+    float   VehicleSpeed;               //车速，单位km/h
+    int8_t GearShiftPositon;                //换挡器位置
+    uint8_t ParkSts;                        //驻车状态
+    // ErrorCode 各位为1代表含义如下：
+    //     0：不能收到can报文
+    //     1：VcuError不能进入自动驾驶状态
+    //     2：横向eps故障
+    //     3：纵向ebs故障
+    //     4：
+    //     5：
+    //     6：
+    //     7：
+    uint8_t ErrorCode;                      //故障码
+    uint16_t RemainingMile;                 //剩余里程计
+    float TolMileage;                   //累计里程计
+
+    float veh_speed;                       //速度，单位km/h
+    float SAS_SteeringAngle;               //方向盘转角速度
+    float EMS_EngineSpeed;                 //发动机转速
+    float EMS_EngineThrottlePosition;      //发动机节气门位置
+    float EMS_AccPedal;                    //加速踏板位置
+    uint8_t   EMS_BrakePedalStatus;            //制动踏板状态
+    uint8_t   TCU_GearShiftPositon;            //换挡器位置
+    uint8_t   VehicleModle;                    //车辆状态：自动驾驶or手动驾驶
+};
+
+#endif
diff --git a/control/vehicle_control/include/ControlMode.h b/control/vehicle_control/include/ControlMode.h
@@ -0,0 +1,18 @@
+#ifndef CONTROLMODE_H
+#define CONTROLMODE_H
+
+#include <iostream>
+#include <cstdint>
+
+struct ControlMode_h
+{
+    uint8_t      start;                     //qidong
+    uint8_t      mode_type;                 //模式类型  00：无， 自动模式：（11：自动生成地图，12：已有地图） 遥控模式：20
+    uint8_t      enable;                    //紧急使能  
+    uint8_t      brake_enable;              //制动使能  0：制动，1：可行驶
+    float    speed_require;             //速度要求
+    float    angle_require;             //角度要求
+};
+
+
+#endif
diff --git a/control/vehicle_control/include/Controller.h b/control/vehicle_control/include/Controller.h
@@ -0,0 +1,39 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include <iostream>
+#include <cstdint>
+
+struct TrqBreCmd_h
+{
+    uint8_t trq_enable;        //发动机驱动扭矩是否使能
+    uint32_t trq_value_2;       //->>扭矩:所需扭矩值     #2号车使用
+    float trq_value_3;         //->>扭矩:所需扭矩百分比  #3号车使用
+    uint8_t bre_enable;         //刹车是否使能
+    float bre_value;            //刹车值
+    uint8_t ACC_DecToStop;      //ACC请求减速到停止 0x0: no demand; 0x1: demand 车子按自己的减速度平滑停车 不受发送的其他指令控制
+    uint8_t ACC_Driveoff;       //释放汽缸压力，必须与ACC_DecToStop配合使
+};
+
+struct SteeringCmd_h
+{
+    float SteeringAngle;         //->>方向： 发送方向角度值
+};
+
+struct AEBCmd_h
+{
+    uint8_t AEB_enable;
+    uint8_t ACC_DecToStop;
+    float AEB_bre_value;
+};
+
+struct PRNDShiftCmd_h
+{
+    uint8_t APA_TransPRNDShiftEnable;
+    uint8_t APA_TransPRNDShiftReqValid;
+    uint8_t APA_TransPRNDShiftRequest;
+};
+
+
+
+#endif
diff --git a/control/vehicle_control/include/Localization.h b/control/vehicle_control/include/Localization.h
@@ -0,0 +1,33 @@
+#ifndef LOCALIZATION_H
+#define LOCALIZATION_H
+
+#include <iostream>
+#include <cstdint>
+
+struct NaviData_h
+{
+    float pitch;          //俯仰角
+    float roll;           //翻滚角
+    float heading;         //航向角
+
+    double longitude;      //经度
+    double latitude;      //纬度
+    double altitude;      //海拔
+
+    float speed2d;         //车辆速度
+// ------------------------------
+    float Ve;
+
+    float Vn;
+
+    float Vu;
+// --------------------------------
+    int32_t  pose_type;     //定位状态
+    double gpsTime;       
+
+    int32_t INS_Status;       //解算状态
+    float Lat_vari;         //纬度标准差
+    float Lon_vari;         //经度标准差
+};
+
+#endif
diff --git a/control/vehicle_control/include/Planning.h b/control/vehicle_control/include/Planning.h
@@ -0,0 +1,34 @@
+#ifndef PLANNING_H
+#define PLANNING_H
+
+#include <iostream>
+#include <cstdint>
+#include<vector>
+
+using namespace std;
+
+struct Request_h
+{
+    uint8_t reques_type;      //前进使能
+    float run_speed;        //行车速度
+    float stop_distance;    //前方停止距离
+    float aeb_distance;     //前方AEB停止距离
+};
+
+struct Path_h
+{
+    vector<float> x_ref;
+    vector<float> y_ref;
+};
+
+enum ControlState : uint8_t {
+    FORWARD_ENABLE_H = 0,     //前进使能
+    BACK_ENABLE_H = 1,    //倒车使能
+    STOP_ENABLE_H = 2,    //停车使能
+    AEB_ENABLE_H = 3      //紧急刹车使能
+};
+
+
+
+
+#endif
diff --git a/control/vehicle_control/include/VehicleStat.h b/control/vehicle_control/include/VehicleStat.h
@@ -0,0 +1,35 @@
+#ifndef VEHICLESTAT_H
+#define VEHICLESTAT_H
+
+#include <iostream>
+#include <cstdint>
+
+struct VehicleStat_h
+{
+    float   VehicleSpeed;               //车速，单位km/h
+    int8_t GearShiftPositon;                //换挡器位置
+    uint8_t ParkSts;                        //驻车状态
+    // ErrorCode 各位为1代表含义如下：
+    //     0：不能收到can报文
+    //     1：VcuError不能进入自动驾驶状态
+    //     2：横向eps故障
+    //     3：纵向ebs故障
+    //     4：
+    //     5：
+    //     6：
+    //     7：
+    uint8_t ErrorCode;                      //故障码
+    uint16_t RemainingMile;                 //剩余里程计
+    float TolMileage;                   //累计里程计
+
+    float veh_speed;                       //速度，单位km/h
+    float SAS_SteeringAngle;               //方向盘转角速度
+    float EMS_EngineSpeed;                 //发动机转速
+    float EMS_EngineThrottlePosition;      //发动机节气门位置
+    float EMS_AccPedal;                    //加速踏板位置
+    uint8_t   EMS_BrakePedalStatus;            //制动踏板状态
+    uint8_t   TCU_GearShiftPositon;            //换挡器位置
+    uint8_t   VehicleModle;                    //车辆状态：自动驾驶or手动驾驶
+};
+
+#endif