bool PauseMenu::init()
{
m_type = PAUSE;
m_numChoices = 3;
unsigned num_p = 1;
sf::Vector2f texCoord, size, pos;
std::ifstream fin;
fin.open("config/pausemenu.txt");
if (!fin) return false;
m_pause = std::vector<sf::Vertex>((num_p + m_numChoices)*4, sf::Vertex());
m_pauseLit = m_pauseDim = std::vector<sf::Vertex>(m_numChoices*4, sf::Vertex());
for (unsigned i = 0; i < num_p && util::read3v2f(fin, texCoord, size, pos, true); i++) {
util::affixTexture(&m_pause[i*4], texCoord, size);
util::affixPos(&m_pause[i*4], pos, size, 0);
}
for (unsigned i = 0; i < m_numChoices && util::read3v2f(fin, texCoord, size, pos, true); i++) {
util::affixTexture(&m_pauseLit[i*4], texCoord, size);
util::affixPos(&m_pauseLit[i*4], pos, size, 0);
util::read3v2f(fin, texCoord, size, pos, true);
util::affixTexture(&m_pauseDim[i*4], texCoord, size);
util::affixPos(&m_pauseDim[i*4], pos, size, 0);
}
m_menu = &m_pause[num_p*4];
fin.close();
for (unsigned i = 0; i < m_numChoices; i++) util::copySprite(&m_pauseDim[i*4], &m_pause[(num_p+i)*4]);
m_choice = 1;
selectChoice(1);
return true;
}
AppLayer::Status PauseMenu::tick(std::vector<sf::Event> &e, const sf::Time &t, sf::Vector2f m)
{
for (unsigned i = 0; i < e.size(); i++) {
//Mouse presses
if (e[i].type == sf::Event::MouseButtonReleased) {
if (e[i].mouseButton.button == sf::Mouse::Left) {
float x = (float)e[i].mouseButton.x;
float y = (float)e[i].mouseButton.y;
unsigned opt = translateOption(x, y);
if (!opt) continue;
processChoice();
}
else if (e[i].mouseButton.button == sf::Mouse::Right) {
Layer::back();
break;
}
}
//Mouse movement
else if (e[i].type == sf::Event::MouseMoved) {
float x = (float)e[i].mouseMove.x;
float y = (float)e[i].mouseMove.y;
unsigned opt = translateOption(x, y);
if (!opt) continue;
selectChoice(opt);
}
//Keyboard events
else if (e[i].type == sf::Event::KeyPressed) {
bool up = controller.pressing(GameController::K_UP, e[i].key.code);
bool down = controller.pressing(GameController::K_DOWN, e[i].key.code);
bool enter = controller.pressing(GameController::K_ENTER, e[i].key.code);
bool esc = controller.pressing(GameController::K_ESCAPE, e[i].key.code);
if (down) selectChoice(m_choice + 1);
else if (up) selectChoice(m_choice - 1);
else if (enter) processChoice();
else if (esc) {
Layer::back();
break;
}
}
}
return AppLayer::HALT;
}
void getAdminChoice(){
int ch;
while(1){
printf("Please Select Your Option\n");
printf("1. Delete Employee Data\n");
printf("2. HR Mangement\n3. Purchasing Department\n4. Switch to Main Menu \n");
scanf("%d",&ch);
switch(ch){
case 1: deleteEmp();
break;
case 2: //HR Management
break;
case 3: //Purchasing Department
break;
case 4 : selectChoice();
break;
default:
break;
}
}
}
bool detection::GraspPointDetector::doProcessing()
{
ROS_INFO_ONCE("'Do Processing' called!");
clock_t begin = clock();
geometry_msgs::PoseStamped initial_pose = r_arm_group_.getCurrentPose(r_arm_group_.getEndEffectorLink());
/** COMPUTE BOUNDINGBOX AND PUBLISH A TF FRAME **/
boost::mutex::scoped_lock bounding_box_lock(update_bounding_box_mutex_);
obj_bounding_box_->computeAndPublish(world_obj_, table_plane_, tf_broadcaster);
planar_obj_ = obj_bounding_box_->getPlanarObj();
draw_bounding_box_ = true;
/** SAMPLING GRASP POSITIONS **/
sampler.configure(obj_bounding_box_);
// Find all the samples positions
// We call grasp filter to make a pre-filtering step, to avoid generate clearly unfeasible grasps samples
if(!sampler.generateSamples(obj_bounding_box_, &GraspFilter::generateSideSamples, &GraspFilter::generateTopSamples))
{
bounding_box_lock.unlock();
return false;
}
draw_sampled_grasps_ = true;
bounding_box_lock.unlock();
pub_samples_.publish(sampler.getSamples());
ROS_INFO("[%g ms] Grasping sampling", durationMillis(begin));
/** FILTERING UNFEASIBLE GRASPS **/
// We need the table bounding box to create a collision object in moveit
table_bounding_box_->buildPlanar(table_cloud_);
try
{
begin = clock();
// Configure filter
grasp_filter_->configure(sampler.getSideGraspHeight(), sampler.getTopGraspHeight(), obj_bounding_box_,
table_bounding_box_);
// Remove infeasible ones
sampler.getTopGrasps()->clear(); //TODO: REMOVE THIS!!!
grasp_filter_->filterGraspingPoses(sampler.getSideGrasps(), sampler.getTopGrasps());
ROS_INFO("[%g ms] Grasping filtering", durationMillis(begin));
}
catch (ComputeFailedException ex)
{
ROS_WARN("%s", ex.what());
return false;
}
if (!grasp_filter_->feasibleGrasps()) return false;
/** GRASP RANKING **/
begin = clock();
grasp_ranker_.configure(obj_bounding_box_, grasp_filter_->getEndEffectorLink(), grasp_filter_->getShoulderLink());
grasp_ranker_.rankGraspingPoses(grasp_filter_->getSideGrasps(), grasp_filter_->getTopGrasps());
moveit_msgs::Grasp grasp_winner = grasp_ranker_.getSelectedGrasp();
ROS_INFO("[%g ms] Grasping ranking", durationMillis(begin));
// Show winner plan
pick(r_arm_group_, GRASPABLE_OBJECT(), grasp_winner, true);
if (selectChoice("Ready: 1=Pick, Any=Again") != 1) return false;
/** ACTUAL PICKING **/
begin = clock();
r_arm_group_.setPlanningTime(25.0);
r_arm_group_.setSupportSurfaceName(SUPPORT_TABLE());
bool result = pick(r_arm_group_, GRASPABLE_OBJECT(), grasp_winner, false);
ROS_INFO("[%g ms] Pick movement", durationMillis(begin));
r_arm_group_.setPoseTarget(initial_pose, r_arm_group_.getEndEffectorLink());
moveit::planning_interface::MoveGroup::Plan plan;
bool success = r_arm_group_.plan(plan);
if (success) r_arm_group_.move();
else ROS_INFO("Unable to return no initial position. No plan found");
return result;
}
