void approach_grab::spin(){ ros::Rate loop_rate( 10 ); while(ros::ok()){ //setRobotinoVelocity(0,0,0); setBHAPose(0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203,false); ROS_INFO("dist to target %i,%i",distToTarget[0],distToTarget[1]); float omega; if(distToTarget[1]<=0) omega=.5; else omega=-.5; // wait for the claw to open and for the ball to roll away. ros::Duration(5).sleep(); // reset the dist to target for a new loop iteration distToTarget[0]=0; distToTarget[1]=0; ROS_INFO("dist to target %i,%i",distToTarget[0],distToTarget[1]); ROS_INFO("Looking For Target Object"); // if target is not found rotate to look for it. while(distToTarget[0]==0||distToTarget[1]==0){ setRobotinoVelocity(0,0,omega); //rotate to look for ball sendPoseMsg(); ros::spinOnce(); loop_rate.sleep(); } // move toward target until it is within tolerances ROS_INFO("moving toward target"); while(moveTowardTarget()){ //ROS_INFO("dist to target %i,%i",distToTarget[0],distToTarget[1]); ros::spinOnce(); loop_rate.sleep(); } ROS_INFO("Target Approached. Error in pixels %i,%i",distToTarget[0],distToTarget[1]); ROS_INFO("Starting Pickup."); // setBHAPose(0.803812,0.459433,0.498534, 0.982405, 0.694037,0.855327, false); ros::spinOnce(); loop_rate.sleep(); ros::Duration(5).sleep(); ROS_INFO("Object Grabbed"); BHAPose.gripperClosed=true; sendPoseMsg(); ros::Duration(2).sleep(); //lift up object ROS_INFO("lifting object"); setBHAPose(0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203,true); // hold it up for 10 secconds ros::Duration(10).sleep(); ros::spinOnce(); loop_rate.sleep(); }// end while }