bool detection::GraspPointDetector::pick(moveit::planning_interface::MoveGroup &group, const std::string &object, moveit_msgs::Grasp &grasp, bool plan_only) { if (!pick_action_client_) { ROS_ERROR_STREAM("Pick action client not found"); return false; } if (!pick_action_client_->isServerConnected()) { ROS_ERROR_STREAM("Pick action server not connected"); return false; } moveit_msgs::PickupGoal goal; constructGoal(group, goal, object, PLANNER_NAME_, plan_only); goal.possible_grasps.push_back(grasp); if (plan_only) { pick_action_client_->cancelAllGoals(); pick_action_client_->sendGoal(goal); if (!pick_action_client_->waitForResult(ros::Duration(5.0))) ROS_DEBUG_STREAM("Pickup action returned early"); bool plan_succeeded = pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED; ROS_DEBUG_STREAM_COND(!plan_succeeded, "Fail: " << pick_action_client_->getState().toString() << ": " << pick_action_client_->getState().getText()); } else { try { pickMovement(pick_action_client_, goal, ros::Duration(25.0)); } catch (ComputeFailedException ex) { ROS_INFO_NAMED(DETECTION(), "%s", ex.what()); return false; } } return true; }
EReturn OMPLsolver::specifyProblem(PlanningProblem_ptr goals, PlanningProblem_ptr costs, PlanningProblem_ptr goalBias, PlanningProblem_ptr samplingBias) { if (goals->type().compare(std::string("exotica::OMPLProblem")) != 0) { ERROR("This solver can't use problem of type '" << goals->type() << "'!"); return PAR_INV; } if (costs && costs->type().compare(std::string("exotica::OMPLProblem")) != 0) { ERROR("This solver can't use problem of type '" << costs->type() << "'!"); return PAR_INV; } if (goalBias && goalBias->type().compare(std::string("exotica::OMPLProblem")) != 0) { ERROR( "This solver can't use problem of type '" << goalBias->type() << "'!"); return PAR_INV; } if (samplingBias && samplingBias->type().compare(std::string("exotica::OMPLProblem")) != 0) { ERROR( "This solver can't use problem of type '" << samplingBias->type() << "'!"); return PAR_INV; } MotionSolver::specifyProblem(goals); prob_ = boost::static_pointer_cast<OMPLProblem>(goals); costs_ = boost::static_pointer_cast<OMPLProblem>(costs); goalBias_ = boost::static_pointer_cast<OMPLProblem>(goalBias); samplingBias_ = boost::static_pointer_cast<OMPLProblem>(samplingBias); for (auto & it : prob_->getScenes()) { if (!ok(it.second->activateTaskMaps())) { INDICATE_FAILURE ; return FAILURE; } } if (costs) { for (auto & it : costs_->getScenes()) { if (!ok(it.second->activateTaskMaps())) { INDICATE_FAILURE ; return FAILURE; } } } if (goalBias_) { for (auto & it : goalBias_->getScenes()) { if (!ok(it.second->activateTaskMaps())) { INDICATE_FAILURE ; return FAILURE; } } } if (samplingBias_) { for (auto & it : samplingBias_->getScenes()) { if (!ok(it.second->activateTaskMaps())) { INDICATE_FAILURE ; return FAILURE; } } } compound_ = prob_->isCompoundStateSpace(); if (compound_) { state_space_ = ob::StateSpacePtr( OMPLSE3RNCompoundStateSpace::FromProblem(prob_, server_)); } else { state_space_ = OMPLStateSpace::FromProblem(prob_); } HIGHLIGHT_NAMED(object_name_, "Using state space: "<<state_space_->getName()); ompl_simple_setup_.reset(new og::SimpleSetup(state_space_)); ompl_simple_setup_->setStateValidityChecker( ob::StateValidityCheckerPtr(new OMPLStateValidityChecker(this))); ompl_simple_setup_->setPlannerAllocator( boost::bind(known_planners_[selected_planner_], _1, this->getObjectName() + "_" + selected_planner_)); if (compound_) ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator( boost::bind(&allocSE3RNStateSampler, (ob::StateSpace*) state_space_.get())); ompl_simple_setup_->getSpaceInformation()->setup(); std::vector<std::string> jnts; prob_->getScenes().begin()->second->getJointNames(jnts); if (projector_.compare("Joints") == 0) { bool projects_ok_ = true; std::vector<int> vars(projection_components_.size()); for (int i = 0; i < projection_components_.size(); i++) { bool found = false; for (int j = 0; j < jnts.size(); j++) { if (projection_components_[i].compare(jnts[j]) == 0) { vars[i] = j; found = true; break; } } if (!found) { WARNING( "Projection joint ["<<projection_components_[i]<<"] does not exist, OMPL Projection Evaluator not used"); projects_ok_ = false; break; } } if (projects_ok_) { ompl_simple_setup_->getStateSpace()->registerDefaultProjection( ompl::base::ProjectionEvaluatorPtr( new exotica::OMPLProjection(state_space_, vars))); std::string tmp; for (int i = 0; i < projection_components_.size(); i++) tmp = tmp + "[" + projection_components_[i] + "] "; HIGHLIGHT_NAMED(object_name_, " Using projection joints "<<tmp); } } else if (projector_.compare("DMesh") == 0) { // Construct default DMesh projection relationship std::vector<std::pair<int, int> > tmp_index( projection_components_.size() - 1); for (int i = 0; i < tmp_index.size(); i++) { tmp_index[i].first = i; tmp_index[i].second = tmp_index.size(); } ompl_simple_setup_->getStateSpace()->registerDefaultProjection( ompl::base::ProjectionEvaluatorPtr( new exotica::DMeshProjection(state_space_, projection_components_, tmp_index, prob_->getScenes().begin()->second))); std::string tmp; for (int i = 0; i < projection_components_.size(); i++) tmp = tmp + "[" + projection_components_[i] + "] "; HIGHLIGHT_NAMED(object_name_, " Using DMesh Projection links "<<tmp); } else { WARNING_NAMED(object_name_, "Unknown projection type "<<projector_<<". Setting ProjectionEvaluator failed."); } ompl_simple_setup_->setGoal(constructGoal()); ompl_simple_setup_->setup(); if (isFlexiblePlanner()) { INFO_NAMED(object_name_, "Setting up FRRT Local planner from file\n"<<prob_->local_planner_config_); if (!ompl_simple_setup_->getPlanner()->as<ompl::geometric::FlexiblePlanner>()->setUpLocalPlanner( prob_->local_planner_config_, prob_->scenes_.begin()->second)) { INDICATE_FAILURE return FAILURE; } }