# PathSmooth

**Repository Path**: yBluePhantom/path-smooth

## Basic Information

- **Project Name**: PathSmooth
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2021-11-24
- **Last Updated**: 2021-11-24

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# readme

## 介绍
该部分含有两个结点，`average_path_smooth`和`normal_path_smooth`，分别使用了均值平滑方式和带有正态分布权重的均值平滑。



## 使用

该部分是一个`working space`，假设解压后得到的名称为`PathSmooth`，执行以下命令即可。

```bash
cd PathSmooth
catkin_make
source devel/setup.bash
```



调用这两个结点的方式：`rosrun path_smooth average_path_smooth/normal_path_smooth origin_path_file`.
`origin_path_file`可以用绝对路径，可以用相对路径(相对路径的起点是在`cmd`中执行`rosrun`的目录)。

```bash
# 假设在path_smooth路径下
rosrun path_smooth average_path_smooth origin_path.txt
rosrun path_smooth normal_path_smooth origin_path.txt
```

在这个项目的根目录下，提供了一个用于测试的原始路径文件：`origin_path.txt` 和两个结点运行结果文件：`normal_path_smooth.txt`和`average_path_smooth.txt`。
路径文件的结构要求：每行代表一个路径点，含有6个元素， position.x, position.y, orientation.x, orientation.y, orientation.z, orientation.w。



## 源文件中的测试代码(调试)

在`average_path_smooth`和`normal_path_smooth`中的`int main()`中，**可以根据需要进行适当地注释来进行测试输出**：

```c++
// 以average_path_smooth.cpp为例

// 关于原始路径的publish
ros::Publisher pub_average_origin_path;  // 发布未平滑的路径数据
pub_average_origin_path = n.advertise<nav_msgs::Path>("average_origin_path", 1);
pub_average_origin_path.publish(origin_path);


// 将平滑后的路径输出到文件中
fstream fout("average_path_smooth.txt", ios::out);
for(int i = 0; i < smooth_path.poses.size(); ++i) {
    fout << smooth_path.poses[i].pose.position.x << " " << smooth_path.poses[i].pose.position.y << " " 
         << smooth_path.poses[i].pose.orientation.x << " " << smooth_path.poses[i].pose.orientation.y << " " 
         << smooth_path.poses[i].pose.orientation.z << " " << smooth_path.poses[i].pose.orientation.w << endl;
}
fout.close();


// 方便在rosrun之后，有多余的时间，跟踪Topic以及更改颜色等属性
sleep(20);
```



## 测试结果

![](img/rviz_screenshot_2021_11_23-22_11_31.png)

![](img/rviz_screenshot_2021_11_23-22_12_06.png)

![](img/rviz_screenshot_2021_11_23-22_12_17.png)

![](img/rviz_screenshot_2021_11_23-22_12_27.png)

* 绿色：原始路径 ； 黑色：使用`normal_path_smooth`处理后的路径；  红色：使用`average_path_smooth`处理后的路径。 （感觉红色效果最好）

* 不过，目前没有进行轨迹的模拟（没有验证四元数是否正确）。