path_follow.cpp

点列で与えられた連続直線に追従する

#include <iostream>
#include <vector>
#include "Roomba_driver.h"
#include "robot_utils.h"
#include "angle_utils.h"
#include "ticks.h"
#include "delay.h"
#include "log_printf.h"

using namespace hrk;
using namespace std;


namespace
{
    void create_follow_lines(vector<PositionF>& lines,
                             const vector<PointF>& points)
    {
        if (points.size() <= 1) {
            return;
        }

        // 始点と終点があったときに、終点の位置を通る直線を定義する
        vector<PointF>::const_iterator from_it = points.begin();
        for (vector<PointF>::const_iterator to_it = from_it + 1;
             to_it != points.end(); ++to_it, ++from_it) {
            Angle angle = angle_to(*from_it, *to_it);
            lines.push_back(PositionF(to_it->x(), to_it->y(), angle));
        }
    }


    void follow(Run_driver& robot, const vector<PositionF>& lines)
    {
        const double Line_change_distance = robot.path_change_distance();
        size_t line_index = 0;
        while (true) {
            // 直線への走行を開始する
            PositionF current_line = lines[line_index];
            robot.follow_line(current_line);

            // 直線を乗り換えるタイミングまで待つ
            while (robot.distance_to_perpendicular(current_line) <
                   -Line_change_distance) {
                // 本来は、センサによる計測などの処理をここで行う
                delay_sec(0.1);
            }

            size_t next_line_index = line_index + 1;
            if (lines.size() <= next_line_index) {
                // 次に追従すべき直線がなくなったら、戻る
                return;
            }

            line_index = next_line_index;
        }
    }
}


int main(int argc, char *argv[])
{
    static_cast<void>(argc);
    static_cast<void>(argv);

    Roomba_driver robot;
    if (!robot.open("/dev/ttyUSB0")) {
        cerr << FILE_POSITION() << robot.what() << endl;
        return 1;
    }

    // 点列の定義
    vector<PointF> points;
    points.push_back(PointF(0.0, 0.0));
    points.push_back(PointF(1.0, 0.0));
    points.push_back(PointF(1.0, 1.0));
    points.push_back(PointF(2.0, 1.0));

    // 追従する直線の生成
    vector<PositionF> lines;
    create_follow_lines(lines, points);

    // 走行の開始
    follow(robot, lines);

    // 次に走行すべき直線がなくなったら停止させる
    robot.stop();
    wait_stable(robot, 0.1);

    return 0;
}