数据结构迭代器的实现示例

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对数据结构迭代器的实现示例做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

速度迭代器示例

1、速度空间迭代器的实现

输入速度采样空间的最大值、最小值和分辨率，得到一系列的采样空间，并一共设置、查询、初始化、复位等操作

#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;
};
#endif

2、【使用示例】使用迭代器生成速度采样空间

    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();
     }

轨迹迭代器

#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

#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(); }

---