TWaypoint.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "nav-precomp.h"  // Precomp header

#include <mrpt/nav/reactive/TWaypoint.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CDisk.h>
#include <mrpt/opengl/CArrow.h>
#include <limits>

using namespace mrpt::nav;
using namespace std;

// TWaypoint  ==========
TWaypoint::TWaypoint()
        : target(INVALID_NUM, INVALID_NUM),
          target_heading(INVALID_NUM),
          target_frame_id("map"),
          allowed_distance(INVALID_NUM),
          allow_skip(true)
{
}

TWaypoint::TWaypoint(
        double target_x, double target_y, double allowed_distance_,
        bool allow_skip_, double target_heading_)
        : target(target_x, target_y),
          target_heading(target_heading_),
          target_frame_id("map"),
          allowed_distance(allowed_distance_),
          allow_skip(allow_skip_)
{
}

bool TWaypoint::isValid() const
{
        return (target.x != INVALID_NUM) && (target.y != INVALID_NUM) &&
                   (allowed_distance != INVALID_NUM);
}

std::string TWaypoint::getAsText() const
{
        std::string s;
        if (target.x != INVALID_NUM && target.y != INVALID_NUM)
                s += mrpt::format("target=(%8.03f,%8.03f) ", target.x, target.y);
        else
                s += "target=(**Coordinates not set!!**) ";

        if (target_heading != INVALID_NUM)
                s += mrpt::format(
                        "phi=%8.03f deg ", mrpt::utils::RAD2DEG(target_heading));
        else
                s += " (heading: any) ";

        if (allowed_distance != INVALID_NUM)
                s += mrpt::format("allowed_dist=%8.03f ", allowed_distance);
        else
                s += " (**allowed_distance not set!!**) ";

        s += (allow_skip ? " allow_skip: YES" : " allow_skip: NO ");

        return s;
}

// TWaypointSequence ==========
TWaypointSequence::TWaypointSequence() {}
// Gets navigation params as a human-readable format:
std::string TWaypointSequence::getAsText() const
{
        string s;
        s += mrpt::format(
                "List of %u waypoints:\n", static_cast<unsigned int>(waypoints.size()));
        unsigned int i = 0;
        for (const auto& wp : waypoints)
        {
                s += mrpt::format(" #%3u: ", i++);
                s += wp.getAsText();
                s += "\n";
        }
        return s;
}

// TWaypointStatus ==========
TWaypointStatus::TWaypointStatus()
        : reached(false),
          skipped(false),
          timestamp_reach(INVALID_TIMESTAMP),
          counter_seen_reachable(0)
{
}
TWaypointStatus& TWaypointStatus::operator=(const TWaypoint& wp)
{
        TWaypoint::operator=(wp);
        return *this;
}
std::string TWaypointStatus::getAsText() const
{
        std::string s = TWaypoint::getAsText();
        s += mrpt::format(" reached=%s", (reached ? "YES" : "NO "));
        ;
        return s;
}

// TWaypointStatusSequence ======
TWaypointStatusSequence::TWaypointStatusSequence()
        : waypoints(),
          timestamp_nav_started(INVALID_TIMESTAMP),
          final_goal_reached(false),
          waypoint_index_current_goal(-1),
          last_robot_pose(
                  TWaypoint::INVALID_NUM, TWaypoint::INVALID_NUM,
                  TWaypoint::INVALID_NUM)
{
}

std::string TWaypointStatusSequence::getAsText() const
{
        string s;
        s += mrpt::format(
                "Status for %u waypoints:\n",
                static_cast<unsigned int>(waypoints.size()));
        unsigned int i = 0;
        for (const auto& wp : waypoints)
        {
                s += mrpt::format(" #%3u: ", i++);
                s += wp.getAsText();
                s += "\n";
        }
        s += mrpt::format(
                " final_goal_reached:%s  waypoint_index_current_goal=%d\n",
                (final_goal_reached ? "YES" : "NO "), waypoint_index_current_goal);
        return s;
}

TWaypointsRenderingParams::TWaypointsRenderingParams()
        : outter_radius(.3),
          inner_radius(.2),
          outter_radius_non_skippable(.3),
          inner_radius_non_skippable(.0),
          outter_radius_reached(.2),
          inner_radius_reached(.1),
          heading_arrow_len(1.0),
          color_regular(mrpt::utils::TColor(0x00, 0x00, 0xff)),
          color_current_goal(mrpt::utils::TColor(0xff, 0x00, 0x20)),
          color_reached(mrpt::utils::TColor(0x00, 0x00, 0xc0, 0xd0)),
          show_labels(true)
{
}

void TWaypointSequence::getAsOpenglVisualization(
        mrpt::opengl::CSetOfObjects& obj,
        const mrpt::nav::TWaypointsRenderingParams& params) const
{
        obj.clear();
        unsigned int idx = 0;
        for (const auto& p : waypoints)
        {
                auto gl_pt = mrpt::make_aligned_shared<mrpt::opengl::CDisk>(
                        p.allow_skip ? params.outter_radius
                                                 : params.outter_radius_non_skippable,
                        p.allow_skip ? params.inner_radius
                                                 : params.inner_radius_non_skippable,
                        15);
                gl_pt->setLocation(p.target.x, p.target.y, 0.01);
                gl_pt->setColor_u8(params.color_regular);
                if (params.show_labels)
                {
                        gl_pt->setName(mrpt::format("WayPt #%2u", idx));
                        gl_pt->enableShowName(true);
                }
                obj.insert(gl_pt);

                if (p.target_heading != TWaypoint::INVALID_NUM)
                {
                        auto o = mrpt::make_aligned_shared<mrpt::opengl::CArrow>(
                                0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
                        o->setPose(
                                mrpt::poses::CPose3D(
                                        p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
                        obj.insert(o);
                }
                ++idx;
        }
}

void TWaypointStatusSequence::getAsOpenglVisualization(
        mrpt::opengl::CSetOfObjects& obj,
        const mrpt::nav::TWaypointsRenderingParams& params) const
{
        obj.clear();
        {
                unsigned int idx = 0;
                for (const auto& p : waypoints)
                {
                        const bool is_cur_goal = (int(idx) == waypoint_index_current_goal);

                        mrpt::opengl::CDisk::Ptr gl_pt =
                                mrpt::make_aligned_shared<mrpt::opengl::CDisk>(
                                        p.reached
                                                ? params.outter_radius_reached
                                                : (p.allow_skip ? params.outter_radius
                                                                                : params.outter_radius_non_skippable),
                                        p.reached
                                                ? params.inner_radius_reached
                                                : (p.allow_skip ? params.inner_radius
                                                                                : params.inner_radius_non_skippable),
                                        15);
                        gl_pt->setLocation(p.target.x, p.target.y, 0.01);
                        if (params.show_labels)
                        {
                                gl_pt->setName(
                                        mrpt::format(
                                                "WayPt #%2u Reach:%s", idx, p.reached ? "YES" : "NO"));
                                gl_pt->enableShowName(true);
                        }
                        gl_pt->setColor_u8(
                                is_cur_goal ? params.color_current_goal
                                                        : (p.reached ? params.color_reached
                                                                                 : params.color_regular));
                        obj.insert(gl_pt);

                        if (p.target_heading != TWaypoint::INVALID_NUM)
                        {
                                auto o = mrpt::make_aligned_shared<mrpt::opengl::CArrow>(
                                        0, 0, 0, params.heading_arrow_len, 0.0f, 0.0f);
                                o->setPose(
                                        mrpt::poses::CPose3D(
                                                p.target.x, p.target.y, 0.02, p.target_heading, 0, 0));
                                obj.insert(o);
                        }
                        ++idx;
                }
        }
}

void TWaypointSequence::save(
        mrpt::utils::CConfigFileBase& c, const std::string& s) const
{
        const unsigned int N = waypoints.size();
        c.write(s, "waypoint_count", N);

        const int NP = 27;  // name padding

        for (unsigned int i = 0; i < N; i++)
        {
                const auto& wp = waypoints[i];

                c.write(
                        s, mrpt::format("wp%03u_allowed_distance", i), wp.allowed_distance,
                        NP);
                c.write(s, mrpt::format("wp%03u_allow_skip", i), wp.allow_skip, NP);
                c.write(s, mrpt::format("wp%03u_target_x", i), wp.target.x, NP);
                c.write(s, mrpt::format("wp%03u_target_y", i), wp.target.y, NP);
                c.write(
                        s, mrpt::format("wp%03u_target_frame_id", i), wp.target_frame_id,
                        NP);
                c.write(
                        s, mrpt::format("wp%03u_target_heading", i), wp.target_heading, NP);
        }
}

void TWaypointSequence::load(
        const mrpt::utils::CConfigFileBase& c, const std::string& s)
{
        this->clear();

        const unsigned int N = c.read_int(s, "waypoint_count", 0, true);
        waypoints.resize(N);

        for (unsigned int i = 0; i < N; i++)
        {
                auto& wp = waypoints[i];

                wp.allowed_distance = c.read_double(
                        s, mrpt::format("wp%03u_allowed_distance", i), 0, true);
                wp.allow_skip =
                        c.read_bool(s, mrpt::format("wp%03u_allow_skip", i), true, true);
                wp.target.x =
                        c.read_double(s, mrpt::format("wp%03u_target_x", i), 0, true);
                wp.target.y =
                        c.read_double(s, mrpt::format("wp%03u_target_y", i), 0, true);
                wp.target_frame_id = c.read_string(
                        s, mrpt::format("wp%03u_target_frame_id", i), "map", false);
                double hd = c.read_double(
                        s, mrpt::format("wp%03u_target_heading", i),
                        mrpt::nav::TWaypoint::INVALID_NUM);
                wp.target_heading = (hd > 100) ? mrpt::nav::TWaypoint::INVALID_NUM : hd;
        }
}