bool Branch::isVirtualHeadHealthy() { if (!isHeadVirtual()) { return false; } //double save_virtual_angle = global_paraMgr.skeleton.getDouble("Save Virtual Angle"); //double save_virtual_dist = global_paraMgr.skeleton.getDouble("Branches Merge Max Dist"); double head_length = GlobalFun::computeEulerDist(curve[0].P(), curve[1].P()); //double bad_virtual_angle = global_paraMgr.skeleton.getDouble("Bad Virtual Angle"); //double follow_dist = global_paraMgr.skeleton.getDouble("Follow Sample Radius"); double angle = getHeadAngle(); //if (angle > bad_virtual_angle || head_length > follow_dist) //{ // return false; //} //if (angle < save_virtual_angle || (head_length < save_virtual_dist && angle < bad_virtual_angle)) //{ // return true; //} //else //{ return false; //} }
void Branch::inactiveAndDropVirtualHead() { if (!isHeadVirtual()) { return; } CVertex& head = curve[0]; head.is_skel_virtual = false; if (back_up_head.X() > -5) { head.P() = back_up_head; if (getHeadAngle() > 100) { curve.erase(curve.begin()); } } else { curve.erase(curve.begin()); } }
task main() { // pose =========================================================== float pose[3]; float* pose_ptr = &pose; float x=0,y=0,phi=0; pose[0]=x;pose[1]=y;pose[2]=phi; // speeds ========================================================= float v=CRUISE, w=0; float w_look = 0.0; float w_head = 0.0; // logObstacle behavior =========================================== // obstacle log float obstacles_xy[N_ANGLES][2]; for(int i=0; i<N_ANGLES; i++){ obstacles_xy[i][0]=10.0;//x obstacles_xy[i][1]=10.0;//y } float* obstacle_ptr; float temp_o_x,temp_o_y; //ranging float range; // head tracking float halfViewingAngle = headCalibrate(); float headAngle = 0; float angleDelta = 2.0*halfViewingAngle/(float)N_ANGLES; float headAngleWorld = 0.0; float prevHeadAngleWorld = headAngleWorld-2.0*angleDelta; resetAngleCounter(N_ANGLES-1,-1); // collision ===================================================== bool collision_flag = false; //more state variables for pose =================================== float ticks[2]; float* ticks_ptr; float prevTicks[2]; prevTicks[0]=0;prevTicks[1]=0; resetWheelEncoders(); while(1){ // update state ================================================================================================ ticks_ptr = readWheelEncoders(); ticks[0]=ticks_ptr[0];ticks[1]=ticks_ptr[1]; pose_ptr = update_odometry(pose,ticks,prevTicks); pose[0]=pose_ptr[0];pose[1]=pose_ptr[1];pose[2]=pose_ptr[2]; x=pose_ptr[0];y=pose_ptr[1];phi=pose_ptr[2]; //store for next pass prevTicks[0] = ticks[0]; prevTicks[1] = ticks[1]; // end update state ============================================================================================ // logObstacle behavior ======================================================================================== headAngle = getHeadAngle(); range = getRange(); headAngleWorld = headAngle-halfViewingAngle+phi; if(abs(headAngleWorld - prevHeadAngleWorld) >= angleDelta){ prevHeadAngleWorld = headAngleWorld; obstacle_ptr = getObstaclePosition(headAngleWorld,range,pose); temp_o_x = obstacle_ptr[0];temp_o_y = obstacle_ptr[1]; obstacles_xy[AngleCounter][0] = temp_o_x; obstacles_xy[AngleCounter][1] = temp_o_y; incrementAngleCounter(); } // end logObstacle behavior ==================================================================================== // collide behavior ============================================================================================ collision_flag = checkForCollisions(obstacles_xy,pose); // end collide behavior ======================================================================================== // send actuator commands ====================================================================================== v = CRUISE; if(collision_flag && v>0.0){ v = 0.0; } set_motor_speeds(v,0.0); //worked before the angle was limited //if(collision_flag)v // //halt(); // //set_motor_speeds(0.0,0.0); // set_motor_speeds(0.0,5.0); //}else{ // set_motor_speeds(v,0.0); //} w_look = lookBackAndForth(w_look); w_head = w_look; headRotate(w_head); // end send actuator commands ================================================================================== } }