Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit 4549d9e

Browse files
daf@blue-ocean-robotics.comdaf@blue-ocean-robotics.com
[email protected]
authored and
[email protected]
committed
adding ManualSelector (enhancement BehaviorTree#174)
1 parent c34a79b commit 4549d9e

File tree

7 files changed

+307
-0
lines changed

7 files changed

+307
-0
lines changed

‎CMakeLists.txt

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -178,6 +178,16 @@ list(APPEND BT_SOURCE
178178
3rdparty/minitrace/minitrace.cpp
179179
)
180180

181+
find_package(Curses QUIET)
182+
183+
if(CURSES_FOUND)
184+
list(APPEND BT_SOURCE
185+
src/controls/manual_node.cpp
186+
)
187+
endif()
188+
list(APPEND BEHAVIOR_TREE_EXTERNAL_LIBRARIES ${CURSES_LIBRARIES})
189+
190+
181191
######################################################
182192

183193
if (UNIX)

‎examples/CMakeLists.txt

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,3 +46,8 @@ target_link_libraries(t10_include_trees ${BEHAVIOR_TREE_LIBRARY} bt_sample_node
4646

4747
add_executable(t11_runtime_ports t11_runtime_ports.cpp )
4848
target_link_libraries(t11_runtime_ports ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes )
49+
50+
if(CURSES_FOUND)
51+
add_executable(t12_ncurses_manual_selector t12_ncurses_manual_selector.cpp )
52+
target_link_libraries(t12_ncurses_manual_selector ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes )
53+
endif()

‎examples/t12_ncurses_manual_selector.cpp

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
#include "behaviortree_cpp_v3/bt_factory.h"
2+
#include "dummy_nodes.h"
3+
4+
using namespace BT;
5+
6+
// clang-format off
7+
static const char* xml_text = R"(
8+
<root main_tree_to_execute = "MainTree" >
9+
<BehaviorTree ID="MainTree">
10+
<ManualSelector name="root">
11+
<SaySomething name="Option1" message="Option1" />
12+
<SaySomething name="Option2" message="Option2" />
13+
<SaySomething name="Option3" message="Option3" />
14+
<SaySomething name="Option4" message="Option4" />
15+
<ManualSelector name="YouChoose" />
16+
</ManualSelector>
17+
</BehaviorTree>
18+
</root>
19+
)";
20+
// clang-format on
21+
22+
int main()
23+
{
24+
BehaviorTreeFactory factory;
25+
factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
26+
27+
auto tree = factory.createTreeFromText(xml_text);
28+
auto ret = tree.tickRoot();
29+
30+
std::cout << "Result: " << ret << std::endl;
31+
32+
return 0;
33+
}
34+
35+

‎include/behaviortree_cpp_v3/behavior_tree.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include "behaviortree_cpp_v3/controls/sequence_node.h"
2222
#include "behaviortree_cpp_v3/controls/sequence_star_node.h"
2323
#include "behaviortree_cpp_v3/controls/switch_node.h"
24+
#include "behaviortree_cpp_v3/controls/manual_node.h"
2425

2526
#include "behaviortree_cpp_v3/action_node.h"
2627
#include "behaviortree_cpp_v3/condition_node.h"
@@ -39,6 +40,7 @@
3940
#include "behaviortree_cpp_v3/decorators/blackboard_precondition.h"
4041
#include "behaviortree_cpp_v3/decorators/timeout_node.h"
4142

43+
4244
namespace BT
4345
{
4446

‎include/behaviortree_cpp_v3/controls/manual_node.h

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
/* Copyright (C) 2020 Davide Faconti - All Rights Reserved
2+
*
3+
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
4+
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
5+
* 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:
6+
* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
7+
*
8+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
9+
* 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,
10+
* 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.
11+
*/
12+
13+
#ifndef MANUAL_SELECTION_NODE_H
14+
#define MANUAL_SELECTION_NODE_H
15+
16+
#include "behaviortree_cpp_v3/control_node.h"
17+
18+
namespace BT
19+
{
20+
/**
21+
* @brief Use a Terminal User Interface (ncurses) to select a certain child manually.
22+
*/
23+
class ManualSelectorNode : public ControlNode
24+
{
25+
public:
26+
ManualSelectorNode(const std::string& name);
27+
28+
virtual ~ManualSelectorNode() override = default;
29+
30+
virtual void halt() override;
31+
32+
private:
33+
34+
virtual BT::NodeStatus tick() override;
35+
int running_child_idx_;
36+
37+
enum NumericarStatus{
38+
NUM_SUCCESS = 253,
39+
NUM_FAILURE = 254,
40+
NUM_RUNNING = 255,
41+
};
42+
43+
NodeStatus selectStatus() const;
44+
45+
uint8_t selectChild() const;
46+
};
47+
48+
}
49+
50+
#endif // MANUAL_SELECTION_NODE_H

‎src/bt_factory.cpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,8 @@ BehaviorTreeFactory::BehaviorTreeFactory()
5555
registerNodeType<SwitchNode<5>>("Switch5");
5656
registerNodeType<SwitchNode<6>>("Switch6");
5757

58+
registerNodeType<ManualSelectorNode>("ManualSelector");
59+
5860
for( const auto& it: builders_)
5961
{
6062
builtin_IDs_.insert( it.first );

‎src/controls/manual_node.cpp

Lines changed: 203 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,203 @@
1+
/* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved
2+
* Copyright (C) 2018-2020 Davide Faconti, Eurecat - All Rights Reserved
3+
*
4+
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
5+
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
6+
* 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:
7+
* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
8+
*
9+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
10+
* 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,
11+
* 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.
12+
*/
13+
14+
#include "behaviortree_cpp_v3/controls/manual_node.h"
15+
#include "behaviortree_cpp_v3/action_node.h"
16+
#include <ncurses.h>
17+
18+
namespace BT
19+
{
20+
21+
22+
ManualSelectorNode::ManualSelectorNode(const std::string& name)
23+
: ControlNode::ControlNode(name, {} )
24+
, running_child_idx_(-1)
25+
{
26+
setRegistrationID("ManualSelector");
27+
}
28+
29+
void ManualSelectorNode::halt()
30+
{
31+
if( running_child_idx_ >= 0 )
32+
{
33+
haltChild( size_t(running_child_idx_) );
34+
}
35+
running_child_idx_ = -1;
36+
ControlNode::halt();
37+
}
38+
39+
NodeStatus ManualSelectorNode::tick()
40+
{
41+
const size_t children_count = children_nodes_.size();
42+
43+
if( children_count == 0 )
44+
{
45+
return selectStatus();
46+
}
47+
48+
setStatus(NodeStatus::RUNNING);
49+
50+
unsigned idx = selectChild();
51+
52+
if( idx == NUM_SUCCESS ){
53+
return NodeStatus::SUCCESS;
54+
}
55+
if( idx == NUM_FAILURE ){
56+
return NodeStatus::FAILURE;
57+
}
58+
if( idx == NUM_RUNNING ){
59+
return NodeStatus::RUNNING;
60+
}
61+
62+
NodeStatus ret = children_nodes_[idx]->executeTick();
63+
64+
if(ret == NodeStatus::RUNNING)
65+
{
66+
running_child_idx_ = idx;
67+
}
68+
return ret;
69+
}
70+
71+
NodeStatus ManualSelectorNode::selectStatus() const
72+
{
73+
WINDOW *win;
74+
initscr();
75+
cbreak();
76+
77+
win = newwin( 6, 70, 1, 1 ); // create a new window
78+
79+
mvwprintw( win, 0, 0, "No children." );
80+
mvwprintw( win, 1, 0, "Press: S to return SUCCESFULL," );
81+
mvwprintw( win, 2, 0, " F to return FAILURE, or" );
82+
mvwprintw( win, 3, 0, " R to return RUNNING." );
83+
84+
wrefresh( win ); // update the terminal screen
85+
noecho(); // disable echoing of characters on the screen
86+
keypad( win, TRUE ); // enable keyboard input for the window.
87+
curs_set( 0 ); // hide the default screen cursor.
88+
89+
int ch = 0;
90+
NodeStatus ret;
91+
while(1)
92+
{
93+
if( ch == 's' || ch == 'S')
94+
{
95+
ret = NodeStatus::SUCCESS;
96+
break;
97+
}
98+
else if( ch == 'f' || ch == 'F')
99+
{
100+
ret = NodeStatus::FAILURE;
101+
break;
102+
}
103+
else if( ch == 'r' || ch == 'R')
104+
{
105+
ret = NodeStatus::RUNNING;
106+
break;
107+
}
108+
ch = wgetch(win);
109+
}
110+
werase( win ) ;
111+
wrefresh( win );
112+
delwin( win );
113+
endwin();
114+
return ret;
115+
}
116+
117+
uint8_t ManualSelectorNode::selectChild() const
118+
{
119+
const size_t children_count = children_nodes_.size();
120+
std::vector<std::string> list;
121+
list.reserve(children_count);
122+
for(const auto& child: children_nodes_)
123+
{
124+
list.push_back(child->name());
125+
}
126+
127+
size_t width = 10;
128+
for(const auto& str: list) {
129+
width = std::max(width, str.size()+2);
130+
}
131+
132+
WINDOW *win;
133+
initscr();
134+
cbreak();
135+
136+
win = newwin( children_count+6, 70, 1, 1 ); // create a new window
137+
138+
mvwprintw( win, 0, 0, "Use UP/DOWN arrow to select the child, Enter to confirm." );
139+
mvwprintw( win, 1, 0, "Press: S to skip and return SUCCESFULL," );
140+
mvwprintw( win, 2, 0, " F to skip and return FAILURE, or" );
141+
mvwprintw( win, 3, 0, " R to skip and return RUNNING." );
142+
143+
// now print all the menu items and highlight the first one
144+
for(size_t i=0; i<list.size(); i++ )
145+
{
146+
mvwprintw( win, i+5, 0, "%2d. %s", i+1, list[i].c_str() );
147+
}
148+
149+
wrefresh( win ); // update the terminal screen
150+
noecho(); // disable echoing of characters on the screen
151+
keypad( win, TRUE ); // enable keyboard input for the window.
152+
curs_set( 0 ); // hide the default screen cursor.
153+
154+
uint8_t row = 0;
155+
int ch=0;
156+
while(1)
157+
{
158+
// right pad with spaces to make the items appear with even width.
159+
wattroff( win, A_STANDOUT );
160+
mvwprintw( win, row+5, 4, "%s", list[row].c_str() );
161+
// use a variable to increment or decrement the value based on the input.
162+
if( ch == KEY_DOWN )
163+
{
164+
row = (row == children_count-1) ? 0 : row+1;
165+
}
166+
else if( ch == KEY_UP )
167+
{
168+
row = ( row == 0) ? : row-1;
169+
}
170+
else if( ch == KEY_ENTER || ch == 10 )
171+
{
172+
break;
173+
}
174+
else if( ch == 's' || ch == 'S')
175+
{
176+
row = NUM_SUCCESS;
177+
break;
178+
}
179+
else if( ch == 'f' || ch == 'F')
180+
{
181+
row = NUM_FAILURE;
182+
break;
183+
}
184+
else if( ch == 'r' || ch == 'R')
185+
{
186+
row = NUM_RUNNING;
187+
break;
188+
}
189+
190+
// now highlight the next item in the list.
191+
wattron( win, A_STANDOUT );
192+
mvwprintw( win, row+5, 4, "%s", list[row].c_str());
193+
ch = wgetch(win);
194+
};
195+
196+
werase( win ) ;
197+
wrefresh( win );
198+
delwin( win );
199+
endwin();
200+
return row;
201+
}
202+
203+
}

0 commit comments

Comments
 (0)