STATE NodeSequenceStar::execute() {
set_highlighted(true);
std::cout << "Executing Sequence Star" << std::endl;
if (current_running_child_ == NULL) {
current_running_child_ = first_child_;
}
exec_child_ = current_running_child_;
do {
child_status_ = exec_child_->execute();
set_highlighted(false);
if (child_status_ == NODE_ERROR) {
current_running_child_ = exec_child_;
return node_status_ = NODE_ERROR;
} else if (child_status_ == RUNNING) {
current_running_child_ = exec_child_;
return node_status_ = RUNNING;
} else if (child_status_ == FAILURE) {
current_running_child_ = NULL;
return node_status_ = FAILURE;
}
exec_child_ = exec_child_->get_next_brother();
} while (exec_child_ != NULL);
current_running_child_ = NULL;
return node_status_ = SUCCESS;
}
STATE NodeParallel::execute() {
set_highlighted(true);
int number_failure = 0;
int number_success = 0;
int number_error = 0;
std::cout << "Executing Parallel" << std::endl;
exec_child_ = first_child_;
for (int i = 0; i < number_children_; i++) {
child_status_ = exec_child_->execute();
set_highlighted(false);
if (child_status_ == NODE_ERROR)
number_error++;
else if (child_status_ == FAILURE)
number_failure++;
else if (child_status_ == SUCCESS)
number_success++;
exec_child_ = exec_child_->get_next_brother();
}
if (number_error > 0)
return node_status_ = NODE_ERROR;
else if (number_success >= number_children_ / 2)
return node_status_ = SUCCESS;
else if (number_failure >= number_children_ / 2)
return node_status_ = FAILURE;
else
return node_status_ = RUNNING;
}
STATE NodeROS::execute()
{
set_highlighted(true);
glut_process();
std::cout << "NodeROS::execute()" << std::endl;
if (overwritten_)
{
set_highlighted(false);
return node_status_ = FAILURE; //overwritten_result_;
}
else
{
bool finished;
// received_ = false;
{
boost::lock_guard<boost::mutex> lock(mutex_finished_);
finished = finished_;
}
ROS_INFO("RECEIVED: %d", received_);
if (!finished && !received_)
{
std::cout << "Sending Goal Client: "
<< ros_node_name_ << std::endl;
behavior_trees::ROSGoal goal;
goal.GOAL_ = 1; // possitive tick
{
boost::lock_guard<boost::mutex> lock(mutex_active_);
active_ = false;
}
ac_.sendGoal(goal,
boost::bind(&NodeROS::doneCb, this, _1, _2),
boost::bind(&NodeROS::activeCb, this),
boost::bind(&NodeROS::feedbackCb, this, _1));
std::cout << "Waiting for Feedback at Node: " << this << std::endl;
while (!received_ && !active_)
{
sleep(0.01);
// std::cout << "*";
}
std::cout << "Received Feedback at Node: " << this << std::endl;
}
{
boost::lock_guard<boost::mutex> lock(mutex_received_);
received_ = false;
}
{
boost::lock_guard<boost::mutex> lock(mutex_node_status_);
std::cout << "STATUS: " << node_status_ << std::endl;
set_highlighted(false);
return node_status_;
}
}
}
STATE NodeRoot::execute() {
// there is no need to reset status because the nodes should
// have a timeout to become NODE_ERROR after a while, if they
// don't receive a feedback from the server.
// execute_reset_status();
set_highlighted(true);
std::cout << "---------- Executing Root ----------" << std::endl;
return child_status_ = first_child_->execute();
set_highlighted(false);
}
bool Button::on_touch(const TouchEvent event) {
switch(event.type) {
case TouchEvent::Type::Start:
set_highlighted(true);
set_dirty();
return true;
case TouchEvent::Type::End:
set_highlighted(false);
set_dirty();
if( on_select ) {
on_select(*this);
}
return true;
default:
return false;
}
#if 0
switch(event.type) {
case TouchEvent::Type::Start:
flags.highlighted = true;
set_dirty();
return true;
case TouchEvent::Type::Move:
{
const bool new_highlighted = screen_rect().contains(event.point);
if( flags.highlighted != new_highlighted ) {
flags.highlighted = new_highlighted;
set_dirty();
}
}
return true;
case TouchEvent::Type::End:
if( flags.highlighted ) {
flags.highlighted = false;
set_dirty();
if( on_select ) {
on_select(*this);
}
}
return true;
default:
return false;
}
#endif
}
STATE NodeSequence::execute() {
set_highlighted(true);
std::cout << "Executing Sequence" << std::endl;
exec_child_ = first_child_;
for (int i = 0; i < number_children_; i++) {
child_status_ = exec_child_->execute();
set_highlighted(false);
if (child_status_ == NODE_ERROR)
return node_status_ = NODE_ERROR;
else if (child_status_ == RUNNING)
return node_status_ = RUNNING;
else if (child_status_ == FAILURE)
return node_status_ = FAILURE;
exec_child_ = exec_child_->get_next_brother();
}
return node_status_ = SUCCESS;
}
bool ImageButton::on_touch(const TouchEvent event) {
switch(event.type) {
case TouchEvent::Type::Start:
set_highlighted(true);
set_dirty();
return true;
case TouchEvent::Type::End:
set_highlighted(false);
set_dirty();
if( on_select ) {
on_select(*this);
}
return true;
default:
return false;
}
}
STATE NodeSelector::execute() {
set_highlighted(true);
std::cout << "Executing Selector" << std::endl;
exec_child_ = first_child_;
for (int i = 0; i < number_children_; i++) {
std::cout << "ticking child: " << i << std::endl;
child_status_ = exec_child_->execute();
set_highlighted(false);
std::cout << "child status for comparison: " << child_status_ << std::endl;
if (child_status_ == NODE_ERROR)
return node_status_ = NODE_ERROR;
else if (child_status_ == RUNNING)
return node_status_ = RUNNING;
else if (child_status_ == SUCCESS)
return node_status_ = SUCCESS;
std::cout << "pointing exec_child_ to next brother" << std::endl;
exec_child_ = exec_child_->get_next_brother();
}
return node_status_ = FAILURE;
}
bool MenuView::on_key(const KeyEvent key) {
switch(key) {
case KeyEvent::Up:
return set_highlighted(highlighted() - 1);
case KeyEvent::Down:
return set_highlighted(highlighted() + 1);
case KeyEvent::Select:
case KeyEvent::Right:
item_view(highlighted())->select();
return true;
case KeyEvent::Left:
if( on_left ) {
on_left();
}
return true;
default:
return false;
}
}
STATE NodeCondition::execute() {
set_highlighted(true);
std::cout << "Executing Condition" << std::endl;
if (relation_.compare("="))
std::cout << "it's equality" << std::endl;
unsigned int idx = 0;
for (std::vector<std::string>::iterator it = global_varname.begin();
it != global_varname.end(); ++it) {
if (*it == varlabel_) {
std::cout << "found match " << idx << std::endl;
break;
}
idx++;
}
double val = global_varvalue.at(idx);
std::cout << "val" << val << std::endl;
set_highlighted(false);
switch (relation_.at(0)) {
case '=':
return node_status_ = (val == std::stod(constant_)) ? SUCCESS : FAILURE;
break;
case '>':
return node_status_ = (val >= std::stod(constant_)) ? SUCCESS : FAILURE;
break;
case '<':
return node_status_ = (val <= std::stod(constant_)) ? SUCCESS : FAILURE;
break;
default:
std::cout << "relation not implemented (choose =, >, <)" << std::endl;
break;
}
return node_status_ = NODE_ERROR;
}
void MenuItemView::unhighlight() {
set_highlighted(false);
set_dirty();
}
void MenuItemView::highlight() {
set_highlighted(true);
set_dirty();
}
bool MenuView::on_encoder(const EncoderEvent event) {
set_highlighted(highlighted() + event);
return true;
}
STATE NodeDecorator::execute() {
set_highlighted(true);
std::cout << "Executing Decorator" << std::endl;
exec_child_ = first_child_;
child_status_ = exec_child_->execute();
set_highlighted(false);
if (child_status_ == SUCCESS) {
unsigned int idx = 0;
for (std::vector<std::string>::iterator it = global_varname.begin();
it != global_varname.end(); ++it) {
if (*it == prev_status_)
break;
idx++;
}
// prev_status = success
global_varvalue[idx] = 0;
std::cout << "global_varvalue" << global_varvalue[idx] << std::endl;
idx = 0;
for (std::vector<std::string>::iterator it = global_varname.begin();
it != global_varname.end(); ++it) {
if (*it == curr_state_)
break;
idx++;
}
// curr_state = next_state
global_varvalue[idx] = std::stod(next_state_);
std::cout << "global_varvalue" << global_varvalue[idx] << std::endl;
return node_status_ = SUCCESS;
} else if (child_status_ == FAILURE) {
unsigned int idx = 0;
for (std::vector<std::string>::iterator it = global_varname.begin();
it != global_varname.end(); ++it) {
if (*it == prev_status_)
break;
idx++;
}
// prev_status = failure
global_varvalue[idx] = 1;
std::cout << "global_varvalue" << global_varvalue[idx] << std::endl;
idx = 0;
for (std::vector<std::string>::iterator it = global_varname.begin();
it != global_varname.end(); ++it) {
if (*it == curr_state_)
break;
idx++;
}
// curr_state = next_state
global_varvalue[idx] = std::stod(next_state_);
std::cout << "global_varvalue" << global_varvalue[idx] << std::endl;
return node_status_ = SUCCESS;
} else if (child_status_ == RUNNING)
return node_status_ = RUNNING;
else
return node_status_ = NODE_ERROR;
}
