void FootstepMarker::processMenuFeedback(uint8_t menu_entry_id) {
switch (menu_entry_id) {
case 1: { // look ground
lookGround();
break;
}
case 2: { // execute
executeCB(std_msgs::Empty::ConstPtr());
break;
}
case 3: { // replan
planIfPossible();
break;
}
case 4: { // cancel walk
cancelWalk();
break;
}
case 5: { // toggle 6dof marker
show_6dof_control_ = !show_6dof_control_;
break;
}
case 6: { // toggle 6dof marker
changePlannerHeuristic(":straight-heuristic");
break;
}
case 7: { // toggle 6dof marker
changePlannerHeuristic(":stepcost-heuristic**");
break;
}
case 8: { // toggle 6dof marker
lleg_first_ = true;
break;
}
case 9: { // toggle 6dof marker
lleg_first_ = false;
break;
}
default: {
break;
}
}
}
// thread that calls executeCB
void ROSAction::executionThread()
{
ros::Rate r(EXECUTION_FREQUENCY);
ros::Time t0 = start_time_ = ros::Time::now();
// while (as_.isPreemptRequested() || ros::ok())
while (is_active() && ros::ok())
{
std::cout << "execution_thread_.get_id()" << execution_thread_.get_id() << std::endl;
bool active = timeout_check(); // check if tick was received
std::cout << "im active: " << active << std::endl;
{
boost::lock_guard<boost::mutex> lock(mutex_active_);
active_ = active = active ? active_ : false;
}
if (active)
{
ros::Duration dt = ros::Time::now() - t0;
t0 = ros::Time::now();
std::cout << "executing cb" << dt << std::endl;
if (executeCB(dt)) // execute user personal code if finished exit fast
{
std::cout << "callback returned!" << std::endl;
break;
}
send_feedback();
}
else
{
std::cout << "executionThread not active" << std::endl;
}
r.sleep(); // wait to match frequency
}
std::cout << "About to Destroy Thread" << std::endl;
std::cout << "----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------" << std::endl;
// set_feedback(NODE_ERROR);
send_feedback();
stop();
}
