系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！
本文先对数据结构迭代器的实现示例做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
速度迭代器示例
1、速度空间迭代器的实现
输入速度采样空间的最大值、最小值和分辨率，得到一系列的采样空间，并一共设置、查询、初始化、复位等操作
cpp
#ifndef DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#define DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#include <algorithm>
#include <cmath>
#include <vector>
/**
* We use the class to get even sized samples between min and max, including
* zero if it is not included (and range goes from negative to positive
*/
class VelocityIterator {
public:
VelocityIterator(double min, double max, int num_samples)
: current_index(0) {
if (min == max) {
samples_.push_back(min);
} else {
// 如果速度边界不同，至少有两个采样维度
num_samples = std::max(2, num_samples);
// 速度采样的单位角度e.g. for 4 samples, split distance in 3 even
// parts
double step_size =
(max - min) / double(std::max(1, (num_samples - 1)));
// 我们确保避免最小值和最大值之间的舍入误差。
double current;
double next = min;
for (int j = 0; j < num_samples - 1; ++j) {
current = next;
next += step_size;
samples_.push_back(current);
// if 0 is among samples, this is never true. Else it inserts a
// 0 between the positive and negative samples
if ((current < 0) && (next > 0)) {
samples_.push_back(0.0);
}
}
samples_.push_back(max);
}
}
double getVelocity() { return samples_.at(current_index); }
VelocityIterator& operator++(int) {
current_index++;
return *this;
}
void reset() { current_index = 0; }
bool isFinished() { return current_index >= samples_.size(); }
private:
std::vector<double> samples_; // 储存各采样角度序列
unsigned int current_index;
};
#endif2、【使用示例】使用迭代器生成速度采样空间
cpp
std::vector<Eigen::Vector3f> sample_params_;
VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
// 把分开三个维度采样的整合到一个整个速度采样空间sample_params中
for (; !x_it.isFinished(); x_it++) {
vel_samp[0] = x_it.getVelocity();
for (; !y_it.isFinished(); y_it++) {
vel_samp[1] = y_it.getVelocity();
for (; !th_it.isFinished(); th_it++) {
vel_samp[2] = th_it.getVelocity();
// MCU_LOG("Sample %f, %f, %f", vel_samp[0], vel_samp[1],
// vel_samp[2]);
sample_params_.push_back(vel_samp);
}
th_it.reset();
}
y_it.reset();
}轨迹迭代器
cpp
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#include <vector>
/**
* @class Trajectory
* @brief x速度、y速度和角速度生成的轨迹
* 轨迹既有位姿信息，也有速度信息，还有代价信息
*/
class Trajectory {
public:
Trajectory();
Trajectory(double xv, double yv, double thetav, double time_delta,
unsigned int num_pts);
double xv_, yv_, thetav_; // 该pose轨迹的速度信息
double cost_; // The cost/score of the trajectory
double time_delta_; // The time gap between points
/**
* @brief Get a point within the trajectory
* @param index The index of the point to get
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getPoint(unsigned int index, double& x, double& y, double& th) const;
/**
* @brief Set a point within the trajectory
* @param index The index of the point to set
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void setPoint(unsigned int index, double x, double y, double th);
/**
* @brief Add a point to the end of a trajectory
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void addPoint(double x, double y, double th);
/**
* @brief Get the last point of the trajectory
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getEndpoint(double& x, double& y, double& th) const;
/**
* @brief Clear the trajectory's points
*/
void resetPoints();
/**
* @brief Return the number of points in the trajectory
* @return The number of points in the trajectory
*/
unsigned int getPointsSize() const;
private:
std::vector<double> x_pts_; ///< @brief The x points in the
///< trajectory，该轨迹中所有点的x坐标
std::vector<double> y_pts_; ///< @brief The y points in the
///< trajectory,该轨迹中所有点的y坐标
std::vector<double> th_pts_; ///< @brief The theta points in the
///< trajectory,该轨迹中所有点的朝向th
};
#endif
cpp
#include <trajectory.h>
Trajectory::Trajectory() : xv_(0.0), yv_(0.0), thetav_(0.0), cost_(-1.0) {}
Trajectory::Trajectory(double xv, double yv, double thetav, double time_delta,
unsigned int num_pts)
: xv_(xv),
yv_(yv),
thetav_(thetav),
cost_(-1.0),
time_delta_(time_delta),
x_pts_(num_pts),
y_pts_(num_pts),
th_pts_(num_pts) {}
void Trajectory::getPoint(unsigned int index, double& x, double& y,
double& th) const {
x = x_pts_[index];
y = y_pts_[index];
th = th_pts_[index];
}
void Trajectory::setPoint(unsigned int index, double x, double y, double th) {
x_pts_[index] = x;
y_pts_[index] = y;
th_pts_[index] = th;
}
void Trajectory::addPoint(double x, double y, double th) {
x_pts_.push_back(x);
y_pts_.push_back(y);
th_pts_.push_back(th);
}
void Trajectory::resetPoints() {
x_pts_.clear();
y_pts_.clear();
th_pts_.clear();
}
void Trajectory::getEndpoint(double& x, double& y, double& th) const {
x = x_pts_.back();
y = y_pts_.back();
th = th_pts_.back();
}
unsigned int Trajectory::getPointsSize() const { return x_pts_.size(); }