void GraspTester::testGrasps(const object_manipulation_msgs::PickupGoal &goal,
const std::vector<object_manipulation_msgs::Grasp> &grasps,
std::vector<GraspExecutionInfo> &execution_info,
bool return_on_first_hit)
{
execution_info.clear();
for (size_t i=0; i<grasps.size(); i++)
{
GraspExecutionInfo info;
ROS_DEBUG_NAMED("manipulation","Grasp tester: testing grasp %zd out of batch of %zd", i, grasps.size());
if (feedback_function_) feedback_function_(i);
if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
if (marker_publisher_)
{
geometry_msgs::PoseStamped marker_pose;
marker_pose.pose = grasps[i].grasp_pose;
marker_pose.header.frame_id = goal.target.reference_frame_id;
marker_pose.header.stamp = ros::Time::now();
info.marker_id_ = marker_publisher_->addGraspMarker(marker_pose);
}
testGrasp(goal, grasps[i], info);
execution_info.push_back(info);
if (info.result_.result_code == info.result_.SUCCESS && return_on_first_hit) return;
}
}
void
GraspTester::mainLoop()
{
GraspPlanningState *s = popCandidate();
if (!s) {
DBGP("Empty buffer for tester");
msleep(100);
return;
}
s->changeHand(mHand, true);
testGrasp(s);
DBGP("TESTER: candidate has energy " << s->getEnergy());
mHand->breakContacts();
if (s->isLegal() && s->getEnergy() < -1.2) {
//save the final grasping position that has resulted from autograsp
s->setPositionType(SPACE_COMPLETE);
s->setPostureType(POSE_DOF);
//save the current transform in absolute terms
s->setRefTran(transf::IDENTITY);
s->saveCurrentHandState();
postSolution(s);
DBGP("Tester posting a solution at iteration " << s->getItNumber());
} else {
DBGP("Tester removing candidate");
delete s;
}
}
