void cartControlReachAvoidThread::run()
{
yDebug("************* run() ************************************");
ts.update();
bool newTarget = false;
avoidanceVectors.clear();
newTargetFromPort = checkTargetFromPortInput(targetPosFromPort,targetRadius);
if(newTargetFromPort || newTargetFromRPC){ //target from RPC is set asynchronously
newTarget = true;
if (newTargetFromRPC){ //RPC has priority
printf("run: setting new target from rpc: %f %f %f \n",targetPos[0],targetPos[1],targetPos[2]);
newTargetFromRPC = false;
}
else{
targetPos = targetPosFromPort;
printf("run: setting new target from port: %f %f %f \n",targetPos[0],targetPos[1],targetPos[2]);
newTargetFromPort = false;
}
}
getReachingGainFromPort();
getAvoidanceGainFromPort();
getAvoidanceVectorsFromPort(); //fills up global avoidanceVectors vector
//debug code
vector<avoidanceStruct_t>::const_iterator it;
for(it=avoidanceVectors.begin(); it!=avoidanceVectors.end(); it++)
{
yDebug("run(): Avoidance struct read from port: SkinPart:%s, x,y,z: %s, n1,n2,n3: %s",SkinPart_s[it->skin_part].c_str(),it->x.toString().c_str(),it->n.toString().c_str());
}
if (state==STATE_IDLE){
yDebug("run(): STATE_IDLE\n");
if (newTarget){
if (reachableTarget(targetPos)){
printf("--- Got new target => REACHING\n");
state = STATE_REACH;
reachTimer = Time::now();
}
else{
printf("Target (%f %f %f) is outside of reachable space - x:<%f %f>, y:<%f %f>, z:<%f %f>\n",
targetPos[0],targetPos[1],targetPos[2],reachableSpace.minX,reachableSpace.maxX,reachableSpace.minY,
reachableSpace.maxY,reachableSpace.minZ,reachableSpace.maxZ);
}
}
//else - remain idle
}
else if(state==STATE_REACH){
yDebug("run(): STATE_REACH\n");
selectArm();
setControlModeArmsAndTorso(VOCAB_CM_VELOCITY);
doReach();
}
/*else if(state == STATE_CHECKMOTIONDONE)
{
yDebug("run(): STATE_CHECKMOTIONDONE\n");
bool done = false;
done = targetCloseEnough();
//cartArm->checkMotionDone(&done); //cannot be used here - we are controlling manually with velocity
if (!done)
{
if( ((Time::now()-reachTimer)>reachTmo)){
fprintf(stdout,"--- Reach timeout => go home\n");
state = STATE_GO_HOME;
}
}
else{ //done
fprintf(stdout,"--- Reach done. => go IDLE\n");
state = STATE_IDLE;
}
}*/
else if(state==STATE_GO_HOME){
yDebug("run(): STATE_GO_HOME\n");
setControlModeArmsAndTorso(VOCAB_CM_POSITION);
steerTorsoToHome();
steerArmToHome(LEFTARM);
steerArmToHome(RIGHTARM);
checkTorsoHome(3.0);
checkArmHome(LEFTARM,3.0);
checkArmHome(RIGHTARM,3.0);
yDebug("--- I'm home => go idle\n");
state=STATE_IDLE;
}
}
void TaskManager::actionlib_callback(const apc_bt_comms::ManagerGoalConstPtr &goal)
{
ROS_INFO("BT Task Manager got a request!");
sleep(0.1);
if (goal->success)
{
if (task_ == "pick")
{
ROS_INFO("Will mark bin as solved!");
if (markAsSolved(current_bin_))
{
nh_.setParam("/apc/task_manager/running", false);
}
}
else
{
ROS_INFO("Will mark object as solved!");
if (markAsSolved(current_object_))
{
nh_.setParam("/apc/task_manager/running", false);
}
}
}
else
{
if (goal->pop)
{
if (task_ == "pick")
{
if (!current_bin_.empty())
{
ROS_INFO("Current bin is not empty! Will mark as impossible");
markAsImpossible(current_bin_);
current_bin_.clear();
current_object_.clear();
}
ROS_INFO("Will pop next mission bin!");
current_bin_ = getNextInMission();
current_object_ = getOrderForBin(current_bin_);
current_arm_ = selectArm(current_object_, current_bin_);
}
else
{
if (!current_object_.empty())
{
ROS_INFO("Current object is not empty! Will mark as impossible");
markAsImpossible(current_object_);
current_bin_.clear();
current_object_.clear();
}
ROS_INFO("Will pop next mission object!");
current_object_ = getNextInMission();
current_arm_ = selectArm(current_object_, current_bin_);
current_bin_ = getBinWithFewerItems(current_arm_);
}
}
else
{
if (task_ == "stow") // Failed to place an object
{
setParam("/apc/task_manager/place_failure", true);
current_bin_ = getOtherBin();
}
}
}
setSystemTarget();
writeOutputFile();
result_.result = true;
action_server_.setSucceeded(result_);
}
