# 算法组2023年代码

**Repository Path**: ucas-sas-robot-team/algorithm-group-2023-code

## Basic Information

- **Project Name**: 算法组2023年代码
- **Description**: 算法组2022-2023年度的工作
- **Primary Language**: C++
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 2
- **Created**: 2022-11-15
- **Last Updated**: 2024-05-17

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 算法组2022-2023年度代码仓库

## 架构

![整体架构](RMDoc-1.png "整体架构")

## 部署

``` sh
cd algorithm-group-2023-code
mkdir build
cd build
sudo cmake ..
sudo make -j8
sudo ./main_sentry 
```

## 重要模块

### 一、AimTotal 自瞄集成模块

#### 1. Camera 相机驱动

通过 MindVisionSDK 开发的相机驱动

```cpp
class MVCamera {
    void OpenCamera(); //启动相机
    vector<Mat> ReadCamera(); //获取一帧图像
    //返回值为获取到的图像，每个相机占用 vector 一个元素，一个 Mat 对应一个相机的图像
}
```

#### 2. Detect 装甲板检测

采用了 YOLOv8 作为神经网络架构，ONNXRuntime 作为运行环境

```cpp
class YOLO {
    vector<Detection> detect(Mat Test); //对一张图片进行一次装甲板检测
    //参数 Mat Test 是待检测的一张图片
    //返回值为识别到的对象，每个对象占用 vector 一个元素，具体内容如下文 struct Detection
}
struct Detection //识别到的对象
{
	int classNumber; //对象类别
	string className; //对象类别对应的名字
	float confidence; //识别正确的概率
	float x1; //左边缘坐标，单位pixel
	float y1; //上边缘坐标，单位pixel
	float x2; //右边缘坐标，单位pixel
	float y2; //下边缘坐标，单位pixel
};
```

#### 3. Solver 位置解算

通过相机的部分参数，解得识别到的对象的空间位置

```cpp
class Solver {
    Point2f solve(Point2f centerPoint, Size size); //对一个对象进行一次位置解算
    //参数 Point2f centerPoint 是对象的中心点
    //参数 Size size 是对象所在的图片的尺寸信息
    //返回值对象中心点解算出的角度信息，属性 x 是 yaw，属性 y 是 pitch
    //以相机视线为x轴正方向，相机的水平向左方向为y轴正方向，相机的垂直向上方向为z轴正方向，建立空间直角坐标系
    //设对象中心点在 xOy 平面上的投影点为 P，则射线 OP 的角度为 yaw，取值范围 (-π, π)
    //设对象中心点在 xOz 平面上的投影点为 Q，则射线 OQ 的角度为 pitch，取值范围 (-π/2, π/2)
}
```

#### 4. Predict 运动预测

通过已有的采样点预测目标的运动趋势

```cpp
class RawPredict {
public:
    Point2f calTargetSend(Point2f targetChosen);
    //参数 Point2f targetChosen 是对象当前角度信息，属性 x 是 yaw，属性 y 是 pitch
    //返回值对象中心点解算出的角度信息，属性 x 是 yaw，属性 y 是 pitch
};
```

### 二、Decision 决策模块

维持一个状态机，根据自瞄结果等输入即时切换机器人状态

```cpp
namespace decision {
/**
 * @brief Behavior state
 */
enum class BehaviorState {
  RUNNING,   ///< Running state in process
  SUCCESS,   ///< Success state as result
  FAILURE,   ///< Failure state as result
  IDLE,      ///< Idle state, state as default or after cancellation
  P1,
  BEGIN,
  PATROL,
  CHASE,
  SEARCH,
  SHOOT,
  SIDEENEMY,
};

enum BehaviorStateEnum{
     INIT = -1,
     BACKBOOT = 0,
     CHASE=1,
     SEARCH=2,
     ESCAPE=3,
     PATROL=4,
     RELOAD=5,
     SHIELD=6,
     AMBUSH=7,
     ATTACK=8

};

}
```

### 三、Mapping 导航模块

根据机器人状态确定机器人行为，拆分机器人动作

```cpp
class Mapping : private Map{
private:
	MAPPING::velocity direction; ///<移动方向
	double speed;///<移动速度
	MAPPING::pos curPos;///<当前位置
	vector<MAPPING::pos> moveHistory;///<历史位置
	vector<MAPPING::pos> path;///<路径规划
	double sampl_interval;//采样间隔
public:
	Mapping();
	~Mapping();
	uint32_t computeVelocityCommands();//Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.
	double feasibility_check_lookahead_distance();//Specify up to which distance(and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if - 1, all poses up to feasibility_check_no_poses are checked.
	void detectObstacles();//每次采样检测是否出现障碍物
	void refreshMap();//每次采样更新地图
	vector<MAPPING::pos> moveLine();//根据当前位置和下一位置生成移动路线序列
	void refresh(MAPPING::velocity cur_direction, double cur_speed, MAPPING::pos cur_pos);//更新当前状态
	
	//friend void reaction(MAPPING::pos curPos);//遇到特殊地带的反应，调用reactFor...函数，定义为友元函数，便于共享数据
};
```

### 四、Serial 串口模块

直接控制串口通信，包括电控单片机和激光雷达

```cpp
typedef unsigned int uint32_t;
typedef float fp32;
typedef struct 
{
    uint32_t start;	// 开始标志符号，'S'
    fp32 pitch;	// 目标delta_pitch角度
    fp32 yaw;		// 目标delta_yaw角度
    fp32 x;		// 云台坐标系下的x速度
    fp32 y;		// 云台坐标系下的y速度
    fp32 w;		// 云台坐标系下的角速度w
    uint32_t shoot;	// 确认发射
    uint32_t found_armor;	// 找到了装甲板
    uint32_t checksum;   //校验和
    uint32_t reserved[6];	// 保留字段
    uint32_t end;	// 结尾标志符号，'E'
} toSTM32_t;

enum my_color_enum {
    COLOR_RED   = 0,
    COLOR_BLUE  = 1,
};

typedef struct
{
    uint32_t start;                 // 开始符号，'S'
    uint32_t blood : 12;            // 剩余血量，不会高于4096
    uint32_t bullet_remaining : 12; // 剩余子弹，不会高于4096
    uint32_t my_color : 8;          // 本方颜色，32bit对齐
    uint32_t checksum;              // 校验和，前方所有数据按32bit分段直接相加
    uint32_t end;                   // 结束符号，'E'
}toNUC_t;

class Serial {
private:
    int fd;
    int nSpeed;
    char nEvent;
    int nBits;
    int nStop;
	std::string portname = "#";

    int set_opt(int fd, int nSpeed, char nEvent, int nBits, int nStop);

public:
    bool OpenSerial(int nSpeed, char nEvent = 'N', int nBits = 8, int nStop = 1);
	Serial();
	Serial(std::string);
    ~Serial();

    bool InitPort(int nSpeed, char  nEvent = 'N', int nBits = 8, int nStop = 1);
//    int GetBytesInCOM() const ;
    bool WriteData(const unsigned char* pData, unsigned int length);
    // return bytes read, -1 for offline, 0 for EOF, >1 for valid bytes
    int ReadData(unsigned char* buffer, unsigned int length);
	std::string get_uart_dev_name();

	bool send_control(
		fp32 pitch,	// pitch轴目标偏移角度，弧度制，向左偏为正
		fp32 yaw,	// yaw轴目标偏移角度，弧度制，向上偏为正
		fp32 x,		// x方向速度（前进），单位m/s
		fp32 y,		// y方向速度（左移），单位m/s
		fp32 w,		// 小陀螺自转角速度 ，单位rad/s
		uint32_t shoot,	// 为1时表示立刻发射，否则设置为0
		uint32_t found_armor	// 为1时表示在当前画面中找到了需要瞄准的装甲板，否则设置为0
	);
};
```