<commit_before>#include "libride/document.commands.h"

#include <iostream>

#include "base/string.h"
#include "base/utf8.h"
#include "base/c.h"

#include "libride/document.h"
#include "libride/command.h"

namespace
{
    bool is_non_word(char c, const std::string& non_word_chars)
    {
        return non_word_chars.find(c) == std::string::npos;
    }
}

position previous_char(Document* doc, const position& pp);
position next_char(Document* doc, const position& pp);
position previous_word_start(Document* doc, const position& pp, const std::string& non_word_chars);
position next_word_end(Document* doc, const position& pp, const std::string& non_word_chars);
position start_of_word(Document* doc, const position& pp, const std::string& non_word_chars);
position end_of_word(Document* doc, const position& pp, const std::string& non_word_chars);
position previous_block_start(Document* doc, const position& pp);
position next_block_end(Document* doc, const position& pp);
position start_of_line(const position& p);
position end_of_line(const position& p);
position start_of_doc();
position end_of_doc(Document* doc);

position previous_char(Document* doc, const position& pp)
{
    auto p = pp;
    do
    {
        p = doc->position_offset(p, -1);
    }
    while(is_utf8_cont_char(doc->get_char(p)));
    return p;
}

position next_char(Document* doc, const position& pp)
{
    auto p = pp;
    do
    {
        p = doc->position_offset(p, 1);
    } while(is_utf8_cont_char(doc->get_char(p)));
    return p;
}

position previous_word_start_old(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    char prev = 0;
    while(p.line > 0 or p.offset > 0)
    {
        const auto np = doc->position_offset(p, -1);
        const auto cchar = doc->get_char(np);
        if( (prev && prev != cchar) || !is_non_word(cchar, non_word_chars))
        {
            break;
        }
        prev = cchar;
        p = np;
    }
    return start_of_word(doc, p, non_word_chars);
}

position previous_word_start(Document* doc, const position& pp, const std::function<bool (char)>& is_word_char)
{
    auto p = pp;

    while(p.line > 0 && p.offset>0 && !is_word_char(doc->get_char(doc->position_offset(p, -1))) )
    {
        p = doc->position_offset(p, -1);
    }

    // skip line if it isn't whitespace
    auto last_p = p;
    while(p.line > 0 && p.offset>0 && is_word_char(doc->get_char(p)) )
    {
        last_p = p;
        p = doc->position_offset(p, -1);
    }

    return last_p;
}

position next_word_end(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    char prev = 0;
    const auto end = end_of_doc(doc);
    while(p.line < end.line || p.offset < end.offset)
    {
        const auto cchar = doc->get_char(p);
        if( (prev && prev) != cchar || !is_non_word(cchar, non_word_chars))
        {
            break;
        }
        p = doc->position_offset(p, 1);
        prev = cchar;
    }
    return end_of_word(doc, p, non_word_chars);
}

position start_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    while(true)
    {
        const auto nep = doc->position_offset(p, -1);
        const auto cchar = doc->get_char(nep);
        if( is_non_word(cchar, non_word_chars) || p == nep)
        {
            break;
        }
        p = nep;
    }
    return p;
}

position end_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    while(true)
    {
        const auto nep = doc->position_offset(p, 1);
        const auto cchar = doc->get_char(p);
        if( is_non_word(cchar, non_word_chars) || p == nep)
        {
            break;
        }
        p = nep;
    }
    return p;
}

position previous_block_start(Document* doc, const position& pp)
{
    auto p = pp;

    const auto line_at = [doc](int line) -> std::string { return doc->lines[Cs(line)]; };

    // skip line if it is whitespace
    while(p.line > 0 && is_only_whitepace(line_at(p.line)) )
    {
        p.line -= 1;
    }

    // skip line if it isn't whitespace
    while(p.line > 0 && !is_only_whitepace(line_at(p.line)) )
    {
        p.line -= 1;
    }

    p.offset = index_of_non_whitespace(line_at(p.line));

    return p;
}

position next_block_end(Document* doc, const position& pp)
{
    auto p = pp;

    const auto lc = C(doc->lines.size());
    const auto line_at = [doc](int line) -> std::string { return doc->lines[Cs(line)]; };

    // skip line if it is whitespace
    while(p.line < lc && is_only_whitepace(line_at(p.line)) )
    {
        p.line += 1;
    }

    // skip line if it isn't whitespace
    while(p.line < lc && !is_only_whitepace(line_at(p.line)) )
    {
        p.line += 1;
    }

    p.offset = index_of_non_whitespace(line_at(p.line));

    return p;
}

position start_of_line(const position& p)
{
    return {p.line, 0};
}

position end_of_line(const position& p)
{
    return {p.line, position::max_offset};
}

position start_of_doc()
{
    return {0, 0};
}

position end_of_doc(Document* doc)
{
    return {static_cast<int>(doc->lines.size()), position::max_offset};
}

///////////////////////////////////////////////////////////////////////////////////////////////////

bool is_whitespace(char c)
{
    switch(c)
    {
    case ' ':
    case '\t':
    case '\n':
        return true;
    default: return false;
    }
}

void add_edit_commands(CommandList* list, active_document_or_null_getter get_doc)
{
    auto add_edit_command = list->add
    (
        [get_doc]() -> bool
        {
            return get_doc() != nullptr;
        }
    );

    auto add_complex_command = [get_doc, &add_edit_command]
    (
        const std::string& base,
        std::function<position (const selection&)> get_position,
        std::function<position (Document*, const position&)> modify_position
    )
    {
        add_edit_command
        (
            "doc.move-" + base,
            [get_doc, get_position, modify_position]()
            {
                auto* doc = get_doc();
                for(auto& sel: doc->cursors)
                {
                    if(sel.is_selection())
                    {
                        const auto p = get_position(sel);
                        sel = {p, p};
                    }
                    else
                    {
                        const auto np = modify_position(doc, sel.b);
                        sel = {np, np};
                    }
                }
                doc->merge_all_cursors();
            }
        );
        add_edit_command
        (
            "doc.select-" + base,
            [get_doc, get_position, modify_position]()
            {
                auto* doc = get_doc();
                for(auto& sel: doc->cursors)
                {
                    sel.b = modify_position(doc, sel.b);
                }
                doc->merge_all_cursors();
            }
        );
    };

    auto get_right_selection = [](const selection& s) -> position
    {
        return s.sorted().b;
    };

    auto get_left_selection = [](const selection& s) -> position
    {
        return s.sorted().a;
    };

    add_complex_command("right-char", get_right_selection, [](Document* doc, const position& p) -> position{
        return next_char(doc, p);
    });
    add_complex_command("left-char", get_left_selection, [](Document* doc, const position& p) -> position{
        return previous_char(doc, p);
    });
    add_complex_command("prev-block", get_left_selection, [](Document* doc, const position& p) -> position{
        return previous_block_start(doc, p);
    });
    add_complex_command("next-block", get_right_selection, [](Document* doc, const position& p) -> position{
        return next_block_end(doc, p);
    });

    // todo(Gustav): handle both vim word and WORD movements
    /*
        A WORD is always delimited by whitespace.
        A word is delimited by non-keyword characters, which are configurable.
        Whitespace characters aren't keywords, and usually other characters (like ()[],-) aren't, neither.
        Therefore, a word usually is smaller than a WORD; the word-navigation is more fine-grained.

        This "stuff" is not-so difficult!
        wwww  wwwww  ww www ww wwwwwwwww    " (key)words, delimiters are non-keywords: "-! and whitespace
        WWWW WWWWWWW WW WWWWWW WWWWWWWWWW   " WORDS, delimiters are whitespace only

        https://stackoverflow.com/a/22931259
    */

   constexpr const char* const non_word_characters = " \t\n/\\()\"':,.;<>~!@#$%^&*|+=[]{}`?-";
   const auto is_word_char = [](char c) -> bool { return is_whitespace(c)==false; };

    add_complex_command("right-word", get_right_selection, [](Document* doc, const position& p) -> position{
        return next_word_end(doc, p, non_word_characters);
    });
    add_complex_command("left-word", get_left_selection, [is_word_char](Document* doc, const position& p) -> position{
        return previous_word_start(doc, p, is_word_char);
    });
    add_complex_command("word-start", get_right_selection, [](Document* doc, const position& p) -> position{
        return start_of_word(doc, p, non_word_characters);
    });
    add_complex_command("word-end", get_left_selection, [](Document* doc, const position& p) -> position{
        return end_of_word(doc, p, non_word_characters);
    });

    // todo(Gustav): handle whitespace, home to start-of-line or home to first non-whitespace
    add_complex_command("home", get_left_selection, [](Document*, const position& p) -> position{
        return start_of_line(p);
    });
    add_complex_command("end", get_right_selection, [](Document*, const position& p) -> position{
        return end_of_line(p);
    });

    add_complex_command("doc-start", get_left_selection, [](Document*, const position&) -> position{
        return start_of_doc();
    });
    add_complex_command("doc-end", get_right_selection, [](Document* doc, const position&) -> position{
        return end_of_doc(doc);
    });

    // add_complex_command("previous-line", foo, docview.previous_line)
    // add_complex_command("next-line", foo, docview.next_line)
    // add_complex_command("previous-page", foo, docview.previous_page)
    // add_complex_command("next-page", foo, docview.next_page)

    // position previous_word_start(Document* doc, const position& pp, const std::string& non_word_chars)
    // position next_word_end(Document* doc, const position& pp, const std::string& non_word_chars)
    // position start_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
    // position end_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
    // position previous_block_start(Document* doc, const position& pp)
    // position next_block_end(Document* doc, const position& pp)

}
<commit_msg>fix: hopefully fixes compilation issues on CI<commit_after>#include "libride/document.commands.h"

#include <iostream>

#include "base/string.h"
#include "base/utf8.h"
#include "base/c.h"

#include "libride/document.h"
#include "libride/command.h"

namespace
{
    bool is_non_word(char c, const std::string& non_word_chars)
    {
        return non_word_chars.find(c) == std::string::npos;
    }
}

position previous_char(Document* doc, const position& pp);
position next_char(Document* doc, const position& pp);
position previous_word_start(Document* doc, const position& pp, const std::string& non_word_chars);
position next_word_end(Document* doc, const position& pp, const std::string& non_word_chars);
position start_of_word(Document* doc, const position& pp, const std::string& non_word_chars);
position end_of_word(Document* doc, const position& pp, const std::string& non_word_chars);
position previous_block_start(Document* doc, const position& pp);
position next_block_end(Document* doc, const position& pp);
position start_of_line(const position& p);
position end_of_line(const position& p);
position start_of_doc();
position end_of_doc(Document* doc);

position previous_char(Document* doc, const position& pp)
{
    auto p = pp;
    do
    {
        p = doc->position_offset(p, -1);
    }
    while(is_utf8_cont_char(doc->get_char(p)));
    return p;
}

position next_char(Document* doc, const position& pp)
{
    auto p = pp;
    do
    {
        p = doc->position_offset(p, 1);
    } while(is_utf8_cont_char(doc->get_char(p)));
    return p;
}

position previous_word_start_old(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    char prev = 0;
    while(p.line > 0 || p.offset > 0)
    {
        const auto np = doc->position_offset(p, -1);
        const auto cchar = doc->get_char(np);
        if( (prev && prev != cchar) || !is_non_word(cchar, non_word_chars))
        {
            break;
        }
        prev = cchar;
        p = np;
    }
    return start_of_word(doc, p, non_word_chars);
}

position previous_word_start(Document* doc, const position& pp, const std::function<bool (char)>& is_word_char)
{
    auto p = pp;

    while(p.line > 0 && p.offset>0 && !is_word_char(doc->get_char(doc->position_offset(p, -1))) )
    {
        p = doc->position_offset(p, -1);
    }

    // skip line if it isn't whitespace
    auto last_p = p;
    while(p.line > 0 && p.offset>0 && is_word_char(doc->get_char(p)) )
    {
        last_p = p;
        p = doc->position_offset(p, -1);
    }

    return last_p;
}

position next_word_end(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    char prev = 0;
    const auto end = end_of_doc(doc);
    while(p.line < end.line || p.offset < end.offset)
    {
        const auto cchar = doc->get_char(p);
        if( (prev && prev != cchar) || !is_non_word(cchar, non_word_chars))
        {
            break;
        }
        p = doc->position_offset(p, 1);
        prev = cchar;
    }
    return end_of_word(doc, p, non_word_chars);
}

position start_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    while(true)
    {
        const auto nep = doc->position_offset(p, -1);
        const auto cchar = doc->get_char(nep);
        if( is_non_word(cchar, non_word_chars) || p == nep)
        {
            break;
        }
        p = nep;
    }
    return p;
}

position end_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
{
    auto p = pp;
    while(true)
    {
        const auto nep = doc->position_offset(p, 1);
        const auto cchar = doc->get_char(p);
        if( is_non_word(cchar, non_word_chars) || p == nep)
        {
            break;
        }
        p = nep;
    }
    return p;
}

position previous_block_start(Document* doc, const position& pp)
{
    auto p = pp;

    const auto line_at = [doc](int line) -> std::string { return doc->lines[Cs(line)]; };

    // skip line if it is whitespace
    while(p.line > 0 && is_only_whitepace(line_at(p.line)) )
    {
        p.line -= 1;
    }

    // skip line if it isn't whitespace
    while(p.line > 0 && !is_only_whitepace(line_at(p.line)) )
    {
        p.line -= 1;
    }

    p.offset = index_of_non_whitespace(line_at(p.line));

    return p;
}

position next_block_end(Document* doc, const position& pp)
{
    auto p = pp;

    const auto lc = C(doc->lines.size());
    const auto line_at = [doc](int line) -> std::string { return doc->lines[Cs(line)]; };

    // skip line if it is whitespace
    while(p.line < lc && is_only_whitepace(line_at(p.line)) )
    {
        p.line += 1;
    }

    // skip line if it isn't whitespace
    while(p.line < lc && !is_only_whitepace(line_at(p.line)) )
    {
        p.line += 1;
    }

    p.offset = index_of_non_whitespace(line_at(p.line));

    return p;
}

position start_of_line(const position& p)
{
    return {p.line, 0};
}

position end_of_line(const position& p)
{
    return {p.line, position::max_offset};
}

position start_of_doc()
{
    return {0, 0};
}

position end_of_doc(Document* doc)
{
    return {static_cast<int>(doc->lines.size()), position::max_offset};
}

///////////////////////////////////////////////////////////////////////////////////////////////////

bool is_whitespace(char c)
{
    switch(c)
    {
    case ' ':
    case '\t':
    case '\n':
        return true;
    default: return false;
    }
}

void add_edit_commands(CommandList* list, active_document_or_null_getter get_doc)
{
    auto add_edit_command = list->add
    (
        [get_doc]() -> bool
        {
            return get_doc() != nullptr;
        }
    );

    auto add_complex_command = [get_doc, &add_edit_command]
    (
        const std::string& base,
        std::function<position (const selection&)> get_position,
        std::function<position (Document*, const position&)> modify_position
    )
    {
        add_edit_command
        (
            "doc.move-" + base,
            [get_doc, get_position, modify_position]()
            {
                auto* doc = get_doc();
                for(auto& sel: doc->cursors)
                {
                    if(sel.is_selection())
                    {
                        const auto p = get_position(sel);
                        sel = {p, p};
                    }
                    else
                    {
                        const auto np = modify_position(doc, sel.b);
                        sel = {np, np};
                    }
                }
                doc->merge_all_cursors();
            }
        );
        add_edit_command
        (
            "doc.select-" + base,
            [get_doc, get_position, modify_position]()
            {
                auto* doc = get_doc();
                for(auto& sel: doc->cursors)
                {
                    sel.b = modify_position(doc, sel.b);
                }
                doc->merge_all_cursors();
            }
        );
    };

    auto get_right_selection = [](const selection& s) -> position
    {
        return s.sorted().b;
    };

    auto get_left_selection = [](const selection& s) -> position
    {
        return s.sorted().a;
    };

    add_complex_command("right-char", get_right_selection, [](Document* doc, const position& p) -> position{
        return next_char(doc, p);
    });
    add_complex_command("left-char", get_left_selection, [](Document* doc, const position& p) -> position{
        return previous_char(doc, p);
    });
    add_complex_command("prev-block", get_left_selection, [](Document* doc, const position& p) -> position{
        return previous_block_start(doc, p);
    });
    add_complex_command("next-block", get_right_selection, [](Document* doc, const position& p) -> position{
        return next_block_end(doc, p);
    });

    // todo(Gustav): handle both vim word and WORD movements
    /*
        A WORD is always delimited by whitespace.
        A word is delimited by non-keyword characters, which are configurable.
        Whitespace characters aren't keywords, and usually other characters (like ()[],-) aren't, neither.
        Therefore, a word usually is smaller than a WORD; the word-navigation is more fine-grained.

        This "stuff" is not-so difficult!
        wwww  wwwww  ww www ww wwwwwwwww    " (key)words, delimiters are non-keywords: "-! and whitespace
        WWWW WWWWWWW WW WWWWWW WWWWWWWWWW   " WORDS, delimiters are whitespace only

        https://stackoverflow.com/a/22931259
    */

   constexpr const char* const non_word_characters = " \t\n/\\()\"':,.;<>~!@#$%^&*|+=[]{}`?-";
   const auto is_word_char = [](char c) -> bool { return is_whitespace(c)==false; };

    add_complex_command("right-word", get_right_selection, [](Document* doc, const position& p) -> position{
        return next_word_end(doc, p, non_word_characters);
    });
    add_complex_command("left-word", get_left_selection, [is_word_char](Document* doc, const position& p) -> position{
        return previous_word_start(doc, p, is_word_char);
    });
    add_complex_command("word-start", get_right_selection, [](Document* doc, const position& p) -> position{
        return start_of_word(doc, p, non_word_characters);
    });
    add_complex_command("word-end", get_left_selection, [](Document* doc, const position& p) -> position{
        return end_of_word(doc, p, non_word_characters);
    });

    // todo(Gustav): handle whitespace, home to start-of-line or home to first non-whitespace
    add_complex_command("home", get_left_selection, [](Document*, const position& p) -> position{
        return start_of_line(p);
    });
    add_complex_command("end", get_right_selection, [](Document*, const position& p) -> position{
        return end_of_line(p);
    });

    add_complex_command("doc-start", get_left_selection, [](Document*, const position&) -> position{
        return start_of_doc();
    });
    add_complex_command("doc-end", get_right_selection, [](Document* doc, const position&) -> position{
        return end_of_doc(doc);
    });

    // add_complex_command("previous-line", foo, docview.previous_line)
    // add_complex_command("next-line", foo, docview.next_line)
    // add_complex_command("previous-page", foo, docview.previous_page)
    // add_complex_command("next-page", foo, docview.next_page)

    // position previous_word_start(Document* doc, const position& pp, const std::string& non_word_chars)
    // position next_word_end(Document* doc, const position& pp, const std::string& non_word_chars)
    // position start_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
    // position end_of_word(Document* doc, const position& pp, const std::string& non_word_chars)
    // position previous_block_start(Document* doc, const position& pp)
    // position next_block_end(Document* doc, const position& pp)

}
<|endoftext|>


<commit_before>/* +---------------------------------------------------------------------------+
   |                     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/CWaypointsNavigator.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/math/wrap2pi.h>

using namespace mrpt::nav;
using namespace std;

CWaypointsNavigator::CWaypointsNavigator(CRobot2NavInterface &robot_if) :
	CAbstractNavigator(robot_if),
	m_was_aligning(false)
{
}

CWaypointsNavigator::~CWaypointsNavigator()
{
}

void CWaypointsNavigator::navigateWaypoints( const TWaypointSequence & nav_request )
{
	MRPT_START

	mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

	m_was_aligning = false;
	m_waypoint_nav_status = TWaypointStatusSequence();
	m_waypoint_nav_status.timestamp_nav_started = mrpt::system::now();

	const size_t N = nav_request.waypoints.size();
	ASSERTMSG_(N>0,"List of waypoints is empty!");

	m_waypoint_nav_status.waypoints.resize(N);
	// Copy waypoints fields data, leave status fields to defaults:
	for (size_t i=0;i<N;i++)
	{
		ASSERT_( nav_request.waypoints[i].isValid() );
		m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
	}

	m_waypoint_nav_status.waypoint_index_current_goal = -1;  // Not started yet.

	// The main loop navigationStep() will iterate over waypoints and send them to navigate()
	MRPT_END
}

void CWaypointsNavigator::getWaypointNavStatus(TWaypointStatusSequence & out_nav_status) const
{
	// No need to lock mutex...
	out_nav_status = m_waypoint_nav_status;
}

void CWaypointsNavigator::cancel()
{
	{
		mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);
		m_waypoint_nav_status = TWaypointStatusSequence();
	}
	CAbstractNavigator::cancel();
}

void CWaypointsNavigator::navigationStep()
{
	MRPT_START

	using mrpt::utils::square;

	// --------------------------------------
	//     Waypoint navigation algorithm
	// --------------------------------------
	bool is_aligning = false;  // the robot is aligning into a waypoint with a desired heading

	{
	mrpt::utils::CTimeLoggerEntry tle(m_timlog_delays,"CWaypointsNavigator::navigationStep()");
	mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

	TWaypointStatusSequence &wps = m_waypoint_nav_status; // shortcut to save typing

	if (wps.waypoints.empty() || wps.final_goal_reached)
	{
		// No nav request is pending or it was canceled
	}
	else
	{
		// 0) Get current robot pose:
		CAbstractNavigator::updateCurrentPoseAndSpeeds();

		// 1) default policy: go thru WPs one by one
		const int prev_wp_index = wps.waypoint_index_current_goal;

		mrpt::math::TSegment2D robot_move_seg;
		robot_move_seg.point1.x = m_curPoseVel.pose.x;
		robot_move_seg.point1.y = m_curPoseVel.pose.y;
		if (wps.last_robot_pose.x==TWaypoint::INVALID_NUM)
		{
			robot_move_seg.point2 = robot_move_seg.point1;
		}
		else
		{
			robot_move_seg.point2.x = wps.last_robot_pose.x;
			robot_move_seg.point2.y = wps.last_robot_pose.y;
		}
		wps.last_robot_pose = m_curPoseVel.pose; // save for next iters

		if (wps.waypoint_index_current_goal >= 0)
		{
			auto &wp = wps.waypoints[wps.waypoint_index_current_goal];
			const double dist2target = robot_move_seg.distance(wp.target);
			if (dist2target < wp.allowed_distance)
			{
				bool consider_wp_reached = false;
				if (wp.target_heading == TWaypoint::INVALID_NUM)
				{
					consider_wp_reached = true;
				}
				else
				{
					// Handle pure-rotation robot interface to honor target_heading
					const double ang_err = mrpt::math::angDistance(m_curPoseVel.pose.phi, wp.target_heading);
					if (std::abs(ang_err) <= params_waypoints_navigator.waypoint_angle_tolerance)
					{
						consider_wp_reached = true;
					}
					else
					{
						is_aligning = true;

						if (!m_was_aligning)
						{
							// 1st time we are aligning:
							// Send vel_cmd to the robot:
							mrpt::kinematics::CVehicleVelCmdPtr align_cmd = m_robot.getAlignCmd(ang_err);

							MRPT_LOG_INFO_FMT(
								"[CWaypointsNavigator::navigationStep] Trying to align to heading: %.02f deg. "
								"Relative heading: %.02f deg. "
								"With motion cmd: %s",
								mrpt::utils::RAD2DEG(wp.target_heading),
								mrpt::utils::RAD2DEG(ang_err),
								align_cmd ? align_cmd->asString().c_str() : "nullptr (operation not supported by this robot)");

							this->stop(false /*not emergency*/); // In any case, do a "stop"
							if (align_cmd) {
								this->changeSpeeds(*align_cmd);
							} else {
								consider_wp_reached = true; // this robot does not support "in place" alignment
							}
						}
						else
						{
							MRPT_LOG_THROTTLE_INFO_FMT(0.5, "[CWaypointsNavigator::navigationStep] Waiting for the robot to get aligned: current_heading=%.02f deg target_heading=%.02f deg", mrpt::utils::RAD2DEG(m_curPoseVel.pose.phi), mrpt::utils::RAD2DEG(wp.target_heading) );
						}
					}
				}

				if (consider_wp_reached)
				{
					MRPT_LOG_DEBUG_STREAM("[CWaypointsNavigator::navigationStep] Waypoint " <<
						(wps.waypoint_index_current_goal + 1) << "/" << wps.waypoints.size() << " reached."
						" segment-to-target dist: " << dist2target << ", allowed_dist: " << wp.allowed_distance
					);

					wp.reached = true;
					wp.skipped = false;
					wp.timestamp_reach = mrpt::system::now();
					m_robot.sendWaypointReachedEvent(wps.waypoint_index_current_goal, true /* reason: really reached*/);

					// Was this the final goal??
					if (wps.waypoint_index_current_goal < int(wps.waypoints.size() - 1)) {
						wps.waypoint_index_current_goal++;
					}
					else {
						wps.final_goal_reached = true;
					}
				}
			}
		}

		// 2) More advanced policy: if available, use children class methods to decide
		//     which is the best candidate for the next waypoint, if we can skip current one:
		if (!wps.final_goal_reached &&
			wps.waypoint_index_current_goal >= 0 &&
			wps.waypoints[wps.waypoint_index_current_goal].allow_skip
			)
		{
			const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
			int most_advanced_wp = wps.waypoint_index_current_goal;
			const int most_advanced_wp_at_begin = most_advanced_wp;

			for (int idx=wps.waypoint_index_current_goal;idx<(int)wps.waypoints.size();idx++)
			{
				if (idx<0) continue;
				if (wps.waypoints[idx].reached) continue;

				// Is it reachable?
				mrpt::math::TPoint2D wp_local_wrt_robot;
				robot_pose.inverseComposePoint(wps.waypoints[idx].target, wp_local_wrt_robot);

				if (params_waypoints_navigator.max_distance_to_allow_skip_waypoint>0 && wp_local_wrt_robot.norm()>params_waypoints_navigator.max_distance_to_allow_skip_waypoint)
					continue; // Skip this one, it is too far away

				const bool is_reachable = this->impl_waypoint_is_reachable(wp_local_wrt_robot);

				if (is_reachable) {
					// Robustness filter: only skip to a future waypoint if it is seen as "reachable" during
					// a given number of timesteps:
					if (++wps.waypoints[idx].counter_seen_reachable > params_waypoints_navigator.min_timesteps_confirm_skip_waypoints) {
						most_advanced_wp = idx;
					}
				}

				// Is allowed to skip it?
				if (!wps.waypoints[idx].allow_skip)
					break; // Do not keep trying, since we are now allowed to skip this one.
			}

			if (most_advanced_wp>=0) {
				wps.waypoint_index_current_goal = most_advanced_wp;
				for (int k=most_advanced_wp_at_begin;k<most_advanced_wp;k++) {
					auto &wp = wps.waypoints[k];
					wp.reached = true;
					wp.skipped = true;
					wp.timestamp_reach = mrpt::system::now();

					m_robot.sendWaypointReachedEvent(k, false /* reason: skipped */);
				}
			}
		}

		// Still not started and no better guess? Start with the first waypoint:
		if (wps.waypoint_index_current_goal<0)
			wps.waypoint_index_current_goal = 0;

		// 3) Should I request a new (single target) navigation command?
		//    Only if the temporary goal changed:
		if (wps.waypoint_index_current_goal>=0 && prev_wp_index!=wps.waypoint_index_current_goal)
		{
			ASSERT_( wps.waypoint_index_current_goal < int(wps.waypoints.size()) );
			TWaypointStatus &wp = wps.waypoints[wps.waypoint_index_current_goal];
			const bool is_final_wp =  ( (wps.waypoint_index_current_goal+1)==int(wps.waypoints.size()) );

			m_robot.sendNewWaypointTargetEvent(wps.waypoint_index_current_goal);

			CAbstractNavigator::TNavigationParams nav_cmd;
			nav_cmd.target.x = wp.target.x;
			nav_cmd.target.y = wp.target.y;
			nav_cmd.target.phi = (wp.target_heading!=TWaypoint::INVALID_NUM ? wp.target_heading : .0);
			nav_cmd.target_frame_id = wp.target_frame_id;
			nav_cmd.targetAllowedDistance = wp.allowed_distance;
			nav_cmd.targetIsRelative = false;
			nav_cmd.targetIsIntermediaryWaypoint = !is_final_wp;
			nav_cmd.targetDesiredRelSpeed = (is_final_wp || (wp.target_heading != TWaypoint::INVALID_NUM)) ? 0.05 : 1.;

			this->navigate( &nav_cmd );

			MRPT_LOG_DEBUG_STREAM( "[CWaypointsNavigator::navigationStep] Active waypoint changed. Current status:\n" << this->getWaypointNavStatus().getAsText());
		}
	}
	}

	// Note: navigationStep() called *after* waypoints part to get end-of-navigation events *after*
	//       waypoints-related events:

	// Call base navigation step to execute one-single waypoint navigation, as usual:
	if (!is_aligning)
	{
		CAbstractNavigator::navigationStep();  // This internally locks "m_nav_cs"
	}

	m_was_aligning = is_aligning; // Let the next timestep know about this

	MRPT_END
}

void CWaypointsNavigator::onStartNewNavigation()
{
}

bool CWaypointsNavigator::isRelativePointReachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const
{
	return impl_waypoint_is_reachable(wp_local_wrt_robot);
}

void CWaypointsNavigator::loadConfigFile(const mrpt::utils::CConfigFileBase &c)
{
	MRPT_START

	params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
	CAbstractNavigator::loadConfigFile(c);

	MRPT_END
}

void CWaypointsNavigator::saveConfigFile(mrpt::utils::CConfigFileBase &c) const
{
	CAbstractNavigator::saveConfigFile(c);
	params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
}

void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFile(const mrpt::utils::CConfigFileBase & c, const std::string & s)
{
	MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
	MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
	MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
}

void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile(mrpt::utils::CConfigFileBase & c, const std::string & s) const
{
	MRPT_SAVE_CONFIG_VAR_COMMENT(max_distance_to_allow_skip_waypoint, "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(min_timesteps_confirm_skip_waypoints, "Min timesteps a `future` waypoint must be seen as reachable to become the active one.");
	MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT("waypoint_angle_tolerance", waypoint_angle_tolerance, "Angular error tolerance for waypoints with an assigned heading [deg] (Default: 5 deg)");
}

CWaypointsNavigator::TWaypointsNavigatorParams::TWaypointsNavigatorParams() :
	max_distance_to_allow_skip_waypoint(-1.0),
	min_timesteps_confirm_skip_waypoints(1),
	waypoint_angle_tolerance( mrpt::utils::DEG2RAD(5.0) )
{
}

bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
{
	return (!m_navigationParams->targetIsIntermediaryWaypoint) && (targetDist < m_navigationParams->targetAllowedDistance);
}
<commit_msg>fix waypoint alignment bug<commit_after>/* +---------------------------------------------------------------------------+
   |                     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/CWaypointsNavigator.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/math/wrap2pi.h>

using namespace mrpt::nav;
using namespace std;

CWaypointsNavigator::CWaypointsNavigator(CRobot2NavInterface &robot_if) :
	CAbstractNavigator(robot_if),
	m_was_aligning(false)
{
}

CWaypointsNavigator::~CWaypointsNavigator()
{
}

void CWaypointsNavigator::navigateWaypoints( const TWaypointSequence & nav_request )
{
	MRPT_START

	mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

	m_was_aligning = false;
	m_waypoint_nav_status = TWaypointStatusSequence();
	m_waypoint_nav_status.timestamp_nav_started = mrpt::system::now();

	const size_t N = nav_request.waypoints.size();
	ASSERTMSG_(N>0,"List of waypoints is empty!");

	m_waypoint_nav_status.waypoints.resize(N);
	// Copy waypoints fields data, leave status fields to defaults:
	for (size_t i=0;i<N;i++)
	{
		ASSERT_( nav_request.waypoints[i].isValid() );
		m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
	}

	m_waypoint_nav_status.waypoint_index_current_goal = -1;  // Not started yet.

	// The main loop navigationStep() will iterate over waypoints and send them to navigate()
	MRPT_END
}

void CWaypointsNavigator::getWaypointNavStatus(TWaypointStatusSequence & out_nav_status) const
{
	// No need to lock mutex...
	out_nav_status = m_waypoint_nav_status;
}

void CWaypointsNavigator::cancel()
{
	{
		mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);
		m_waypoint_nav_status = TWaypointStatusSequence();
	}
	CAbstractNavigator::cancel();
}

void CWaypointsNavigator::navigationStep()
{
	MRPT_START

	using mrpt::utils::square;

	// --------------------------------------
	//     Waypoint navigation algorithm
	// --------------------------------------
	bool is_aligning = false;  // the robot is aligning into a waypoint with a desired heading

	{
	mrpt::utils::CTimeLoggerEntry tle(m_timlog_delays,"CWaypointsNavigator::navigationStep()");
	mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

	TWaypointStatusSequence &wps = m_waypoint_nav_status; // shortcut to save typing

	if (wps.waypoints.empty() || wps.final_goal_reached)
	{
		// No nav request is pending or it was canceled
	}
	else
	{
		// 0) Get current robot pose:
		CAbstractNavigator::updateCurrentPoseAndSpeeds();

		// 1) default policy: go thru WPs one by one
		const int prev_wp_index = wps.waypoint_index_current_goal;

		mrpt::math::TSegment2D robot_move_seg;
		robot_move_seg.point1.x = m_curPoseVel.pose.x;
		robot_move_seg.point1.y = m_curPoseVel.pose.y;
		if (wps.last_robot_pose.x==TWaypoint::INVALID_NUM)
		{
			robot_move_seg.point2 = robot_move_seg.point1;
		}
		else
		{
			robot_move_seg.point2.x = wps.last_robot_pose.x;
			robot_move_seg.point2.y = wps.last_robot_pose.y;
		}
		wps.last_robot_pose = m_curPoseVel.pose; // save for next iters

		if (wps.waypoint_index_current_goal >= 0)
		{
			auto &wp = wps.waypoints[wps.waypoint_index_current_goal];
			const double dist2target = robot_move_seg.distance(wp.target);
			if (dist2target < wp.allowed_distance || m_was_aligning /* we were already aligning at a WP */)
			{
				bool consider_wp_reached = false;
				if (wp.target_heading == TWaypoint::INVALID_NUM)
				{
					consider_wp_reached = true;
				}
				else
				{
					// Handle pure-rotation robot interface to honor target_heading
					const double ang_err = mrpt::math::angDistance(m_curPoseVel.pose.phi, wp.target_heading);
					if (std::abs(ang_err) <= params_waypoints_navigator.waypoint_angle_tolerance)
					{
						consider_wp_reached = true;
					}
					else
					{
						is_aligning = true;

						if (!m_was_aligning)
						{
							// 1st time we are aligning:
							// Send vel_cmd to the robot:
							mrpt::kinematics::CVehicleVelCmdPtr align_cmd = m_robot.getAlignCmd(ang_err);

							MRPT_LOG_INFO_FMT(
								"[CWaypointsNavigator::navigationStep] Trying to align to heading: %.02f deg. "
								"Relative heading: %.02f deg. "
								"With motion cmd: %s",
								mrpt::utils::RAD2DEG(wp.target_heading),
								mrpt::utils::RAD2DEG(ang_err),
								align_cmd ? align_cmd->asString().c_str() : "nullptr (operation not supported by this robot)");

							this->stop(false /*not emergency*/); // In any case, do a "stop"
							if (align_cmd) {
								this->changeSpeeds(*align_cmd);
							} else {
								consider_wp_reached = true; // this robot does not support "in place" alignment
							}
						}
						else
						{
							MRPT_LOG_THROTTLE_INFO_FMT(0.5, "[CWaypointsNavigator::navigationStep] Waiting for the robot to get aligned: current_heading=%.02f deg target_heading=%.02f deg", mrpt::utils::RAD2DEG(m_curPoseVel.pose.phi), mrpt::utils::RAD2DEG(wp.target_heading) );
						}
					}
				}

				if (consider_wp_reached)
				{
					MRPT_LOG_DEBUG_STREAM("[CWaypointsNavigator::navigationStep] Waypoint " <<
						(wps.waypoint_index_current_goal + 1) << "/" << wps.waypoints.size() << " reached."
						" segment-to-target dist: " << dist2target << ", allowed_dist: " << wp.allowed_distance
					);

					wp.reached = true;
					wp.skipped = false;
					wp.timestamp_reach = mrpt::system::now();
					m_robot.sendWaypointReachedEvent(wps.waypoint_index_current_goal, true /* reason: really reached*/);

					// Was this the final goal??
					if (wps.waypoint_index_current_goal < int(wps.waypoints.size() - 1)) {
						wps.waypoint_index_current_goal++;
					}
					else {
						wps.final_goal_reached = true;
					}
				}
			}
		}

		// 2) More advanced policy: if available, use children class methods to decide
		//     which is the best candidate for the next waypoint, if we can skip current one:
		if (!wps.final_goal_reached &&
			wps.waypoint_index_current_goal >= 0 &&
			wps.waypoints[wps.waypoint_index_current_goal].allow_skip
			)
		{
			const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
			int most_advanced_wp = wps.waypoint_index_current_goal;
			const int most_advanced_wp_at_begin = most_advanced_wp;

			for (int idx=wps.waypoint_index_current_goal;idx<(int)wps.waypoints.size();idx++)
			{
				if (idx<0) continue;
				if (wps.waypoints[idx].reached) continue;

				// Is it reachable?
				mrpt::math::TPoint2D wp_local_wrt_robot;
				robot_pose.inverseComposePoint(wps.waypoints[idx].target, wp_local_wrt_robot);

				if (params_waypoints_navigator.max_distance_to_allow_skip_waypoint>0 && wp_local_wrt_robot.norm()>params_waypoints_navigator.max_distance_to_allow_skip_waypoint)
					continue; // Skip this one, it is too far away

				const bool is_reachable = this->impl_waypoint_is_reachable(wp_local_wrt_robot);

				if (is_reachable) {
					// Robustness filter: only skip to a future waypoint if it is seen as "reachable" during
					// a given number of timesteps:
					if (++wps.waypoints[idx].counter_seen_reachable > params_waypoints_navigator.min_timesteps_confirm_skip_waypoints) {
						most_advanced_wp = idx;
					}
				}

				// Is allowed to skip it?
				if (!wps.waypoints[idx].allow_skip)
					break; // Do not keep trying, since we are now allowed to skip this one.
			}

			if (most_advanced_wp>=0) {
				wps.waypoint_index_current_goal = most_advanced_wp;
				for (int k=most_advanced_wp_at_begin;k<most_advanced_wp;k++) {
					auto &wp = wps.waypoints[k];
					wp.reached = true;
					wp.skipped = true;
					wp.timestamp_reach = mrpt::system::now();

					m_robot.sendWaypointReachedEvent(k, false /* reason: skipped */);
				}
			}
		}

		// Still not started and no better guess? Start with the first waypoint:
		if (wps.waypoint_index_current_goal<0)
			wps.waypoint_index_current_goal = 0;

		// 3) Should I request a new (single target) navigation command?
		//    Only if the temporary goal changed:
		if (wps.waypoint_index_current_goal>=0 && prev_wp_index!=wps.waypoint_index_current_goal)
		{
			ASSERT_( wps.waypoint_index_current_goal < int(wps.waypoints.size()) );
			TWaypointStatus &wp = wps.waypoints[wps.waypoint_index_current_goal];
			const bool is_final_wp =  ( (wps.waypoint_index_current_goal+1)==int(wps.waypoints.size()) );

			m_robot.sendNewWaypointTargetEvent(wps.waypoint_index_current_goal);

			CAbstractNavigator::TNavigationParams nav_cmd;
			nav_cmd.target.x = wp.target.x;
			nav_cmd.target.y = wp.target.y;
			nav_cmd.target.phi = (wp.target_heading!=TWaypoint::INVALID_NUM ? wp.target_heading : .0);
			nav_cmd.target_frame_id = wp.target_frame_id;
			nav_cmd.targetAllowedDistance = wp.allowed_distance;
			nav_cmd.targetIsRelative = false;
			nav_cmd.targetIsIntermediaryWaypoint = !is_final_wp;
			nav_cmd.targetDesiredRelSpeed = (is_final_wp || (wp.target_heading != TWaypoint::INVALID_NUM)) ? 0.05 : 1.;

			this->navigate( &nav_cmd );

			MRPT_LOG_DEBUG_STREAM( "[CWaypointsNavigator::navigationStep] Active waypoint changed. Current status:\n" << this->getWaypointNavStatus().getAsText());
		}
	}
	}

	// Note: navigationStep() called *after* waypoints part to get end-of-navigation events *after*
	//       waypoints-related events:

	// Call base navigation step to execute one-single waypoint navigation, as usual:
	if (!is_aligning)
	{
		CAbstractNavigator::navigationStep();  // This internally locks "m_nav_cs"
	}

	m_was_aligning = is_aligning; // Let the next timestep know about this

	MRPT_END
}

void CWaypointsNavigator::onStartNewNavigation()
{
}

bool CWaypointsNavigator::isRelativePointReachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const
{
	return impl_waypoint_is_reachable(wp_local_wrt_robot);
}

void CWaypointsNavigator::loadConfigFile(const mrpt::utils::CConfigFileBase &c)
{
	MRPT_START

	params_waypoints_navigator.loadFromConfigFile(c, "CWaypointsNavigator");
	CAbstractNavigator::loadConfigFile(c);

	MRPT_END
}

void CWaypointsNavigator::saveConfigFile(mrpt::utils::CConfigFileBase &c) const
{
	CAbstractNavigator::saveConfigFile(c);
	params_waypoints_navigator.saveToConfigFile(c, "CWaypointsNavigator");
}

void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFile(const mrpt::utils::CConfigFileBase & c, const std::string & s)
{
	MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
	MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
	MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
}

void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile(mrpt::utils::CConfigFileBase & c, const std::string & s) const
{
	MRPT_SAVE_CONFIG_VAR_COMMENT(max_distance_to_allow_skip_waypoint, "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(min_timesteps_confirm_skip_waypoints, "Min timesteps a `future` waypoint must be seen as reachable to become the active one.");
	MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT("waypoint_angle_tolerance", waypoint_angle_tolerance, "Angular error tolerance for waypoints with an assigned heading [deg] (Default: 5 deg)");
}

CWaypointsNavigator::TWaypointsNavigatorParams::TWaypointsNavigatorParams() :
	max_distance_to_allow_skip_waypoint(-1.0),
	min_timesteps_confirm_skip_waypoints(1),
	waypoint_angle_tolerance( mrpt::utils::DEG2RAD(5.0) )
{
}

bool CWaypointsNavigator::checkHasReachedTarget(const double targetDist) const
{
	return (!m_navigationParams->targetIsIntermediaryWaypoint) && (targetDist < m_navigationParams->targetAllowedDistance);
}
<|endoftext|>


<commit_before>#include "ofURLFileLoader.h"
#include "ofAppRunner.h"
#include "ofUtils.h"

#include "Poco/Net/HTTPClientSession.h"
#include "Poco/Net/HTTPRequest.h"
#include "Poco/Net/HTTPResponse.h"
#include "Poco/StreamCopier.h"
#include "Poco/Path.h"
#include "Poco/URI.h"
#include "Poco/Exception.h"
#include "Poco/URIStreamOpener.h"
#include "Poco/Net/HTTPStreamFactory.h"

using Poco::Net::HTTPClientSession;
using Poco::Net::HTTPRequest;
using Poco::Net::HTTPResponse;
using Poco::Net::HTTPMessage;
using Poco::Net::HTTPStreamFactory;

using Poco::StreamCopier;
using Poco::Path;
using Poco::URI;
using Poco::URIStreamOpener;
using Poco::Exception;

#include "ofConstants.h"

static bool factoryLoaded = false;
int	ofHttpRequest::nextID = 0;
ofEvent<ofHttpResponse> ofURLResponseEvent;

ofURLFileLoader::ofURLFileLoader() {
	if(!factoryLoaded){
		HTTPStreamFactory::registerFactory();
		factoryLoaded = true;
	}
}

ofHttpResponse ofURLFileLoader::get(string url) {
    ofHttpRequest request(url,url);
    return handleRequest(request);
}

void ofURLFileLoader::getAsync(string url, string name){
	if(name=="") name=url;
	ofHttpRequest request(url,name);
	lock();
	requests.push_back(request);
	unlock();
	start();
}

void ofURLFileLoader::remove(ofHttpRequest httpRequest){
	lock();
	for(int i=0;i<requests.size();i++){
		if(requests[i].getID()==httpRequest.getID()){
			requests.erase(requests.begin()+i);
			return;
		}
	}
	unlock();
}

void ofURLFileLoader::start() {
     if (isThreadRunning() == false){
    	 ofAddListener(ofEvents.update,this,&ofURLFileLoader::update);
         startThread(false, false);   // blocking, verbose
    }
}

void ofURLFileLoader::stop() {
    stopThread();
}

void ofURLFileLoader::threadedFunction() {
	while( isThreadRunning() == true ){
		lock();
		if(requests.size()>0){
			ofHttpRequest request(requests.front());
			unlock();
			ofHttpResponse response(handleRequest(request));
			if(response.status!=-1){
				lock();
				responses.push(response);
				requests.pop_front();
				unlock();
			}
		}else{
			unlock();
			stop();
		}
		ofSleepMillis(10);
	}
}

ofHttpResponse ofURLFileLoader::handleRequest(ofHttpRequest request) {
	try {
		URI uri(request.url);
		std::string path(uri.getPathAndQuery());
		if (path.empty()) path = "/";

		HTTPClientSession session(uri.getHost(), uri.getPort());
		HTTPRequest req(HTTPRequest::HTTP_GET, path, HTTPMessage::HTTP_1_1);
		session.setTimeout(Poco::Timespan(20,0));
		session.sendRequest(req);
		HTTPResponse res;
		istream& rs = session.receiveResponse(res);
		return ofHttpResponse(request,rs,res.getStatus(),res.getReason());
	} catch (Exception& exc) {
        ofLog(OF_LOG_ERROR, "ofURLFileLoader " + exc.displayText());

        return ofHttpResponse(request,-1,exc.displayText());
    }

}

void ofURLFileLoader::update(ofEventArgs & args){
	if(responses.size()){
		ofHttpResponse response(responses.front());
		ofNotifyEvent(ofURLResponseEvent,response);
		responses.pop();
		if(!isThreadRunning() && !responses.size())
			ofRemoveListener(ofEvents.update,this,&ofURLFileLoader::update);
	}

}

static ofURLFileLoader fileLoader;

ofHttpResponse ofLoadURL(string url){
	return fileLoader.get(url);
}

void ofLoadURLAsync(string url, string name){
	fileLoader.getAsync(url,name);
}

void ofRemoveURLRequest(ofHttpRequest request){
	fileLoader.remove(request);
}
<commit_msg>Added ofThreadedImageLoader to devapps<commit_after>#include "ofURLFileLoader.h"
#include "ofAppRunner.h"
#include "ofUtils.h"

#include "Poco/Net/HTTPClientSession.h"
#include "Poco/Net/HTTPRequest.h"
#include "Poco/Net/HTTPResponse.h"
#include "Poco/StreamCopier.h"
#include "Poco/Path.h"
#include "Poco/URI.h"
#include "Poco/Exception.h"
#include "Poco/URIStreamOpener.h"
#include "Poco/Net/HTTPStreamFactory.h"

using Poco::Net::HTTPClientSession;
using Poco::Net::HTTPRequest;
using Poco::Net::HTTPResponse;
using Poco::Net::HTTPMessage;
using Poco::Net::HTTPStreamFactory;

using Poco::StreamCopier;
using Poco::Path;
using Poco::URI;
using Poco::URIStreamOpener;
using Poco::Exception;

#include "ofConstants.h"

static bool factoryLoaded = false;
int	ofHttpRequest::nextID = 0;
ofEvent<ofHttpResponse> ofURLResponseEvent;

ofURLFileLoader::ofURLFileLoader() {
	if(!factoryLoaded){
		try {
			HTTPStreamFactory::registerFactory();
			factoryLoaded = true;
		}
		catch (Poco::SystemException PS) {
			ofLog(OF_LOG_ERROR, "Got exception in url ofURLFileloader");
		}
	}
}

ofHttpResponse ofURLFileLoader::get(string url) {
    ofHttpRequest request(url,url);
    return handleRequest(request);
}

void ofURLFileLoader::getAsync(string url, string name){
	if(name=="") name=url;
	ofHttpRequest request(url,name);
	lock();
	requests.push_back(request);
	unlock();
	start();
}

void ofURLFileLoader::remove(ofHttpRequest httpRequest){
	lock();
	for(int i=0;i<requests.size();i++){
		if(requests[i].getID()==httpRequest.getID()){
			requests.erase(requests.begin()+i);
			return;
		}
	}
	unlock();
}

void ofURLFileLoader::start() {
     if (isThreadRunning() == false){
    	 ofAddListener(ofEvents.update,this,&ofURLFileLoader::update);
         startThread(false, false);   // blocking, verbose
    }
}

void ofURLFileLoader::stop() {
    stopThread();
}

void ofURLFileLoader::threadedFunction() {
	while( isThreadRunning() == true ){
		lock();
		if(requests.size()>0){
			ofHttpRequest request(requests.front());
			unlock();
			ofHttpResponse response(handleRequest(request));
			if(response.status!=-1){
				lock();
				responses.push(response);
				requests.pop_front();
				unlock();
			}
		}else{
			unlock();
			stop();
		}
		ofSleepMillis(10);
	}
}

ofHttpResponse ofURLFileLoader::handleRequest(ofHttpRequest request) {
	try {
		URI uri(request.url);
		std::string path(uri.getPathAndQuery());
		if (path.empty()) path = "/";

		HTTPClientSession session(uri.getHost(), uri.getPort());
		HTTPRequest req(HTTPRequest::HTTP_GET, path, HTTPMessage::HTTP_1_1);
		session.setTimeout(Poco::Timespan(20,0));
		session.sendRequest(req);
		HTTPResponse res;
		istream& rs = session.receiveResponse(res);
		return ofHttpResponse(request,rs,res.getStatus(),res.getReason());
	} catch (Exception& exc) {
        ofLog(OF_LOG_ERROR, "ofURLFileLoader " + exc.displayText());

        return ofHttpResponse(request,-1,exc.displayText());
    }

}

void ofURLFileLoader::update(ofEventArgs & args){
	if(responses.size()){
		ofHttpResponse response(responses.front());
		ofNotifyEvent(ofURLResponseEvent,response);
		responses.pop();
		if(!isThreadRunning() && !responses.size())
			ofRemoveListener(ofEvents.update,this,&ofURLFileLoader::update);
	}

}

static ofURLFileLoader fileLoader;

ofHttpResponse ofLoadURL(string url){
	return fileLoader.get(url);
}

void ofLoadURLAsync(string url, string name){
	fileLoader.getAsync(url,name);
}

void ofRemoveURLRequest(ofHttpRequest request){
	fileLoader.remove(request);
}
<|endoftext|>


<commit_before>/* ****************************************************************************
 *
 * FILE                     DelilahConsole.h
 *
 * DESCRIPTION			   Console terminal for delilah
 *
 */

#include <ncurses.h>
#include <sys/stat.h>					// stat(.)
#include <dirent.h>						// DIR directory header
#include <iostream>
#include <iomanip>

#include "au/string.h"						// au::Format
#include "au/CommandLine.h"				// au::CommandLine

#include "pugi/pugi.h"                      // pugi::Pugi

#include "engine/MemoryManager.h"				// samson::MemoryManager

#include "samson/common/EnvironmentOperations.h"						// Environment operations (CopyFrom)
#include "samson/common/Info.h"                             // samson::Info

#include "samson/network/Packet.h"						// ss:Packet

#include "samson/module/samsonVersion.h"		// SAMSON_VERSION
#include "samson/module/samsonVersion.h"

#include "samson/delilah/Delilah.h"					// samson::Delailh
#include "samson/stream/BlockManager.h"     // samson::stream::BlockManager

#include "UploadDelilahComponent.h"		// samson::DelilahUploadComponent
#include "DownloadDelilahComponent.h"		// samson::DelilahDownloadComponent
#include "PushDelilahComponent.h"
#include "PopDelilahComponent.h"

#include "samson/delilah/DelilahUtils.h"        // getXXXInfo()

#include "DelilahMonitorization.h"				// Own interface

namespace samson
{

    std::string DelilahMonitorization::getHeaderLeft()
    {
        switch (type) {
            case general:
                return "General information";
                break;

            case task:
                return "Tasks";
                break;

            case queues:
                return "Stream queues information";
                break;

            case queues_tasks:
                return "Stream queue tasks";
                break;
        }

        return "Unknown information";

    }

    std::string DelilahMonitorization::getHeaderRight()
    {
        std::ostringstream txt;
        txt << " ( Updated: " << getUpdateSeconds() <<  " )";
        return txt.str();
    }

    void DelilahMonitorization::evalComamnd( )
    {
        if( command == "ps_tasks" )
        {
            type = task;
            return;
        }

        if( command == "engine_show" )
        {
            type = general;
            return;
        }

        if( command == "ls_queues" )
        {
            type = queues;
            return;
        }

        if( command == "ps_stream" )
        {
            type = queues_tasks;
            return;
        }

        if( command == "quit" )
        {
            quitConsole = true;
            return;
        }

    }

    void DelilahMonitorization::printContent()
    {

        print("");

        switch (type) {
            case general:
                printLines( getStringInfo("/engine_system", getEngineSimplifiedSystemInfo, i_worker ) );
                break;

            case task:
                printLines( getStringInfo("/controller_task_manager//controller_task", getTaskInfo, i_controller ) );

                printLines( getStringInfo("/worker_task_manager//worker_task", getWorkerTaskInfo, i_worker  ) );
                break;

            case queues:
                printLines(getStringInfo("/stream_manager/queues/queue", getQueueInfo, i_worker | i_no_title ));
                break;

            case queues_tasks:
                printLines( getStringInfo("/stream_manager//queue_task", getQueueTaskInfo, i_worker ) );
                break;

        }

    }

    void DelilahMonitorization::auto_complete
        ( std::vector<std::string>& previous_words , std::string& current_word ,std::vector<std::string>& command_options )
    {
        // If no previous command, just main commands
        if( previous_words.size() == 0)
        {
            command_options.insert( command_options.end() , main_commands.begin() ,  main_commands.end() );
            return;
        }
    }

}
<commit_msg>Show title when displaying queues in a cluster<commit_after>/* ****************************************************************************
 *
 * FILE                     DelilahConsole.h
 *
 * DESCRIPTION			   Console terminal for delilah
 *
 */

#include <ncurses.h>
#include <sys/stat.h>					// stat(.)
#include <dirent.h>						// DIR directory header
#include <iostream>
#include <iomanip>

#include "au/string.h"						// au::Format
#include "au/CommandLine.h"				// au::CommandLine

#include "pugi/pugi.h"                      // pugi::Pugi

#include "engine/MemoryManager.h"				// samson::MemoryManager

#include "samson/common/EnvironmentOperations.h"						// Environment operations (CopyFrom)
#include "samson/common/Info.h"                             // samson::Info

#include "samson/network/Packet.h"						// ss:Packet

#include "samson/module/samsonVersion.h"		// SAMSON_VERSION
#include "samson/module/samsonVersion.h"

#include "samson/delilah/Delilah.h"					// samson::Delailh
#include "samson/stream/BlockManager.h"     // samson::stream::BlockManager

#include "UploadDelilahComponent.h"		// samson::DelilahUploadComponent
#include "DownloadDelilahComponent.h"		// samson::DelilahDownloadComponent
#include "PushDelilahComponent.h"
#include "PopDelilahComponent.h"

#include "samson/delilah/DelilahUtils.h"        // getXXXInfo()

#include "DelilahMonitorization.h"				// Own interface

namespace samson
{

    std::string DelilahMonitorization::getHeaderLeft()
    {
        switch (type) {
            case general:
                return "General information";
                break;

            case task:
                return "Tasks";
                break;

            case queues:
                return "Stream queues information";
                break;

            case queues_tasks:
                return "Stream queue tasks";
                break;
        }

        return "Unknown information";

    }

    std::string DelilahMonitorization::getHeaderRight()
    {
        std::ostringstream txt;
        txt << " ( Updated: " << getUpdateSeconds() <<  " )";
        return txt.str();
    }

    void DelilahMonitorization::evalComamnd( )
    {
        if( command == "ps_tasks" )
        {
            type = task;
            return;
        }

        if( command == "engine_show" )
        {
            type = general;
            return;
        }

        if( command == "ls_queues" )
        {
            type = queues;
            return;
        }

        if( command == "ps_stream" )
        {
            type = queues_tasks;
            return;
        }

        if( command == "quit" )
        {
            quitConsole = true;
            return;
        }

    }

    void DelilahMonitorization::printContent()
    {

        print("");

        switch (type) {
            case general:
                printLines( getStringInfo("/engine_system", getEngineSimplifiedSystemInfo, i_worker ) );
                break;

            case task:
                printLines( getStringInfo("/controller_task_manager//controller_task", getTaskInfo, i_controller ) );

                printLines( getStringInfo("/worker_task_manager//worker_task", getWorkerTaskInfo, i_worker  ) );
                break;

            case queues:
                printLines(getStringInfo("/stream_manager/queues/queue", getQueueInfo, i_worker  ));
                break;

            case queues_tasks:
                printLines( getStringInfo("/stream_manager//queue_task", getQueueTaskInfo, i_worker ) );
                break;

        }

    }

    void DelilahMonitorization::auto_complete
        ( std::vector<std::string>& previous_words , std::string& current_word ,std::vector<std::string>& command_options )
    {
        // If no previous command, just main commands
        if( previous_words.size() == 0)
        {
            command_options.insert( command_options.end() , main_commands.begin() ,  main_commands.end() );
            return;
        }
    }

}
<|endoftext|>


<commit_before>/*
 * CAN bus IO logic.
 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
 */

#pragma once

#include <cassert>
#include <uavcan/error.hpp>
#include <uavcan/stdint.hpp>
#include <uavcan/util/linked_list.hpp>
#include <uavcan/dynamic_memory.hpp>
#include <uavcan/build_config.hpp>
#include <uavcan/util/templates.hpp>
#include <uavcan/util/lazy_constructor.hpp>
#include <uavcan/driver/can.hpp>
#include <uavcan/driver/system_clock.hpp>
#include <uavcan/time.hpp>

namespace uavcan
{

enum { MaxCanIfaces = 3 };

struct UAVCAN_EXPORT CanRxFrame : public CanFrame
{
    MonotonicTime ts_mono;
    UtcTime ts_utc;
    uint8_t iface_index;

    CanRxFrame()
        : iface_index(0)
    { }

#if UAVCAN_TOSTRING
    std::string toString(StringRepresentation mode = StrTight) const;
#endif
};

class UAVCAN_EXPORT CanTxQueue : Noncopyable
{
public:
    enum Qos { Volatile, Persistent };

    struct Entry : public LinkedListNode<Entry>  // Not required to be packed - fits the block in any case
    {
        MonotonicTime deadline;
        CanFrame frame;
        uint8_t qos;
        CanIOFlags flags;

        Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags)
            : deadline(arg_deadline)
            , frame(arg_frame)
            , qos(uint8_t(arg_qos))
            , flags(arg_flags)
        {
            UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent));
            IsDynamicallyAllocatable<Entry>::check();
        }

        static void destroy(Entry*& obj, IPoolAllocator& allocator);

        bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; }

        bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
        bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
        bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); }
        bool qosLowerThan(const Entry& rhs)  const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); }

#if UAVCAN_TOSTRING
        std::string toString() const;
#endif
    };

private:
    class PriorityInsertionComparator
    {
        const CanFrame& frm_;
    public:
        explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { }
        bool operator()(const Entry* entry)
        {
            UAVCAN_ASSERT(entry);
            return frm_.priorityHigherThan(entry->frame);
        }
    };

    LinkedListRoot<Entry> queue_;
    LimitedPoolAllocator allocator_;
    ISystemClock& sysclock_;
    uint32_t rejected_frames_cnt_;

    void registerRejectedFrame();

public:
    CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota)
        : allocator_(allocator, allocator_quota)
        , sysclock_(sysclock)
        , rejected_frames_cnt_(0)
    { }

    ~CanTxQueue();

    void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags);

    Entry* peek();               // Modifier
    void remove(Entry*& entry);

    bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const;

    uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; }

    bool isEmpty() const { return queue_.isEmpty(); }
};

struct UAVCAN_EXPORT CanIfacePerfCounters
{
    uint64_t frames_tx;
    uint64_t frames_rx;
    uint64_t errors;

    CanIfacePerfCounters()
        : frames_tx(0)
        , frames_rx(0)
        , errors(0)
    { }
};

class UAVCAN_EXPORT CanIOManager : Noncopyable
{
    struct IfaceFrameCounters
    {
        uint64_t frames_tx;
        uint64_t frames_rx;

        IfaceFrameCounters()
            : frames_tx(0)
            , frames_rx(0)
        { }
    };

    ICanDriver& driver_;
    ISystemClock& sysclock_;

    LazyConstructor<CanTxQueue> tx_queues_[MaxCanIfaces];
    IfaceFrameCounters counters_[MaxCanIfaces];

    const uint8_t num_ifaces_;

    int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags);
    int sendFromTxQueue(uint8_t iface_index);
    uint8_t makePendingTxMask() const;
    int callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline);

public:
    CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
                 std::size_t mem_blocks_per_iface = 0);

    uint8_t getNumIfaces() const { return num_ifaces_; }

    CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const;

    /**
     * Returns:
     *  0 - rejected/timedout/enqueued
     *  1+ - sent/received
     *  negative - failure
     */
    int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
             uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags);
    int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags);
};

}
<commit_msg>CanIOManager::getCanDriver()<commit_after>/*
 * CAN bus IO logic.
 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
 */

#pragma once

#include <cassert>
#include <uavcan/error.hpp>
#include <uavcan/stdint.hpp>
#include <uavcan/util/linked_list.hpp>
#include <uavcan/dynamic_memory.hpp>
#include <uavcan/build_config.hpp>
#include <uavcan/util/templates.hpp>
#include <uavcan/util/lazy_constructor.hpp>
#include <uavcan/driver/can.hpp>
#include <uavcan/driver/system_clock.hpp>
#include <uavcan/time.hpp>

namespace uavcan
{

enum { MaxCanIfaces = 3 };

struct UAVCAN_EXPORT CanRxFrame : public CanFrame
{
    MonotonicTime ts_mono;
    UtcTime ts_utc;
    uint8_t iface_index;

    CanRxFrame()
        : iface_index(0)
    { }

#if UAVCAN_TOSTRING
    std::string toString(StringRepresentation mode = StrTight) const;
#endif
};

class UAVCAN_EXPORT CanTxQueue : Noncopyable
{
public:
    enum Qos { Volatile, Persistent };

    struct Entry : public LinkedListNode<Entry>  // Not required to be packed - fits the block in any case
    {
        MonotonicTime deadline;
        CanFrame frame;
        uint8_t qos;
        CanIOFlags flags;

        Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags)
            : deadline(arg_deadline)
            , frame(arg_frame)
            , qos(uint8_t(arg_qos))
            , flags(arg_flags)
        {
            UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent));
            IsDynamicallyAllocatable<Entry>::check();
        }

        static void destroy(Entry*& obj, IPoolAllocator& allocator);

        bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; }

        bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
        bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
        bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); }
        bool qosLowerThan(const Entry& rhs)  const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); }

#if UAVCAN_TOSTRING
        std::string toString() const;
#endif
    };

private:
    class PriorityInsertionComparator
    {
        const CanFrame& frm_;
    public:
        explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { }
        bool operator()(const Entry* entry)
        {
            UAVCAN_ASSERT(entry);
            return frm_.priorityHigherThan(entry->frame);
        }
    };

    LinkedListRoot<Entry> queue_;
    LimitedPoolAllocator allocator_;
    ISystemClock& sysclock_;
    uint32_t rejected_frames_cnt_;

    void registerRejectedFrame();

public:
    CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota)
        : allocator_(allocator, allocator_quota)
        , sysclock_(sysclock)
        , rejected_frames_cnt_(0)
    { }

    ~CanTxQueue();

    void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags);

    Entry* peek();               // Modifier
    void remove(Entry*& entry);

    bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const;

    uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; }

    bool isEmpty() const { return queue_.isEmpty(); }
};

struct UAVCAN_EXPORT CanIfacePerfCounters
{
    uint64_t frames_tx;
    uint64_t frames_rx;
    uint64_t errors;

    CanIfacePerfCounters()
        : frames_tx(0)
        , frames_rx(0)
        , errors(0)
    { }
};

class UAVCAN_EXPORT CanIOManager : Noncopyable
{
    struct IfaceFrameCounters
    {
        uint64_t frames_tx;
        uint64_t frames_rx;

        IfaceFrameCounters()
            : frames_tx(0)
            , frames_rx(0)
        { }
    };

    ICanDriver& driver_;
    ISystemClock& sysclock_;

    LazyConstructor<CanTxQueue> tx_queues_[MaxCanIfaces];
    IfaceFrameCounters counters_[MaxCanIfaces];

    const uint8_t num_ifaces_;

    int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags);
    int sendFromTxQueue(uint8_t iface_index);
    uint8_t makePendingTxMask() const;
    int callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline);

public:
    CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
                 std::size_t mem_blocks_per_iface = 0);

    uint8_t getNumIfaces() const { return num_ifaces_; }

    CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const;

    const ICanDriver& getCanDriver() const { return driver_; }

    /**
     * Returns:
     *  0 - rejected/timedout/enqueued
     *  1+ - sent/received
     *  negative - failure
     */
    int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
             uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags);
    int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags);
};

}
<|endoftext|>


<commit_before>//===========================================================================
//
// File: benchmarkLRSurfacePointEval.C
//
// Created: Tue Jan 29 13:57:16 2013
//
// Author: Sverre Briseid <Sverre.Briseid@sintef.no>
//
// Revision: $Id$
//
// Description: Benchmark the point evaluation (with higher orders)
//              for a LRSplineSurface.  If input is a regular
//              SplineSurface, we convert to LRSplineSurface and
//              compare. If input is a LRSplineSurface, we compare
//              with the corresponding refined SplineSurface.
//
//===========================================================================

#include "GoTools/lrsplines2D/LRSplineSurface.h"
#include "GoTools/lrsplines2D/Mesh2D.h"
#include "GoTools/lrsplines2D/Direction2D.h"
#include "GoTools/lrsplines2D/LRBenchmarkUtils.h"
#include "GoTools/lrsplines2D/LRSplinePlotUtils.h"
#include "GoTools/geometry/ObjectHeader.h"
#include "GoTools/geometry/SplineSurface.h"
#include "GoTools/geometry/GeometryTools.h"
#include "GoTools/utils/config.h"

#include <iostream>
#include <assert.h>
#include <fstream>
#include <string>
#include "string.h"

using Go::Header;
using Go::SplineSurface;
using Go::LRSplineSurface;

// Assuming the domain is the same.
double maxDist(const SplineSurface& spline_sf,
	       const LRSplineSurface& lr_spline_sf,
	       int num_samples_u, int num_samples_v);

int main(int argc, char *argv[])
{
  if (argc != 5)
  {
      std::cout << "Usage: (lr)spline_sf.g2 num_dir_samples num_derivs num_iter" << std::endl;
      return -1;
  }

  char* filein_char = argv[1];
  std::ifstream filein(argv[1]); // Input lr spline.
  // If input surface is not fine enough we refine.
  int num_dir_samples = atoi(argv[2]);
  int num_derivs = atoi(argv[3]);
  int num_iter = atoi(argv[4]);
  if (num_iter != 1)
      puts("Not using num_iter yet.");

  shared_ptr<SplineSurface> spline_sf;
  shared_ptr<LRSplineSurface> lr_spline_sf;
  double knot_tol = 1e-10;

  // If we refine the lr-surface in order to extract a regular
  // spline-version we must perform the operations on a copy.
  shared_ptr<LRSplineSurface> lr_spline_sf_copy;

  int order_u, order_v, num_coefs_u, num_coefs_v, dim, num_bases=-1;
  if (strcasestr(filein_char, ".g2"))
    {
      ObjectHeader header;
      filein >> header;
      if (header.classType() == Class_SplineSurface)
	{
	  spline_sf = shared_ptr<SplineSurface>(new SplineSurface());
	  filein >> *spline_sf;
	  // We create the lr-version.
	  lr_spline_sf = shared_ptr<LRSplineSurface>(new LRSplineSurface(spline_sf.get(), knot_tol));
	}
      else if (header.classType() == Class_LRSplineSurface)
	{
	  lr_spline_sf = shared_ptr<LRSplineSurface>(new LRSplineSurface());
	  filein >> *spline_sf;
	  lr_spline_sf_copy = shared_ptr<LRSplineSurface>(lr_spline_sf->clone());

	}
      else
	{
	  std::cout << "Input was not a SplineSurface or a LRSplineSurface, exiting!" << std::endl;
	}
    }
  else
    {
      MESSAGE("Input was not a g2-file!");
      return -1;
    }

  // order_u = spline_sf->order_u();
  // order_v = spline_sf->order_v();
  num_coefs_u = spline_sf->numCoefs_u();
  num_coefs_v = spline_sf->numCoefs_v();
  // We must count the number of segments. We do this the easiest way
  // by looking at the number of different knots in each direction,
  // (and subtract one).
  vector<double> all_knots_u(spline_sf->basis_u().begin(), spline_sf->basis_u().end());
  vector<double> all_knots_v(spline_sf->basis_v().begin(), spline_sf->basis_v().end());
  vector<double>::iterator last_uniq_u = unique(all_knots_u.begin(), all_knots_u.end(), my_equal_function);
  vector<double>::iterator last_uniq_v = unique(all_knots_v.begin(), all_knots_v.end(), my_equal_function);
  vector<double> unique_knots_u(all_knots_u.begin(), last_uniq_u);
  vector<double> unique_knots_v(all_knots_v.begin(), last_uniq_v);

  int num_segs_u = unique_knots_u.size() - 1;
  int num_segs_v = unique_knots_v.size() - 1;
  std::cout << "num_segs_u: " << num_segs_u << ", num_segs_v: " << num_segs_v << std::endl;

  // We create the refinement vectors.
  // First segments with a constant u parameter.
  vector<LRSplineSurface::Refinement2D> refs_u, refs_v;
  int mult = 1;
  Direction2D dir = XFIXED; // XDIRFIXED, refining in the x-dir (u).
  for (size_t ki = 0; ki < num_segs_u - 1; ++ki)
  {
      // We bisect the interval.
      double wgt = 0.4; // Value in the range [0.0, 1.0].
      double ref_par = wgt*unique_knots_u[ki] + (1.0 - wgt)*unique_knots_u[ki+1];

      // We insert line segments which cover order_v - 1 intervals.
      for (size_t kj = order_v - 1; kj < num_segs_v; kj += order_v)
      {
	  // To avoid a too regular pattern we alter start
	  // values. Maybe we should use a random function ...
	  double tmin = (ki%2 == 1) ? unique_knots_v[kj - order_v + 1] : unique_knots_v[kj - order_v + 2];
	  double tmax = (ki%2 == 1) ? unique_knots_v[kj] : unique_knots_v[kj + 1];

	  LRSplineSurface::Refinement2D ref;
	  ref.kval = ref_par;
	  ref.start = tmin;
	  ref.end = tmax;
	  ref.d = dir;
	  ref.multiplicity = mult;
	  refs_u.push_back(ref); // Refinements in the u-dir.
      }
  }

  dir = YFIXED; // YDIRFIXED, refining in the y-dir (v).
  for (size_t ki = 0; ki < num_segs_v - 1; ++ki)
  {
      // We bisect the interval.
      double wgt = 0.4;
      double ref_par = wgt*unique_knots_v[ki] + (1.0 - wgt)*unique_knots_v[ki+1];

      // We insert line segments which cover order_u - 1 intervals.
      for (size_t kj = order_u - 1; kj < num_segs_u; kj += order_u)
      {
	  // To avoid a too regular pattern we alter start
	  // values. Maybe we should use a random function ...
	  double tmin = (ki%2 == 1) ? unique_knots_u[kj - order_u + 1] : unique_knots_u[kj - order_u + 2];
	  double tmax = (ki%2 == 1) ? unique_knots_u[kj] : unique_knots_u[kj + 1];

	  LRSplineSurface::Refinement2D ref;
	  ref.kval = ref_par;
	  ref.start = tmin;
	  ref.end = tmax;
	  ref.d = dir;
	  ref.multiplicity = mult;
	  refs_v.push_back(ref); // Refinements in the v-dir.
      }
  }

  vector<LRSplineSurface::Refinement2D> all_refs(refs_u.begin(), refs_u.end());
  all_refs.insert(all_refs.end(), refs_v.begin(), refs_v.end());
  // if (all_refs.size() > 0)
  //     all_refs.pop_back();
  // if (all_refs.size() > 0)
  //     all_refs.pop_back();
  // if (all_refs.size() > 0)
  //     all_refs.pop_back();
  std::cout << "num_refs_to_insert: " << all_refs.size() << std::endl;

//  all_refs.erase(all_refs.begin() + 1, all_refs.begin() + 3);
//  all_refs.erase(all_refs.begin() + 4, all_refs.end());
//  all_refs.erase(all_refs.begin(), all_refs.begin() + 2);
  std::cout << "num_refs_to_insert: " << all_refs.size() << std::endl;

  // We then create and refine the LRSplineSurface.

  lr_spline_sf_single_refs = shared_ptr<LRSplineSurface>
      // (new LRSpline<vector<double>::const_iterator, vector<double>::const_iterator> >
      (new LRSplineSurface(order_u - 1, order_v - 1,
			   num_coefs_u, num_coefs_v,
			   dim,
			   spline_sf->basis_u().begin(),
			   spline_sf->basis_v().begin(),
			   spline_sf->coefs_begin()));

  bool refine_multi = true;
  if (refine_multi)
  {
      double time_lrspline_lr_ref = benchmarkSfRefinement(*lr_spline_sf, all_refs);
      std::cout << "Time lr refinement: " << time_lrspline_lr_ref << std::endl;
      // We write to screen the number of basis functions for both
      // versions.
      int num_elem = lr_spline_sf->numElements();
      int num_basis_funcs = lr_spline_sf->numBasisFunctions();
      std::cout << "num_elem: " << num_elem << ", num_basis_funcs: " << num_basis_funcs << std::endl;

      double max_dist = maxDist(spline_sf.get(), *lr_spline_sf, num_samples_u, num_samples_v);
      std::cout << "Max dist between input and (multi) refined surface: " << max_dist << std::endl;

      std::ofstream fileout("tmp/ref_lr_multi.g2");
      lr_spline_sf->writeStandardHeader(fileout);
      lr_spline_sf->write(fileout);

      // MESSAGE("Missing writing refined surface grid to file!");
      std::ofstream lrsf_grid_ps("tmp/lrsf_grid.ps");
//  writePostscriptMesh(*lrsf);
      writePostscriptMesh(*lr_spline_sf, lrsf_grid_ps);

  }

  bool refine_single = true;
  if (refine_single)
  {
      double time_lrspline_lr_ref_single = benchmarkSfRefinement(*lr_spline_sf_single_refs, all_refs, true);
      std::cout << "Time lr refinement single refs: " << time_lrspline_lr_ref_single << std::endl;
      int num_elem_single = lr_spline_sf_single_refs->numElements();
      int num_basis_funcs_single = lr_spline_sf_single_refs->numBasisFunctions();
      std::cout << "num_elem_single_refs: " << num_elem_single <<
	  ", num_basis_funcs_single_refs: " << num_basis_funcs_single << std::endl;

#if 0//ndef NDEBUG
  vector<LRBSpline2D*> bas_funcs;
  for (auto iter = lr_spline_sf_single_refs->basisFunctionsBegin(); iter != lr_spline_sf_single_refs->basisFunctionsEnd(); ++iter)
    {
      bas_funcs.push_back((*iter).second.get());
    }
  vector<Element2D*> elems;
  for (auto iter = lr_spline_sf_single_refs->elementsBegin(); iter != lr_spline_sf_single_refs->elementsEnd(); ++iter)
  {
      elems.push_back(((*iter).second.get()));
  }
  puts("Remove when done debugging!");
#endif

      double max_dist_post_ref_multi_ref = maxDist(spline_sf.get(), *lr_spline_sf_single_refs, num_samples_u, num_samples_v);
      std::cout << "Max dist input and (single) ref surface: " << max_dist_post_ref_multi_ref << std::endl;

      std::ofstream fileout2("tmp/ref_lr_single.g2");
      lr_spline_sf_single_refs->writeStandardHeader(fileout2);
      lr_spline_sf_single_refs->write(fileout2);
  }
}

double maxDist(const SplineSurface* param_sf,
	       const LRSplineSurface& lr_spline_sf,
	       int num_samples_u, int num_samples_v)
{
    // Assuming the domain is the same.
    double umin = lr_spline_sf.startparam_u();
    double umax = lr_spline_sf.endparam_u();
    double vmin = lr_spline_sf.startparam_v();
    double vmax = lr_spline_sf.endparam_v();
    double ustep = (umax - umin)/((double)num_samples_u - 1);
    double vstep = (vmax - vmin)/((double)num_samples_v - 1);
    Point go_pt(3), lr_pt(3);
    double max_dist = -1.0;
// #ifndef NDEBUG
    double max_dist_u = 0.0;
    double max_dist_v = 0.0;
    Point max_go_pt(3), max_lr_pt(3);
// #endif
    for (int kj = 0; kj < num_samples_v; ++kj)
    {
	double vpar = vmin + kj*vstep;
	for (int ki = 0; ki < num_samples_u; ++ki)
	{
	    double upar = umin + ki*ustep;
	    param_sf->point(go_pt, upar, vpar);
	    lr_spline_sf.point(lr_pt, upar, vpar);
	    double dist = go_pt.dist(lr_pt);
	    if (dist > max_dist)
	    {
		max_dist = dist;
// #ifndef NDEBUG
		max_dist_u = upar;
		max_dist_v = vpar;
		max_go_pt = go_pt;
		max_lr_pt = lr_pt;
// #endif
	    }
	}
    }

// #ifndef NDEBUG
    std::cout << "max_dist: u = " << max_dist_u << ", v = " << max_dist_v << std::endl;
    std::cout << "max_go_pt = " << max_go_pt << std::endl;
    std::cout << "max_lr_pt = " << max_lr_pt << std::endl;
// #endif

    return max_dist;

}
<commit_msg>Added fullTensorProductSurface() (creates regular SplineSurface version through refinement).<commit_after>//===========================================================================
//
// File: benchmarkLRSurfacePointEval.C
//
// Created: Tue Jan 29 13:57:16 2013
//
// Author: Sverre Briseid <Sverre.Briseid@sintef.no>
//
// Revision: $Id$
//
// Description: Benchmark the point evaluation (with higher orders)
//              for a LRSplineSurface.  If input is a regular
//              SplineSurface, we convert to LRSplineSurface and
//              compare. If input is a LRSplineSurface, we compare
//              with the corresponding refined SplineSurface.
//
//===========================================================================

#include "GoTools/lrsplines2D/LRSplineSurface.h"
#include "GoTools/lrsplines2D/Mesh2D.h"
#include "GoTools/lrsplines2D/Direction2D.h"
#include "GoTools/lrsplines2D/LRBenchmarkUtils.h"
#include "GoTools/lrsplines2D/LRSplinePlotUtils.h"
#include "GoTools/geometry/ObjectHeader.h"
#include "GoTools/geometry/SplineSurface.h"
#include "GoTools/geometry/GeometryTools.h"
#include "GoTools/geometry/ClassType.h"
#include "GoTools/utils/config.h"

#include <iostream>
#include <assert.h>
#include <fstream>
#include <string>
#include "string.h"

using Go::ObjectHeader;
using Go::SplineSurface;
using Go::LRSplineSurface;

using namespace Go;

// Assuming the domain is the same.
double maxDist(const SplineSurface& spline_sf,
	       const LRSplineSurface& lr_spline_sf,
	       int num_samples_u, int num_samples_v);

int main(int argc, char *argv[])
{
  if (argc != 5)
  {
      std::cout << "Usage: (lr)spline_sf.g2 num_dir_samples num_derivs num_iter" << std::endl;
      return -1;
  }

  char* filein_char = argv[1];
  std::ifstream filein(argv[1]); // Input lr spline.
  // If input surface is not fine enough we refine.
  int num_dir_samples = atoi(argv[2]);
  int num_derivs = atoi(argv[3]);
  int num_iter = atoi(argv[4]);
  if (num_iter != 1)
      puts("Not using num_iter yet.");

  shared_ptr<SplineSurface> spline_sf;
  shared_ptr<LRSplineSurface> lr_spline_sf;
  double knot_tol = 1e-10;

  // If we refine the lr-surface in order to extract a regular
  // spline-version we must perform the operations on a copy.
  shared_ptr<LRSplineSurface> lr_spline_sf_copy;

  int order_u, order_v, num_coefs_u, num_coefs_v, dim, num_bases=-1;
  if (strcasestr(filein_char, ".g2"))
    {
      ObjectHeader header;
      filein >> header;
      if (header.classType() == Class_SplineSurface)
	{
	  spline_sf = shared_ptr<SplineSurface>(new SplineSurface());
	  filein >> *spline_sf;
	  // We create the lr-version.
	  lr_spline_sf = shared_ptr<LRSplineSurface>(new LRSplineSurface(spline_sf.get(), knot_tol));
	}
      else if (header.classType() == Class_LRSplineSurface)
	{
	  lr_spline_sf = shared_ptr<LRSplineSurface>(new LRSplineSurface());
	  filein >> *spline_sf;
	  lr_spline_sf_copy = shared_ptr<LRSplineSurface>(lr_spline_sf->clone());

	}
      else
	{
	  std::cout << "Input was not a SplineSurface or a LRSplineSurface, exiting!" << std::endl;
	}
    }
  else
    {
      MESSAGE("Input was not a g2-file!");
      return -1;
    }

  double max_dist = maxDist(*spline_sf, *lr_spline_sf, num_dir_samples, num_dir_samples);

  std::cout << "Max dist: " << max_dist << std::endl;

}

double maxDist(const SplineSurface& spline_sf,
	       const LRSplineSurface& lr_spline_sf,
	       int num_samples_u, int num_samples_v)
{
    // Assuming the domain is the same.
    double umin = lr_spline_sf.startparam_u();
    double umax = lr_spline_sf.endparam_u();
    double vmin = lr_spline_sf.startparam_v();
    double vmax = lr_spline_sf.endparam_v();
    double ustep = (umax - umin)/((double)num_samples_u - 1);
    double vstep = (vmax - vmin)/((double)num_samples_v - 1);
    Point go_pt(3), lr_pt(3);
    double max_dist = -1.0;
// #ifndef NDEBUG
    double max_dist_u = 0.0;
    double max_dist_v = 0.0;
    Point max_go_pt(3), max_lr_pt(3);
// #endif
    for (int kj = 0; kj < num_samples_v; ++kj)
    {
	double vpar = vmin + kj*vstep;
	for (int ki = 0; ki < num_samples_u; ++ki)
	{
	    double upar = umin + ki*ustep;
	    spline_sf.point(go_pt, upar, vpar);
	    lr_spline_sf.point(lr_pt, upar, vpar);
	    double dist = go_pt.dist(lr_pt);
	    if (dist > max_dist)
	    {
		max_dist = dist;
// #ifndef NDEBUG
		max_dist_u = upar;
		max_dist_v = vpar;
		max_go_pt = go_pt;
		max_lr_pt = lr_pt;
// #endif
	    }
	}
    }

// #ifndef NDEBUG
    std::cout << "max_dist: u = " << max_dist_u << ", v = " << max_dist_v << std::endl;
    std::cout << "max_go_pt = " << max_go_pt << std::endl;
    std::cout << "max_lr_pt = " << max_lr_pt << std::endl;
// #endif

    return max_dist;

}

<|endoftext|>