manual_node.cpp

Go to the documentation of this file.

/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
 * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
*
*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*
*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include "behaviortree_cpp_v3/controls/manual_node.h"
#include "behaviortree_cpp_v3/action_node.h"
#include <ncurses.h>

namespace BT
{


ManualSelectorNode::ManualSelectorNode(const std::string& name, const NodeConfiguration& config)
    : ControlNode::ControlNode(name, config )
  , running_child_idx_(-1)
  , previously_executed_idx_(-1)
{
    setRegistrationID("ManualSelector");
}

void ManualSelectorNode::halt()
{
    if( running_child_idx_ >= 0 )
    {
        haltChild( size_t(running_child_idx_) );
    }
    running_child_idx_ = -1;
    ControlNode::halt();
}

NodeStatus ManualSelectorNode::tick()
{
    const size_t children_count = children_nodes_.size();

    if( children_count == 0 )
    {
        return selectStatus();
    }

    bool repeat_last = false;
    getInput(REPEAT_LAST_SELECTION, repeat_last);

    int idx = 0;

    if( repeat_last && previously_executed_idx_ >= 0)
    {
        idx = previously_executed_idx_;
    }
    else{
        setStatus(NodeStatus::RUNNING);
        idx = selectChild();
        previously_executed_idx_ = idx;

        if( idx == NUM_SUCCESS ){
            return NodeStatus::SUCCESS;
        }
        if( idx == NUM_FAILURE ){
            return NodeStatus::FAILURE;
        }
        if( idx == NUM_RUNNING ){
            return NodeStatus::RUNNING;
        }
    }

    NodeStatus ret = children_nodes_[idx]->executeTick();
    if(ret == NodeStatus::RUNNING)
    {
        running_child_idx_ = idx;
    }
    return ret;
}

NodeStatus ManualSelectorNode::selectStatus() const
{
    WINDOW *win;
    initscr();
    cbreak();

    win = newwin( 6, 70, 1, 1 ); // create a new window

    mvwprintw( win, 0, 0, "No children." );
    mvwprintw( win, 1, 0, "Press: S to return SUCCESFULL," );
    mvwprintw( win, 2, 0, "       F to return FAILURE, or" );
    mvwprintw( win, 3, 0, "       R to return RUNNING." );

    wrefresh( win ); // update the terminal screen
    noecho(); // disable echoing of characters on the screen
    keypad( win, TRUE ); // enable keyboard input for the window.
    curs_set( 0 ); // hide the default screen cursor.

    int ch = 0;
    NodeStatus ret;
    while(1)
    {
        if( ch == 's' || ch == 'S')
        {
            ret = NodeStatus::SUCCESS;
            break;
        }
        else if( ch == 'f' || ch == 'F')
        {
            ret = NodeStatus::FAILURE;
            break;
        }
        else if( ch == 'r' || ch == 'R')
        {
            ret = NodeStatus::RUNNING;
            break;
        }
        ch = wgetch(win);
    }
    werase( win ) ;
    wrefresh( win );
    delwin( win );
    endwin();
    return ret;
}

uint8_t ManualSelectorNode::selectChild() const
{
    const size_t children_count = children_nodes_.size();

    std::vector<std::string> list;
    list.reserve(children_count);
    for(const auto& child: children_nodes_)
    {
        list.push_back(child->name());
    }

    size_t width = 10;
    for(const auto& str: list) {
        width = std::max(width, str.size()+2);
    }

    WINDOW *win;
    initscr();
    cbreak();

    win = newwin( children_count+6, 70, 1, 1 ); // create a new window

    mvwprintw( win, 0, 0, "Use UP/DOWN arrow to select the child, Enter to confirm." );
    mvwprintw( win, 1, 0, "Press: S to skip and return SUCCESFULL," );
    mvwprintw( win, 2, 0, "       F to skip and return FAILURE, or" );
    mvwprintw( win, 3, 0, "       R to skip and return RUNNING." );

    // now print all the menu items and highlight the first one
    for(size_t i=0; i<list.size(); i++ )
    {
        mvwprintw( win, i+5, 0, "%2d. %s", i+1, list[i].c_str() );
    }

    wrefresh( win ); // update the terminal screen
    noecho(); // disable echoing of characters on the screen
    keypad( win, TRUE ); // enable keyboard input for the window.
    curs_set( 0 ); // hide the default screen cursor.

    uint8_t row = 0;
    int ch=0;
    while(1)
    {
        // right pad with spaces to make the items appear with even width.
        wattroff( win, A_STANDOUT );
        mvwprintw( win, row+5, 4, "%s", list[row].c_str() );
        // use a variable to increment or decrement the value based on the input.
        if( ch == KEY_DOWN )
        {
            row = (row == children_count-1) ? 0 : row+1;
        }
        else if( ch == KEY_UP )
        {
            row = ( row == 0) ? (children_count-1) : row-1;
        }
        else if( ch == KEY_ENTER || ch == 10 )
        {
            break;
        }
        else if( ch == 's' || ch == 'S')
        {
            row = NUM_SUCCESS;
            break;
        }
        else if( ch == 'f' || ch == 'F')
        {
            row = NUM_FAILURE;
            break;
        }
        else if( ch == 'r' || ch == 'R')
        {
            row = NUM_RUNNING;
            break;
        }

        // now highlight the next item in the list.
        wattron( win, A_STANDOUT );
        mvwprintw( win, row+5, 4, "%s", list[row].c_str());
        ch = wgetch(win);
    };

    werase( win ) ;
    wrefresh( win );
    delwin( win );
    endwin();
    return row;
}

}