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(); }