void CAbstractPTGBasedReactive::setHolonomicMethod( const THolonomicMethod method, const mrpt::utils::CConfigFileBase &ini) { this->deleteHolonomicObjects(); const size_t nPTGs = this->getPTG_count(); m_holonomicMethod.resize(nPTGs); for (size_t i=0; i<nPTGs; i++) { switch (method) { case hmSEARCH_FOR_BEST_GAP: m_holonomicMethod[i] = new CHolonomicND(); break; case hmVIRTUAL_FORCE_FIELDS: m_holonomicMethod[i] = new CHolonomicVFF(); break; default: THROW_EXCEPTION_CUSTOM_MSG1("Unknown Holonomic method: %u",static_cast<unsigned int>(method)) }; // Load params: m_holonomicMethod[i]->initialize( ini ); } } // The main method: executes one time-iteration of the reactive navigation algorithm. void CAbstractPTGBasedReactive::performNavigationStep() { // Security tests: if (m_closing_navigator) return; // Are we closing in the main thread? if (!m_init_done) THROW_EXCEPTION("Have you called loadConfigFile() before?") const size_t nPTGs = this->getPTG_count(); // Whether to worry about log files: const bool fill_log_record = (m_logFile!=NULL || m_enableKeepLogRecords); CLogFileRecord newLogRec; newLogRec.infoPerPTG.resize(nPTGs); // Lock mrpt::synch::CCriticalSectionLocker lock( &m_critZoneNavigating ); CTimeLoggerEntry tle1(m_timelogger,"navigationStep"); try { totalExecutionTime.Tic(); // Start timer const mrpt::system::TTimeStamp tim_start_iteration = mrpt::system::now(); /* ---------------------------------------------------------------- Request current robot pose and velocities ---------------------------------------------------------------- */ poses::CPose2D curPose; float curVL; // in m/s float curW; // in rad/s { CTimeLoggerEntry tle2(m_timelogger,"navigationStep.getCurrentPoseAndSpeeds"); if ( !m_robot.getCurrentPoseAndSpeeds(curPose, curVL, curW ) ) { doEmergencyStop("ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot and finishing navigation"); return; } } /* ---------------------------------------------------------------- Have we reached the target location? ---------------------------------------------------------------- */ const double targetDist = curPose.distance2DTo( m_navigationParams->target.x, m_navigationParams->target.y ); // Should "End of navigation" event be sent?? if (!navigationEndEventSent && targetDist < DIST_TO_TARGET_FOR_SENDING_EVENT) { navigationEndEventSent = true; m_robot.sendNavigationEndEvent(); } // Have we really reached the target? if ( targetDist < m_navigationParams->targetAllowedDistance ) { m_robot.stop(); m_navigationState = IDLE; if (m_enableConsoleOutput) printf_debug("Navigation target (%.03f,%.03f) was reached\n", m_navigationParams->target.x,m_navigationParams->target.y); if (!navigationEndEventSent) { navigationEndEventSent = true; m_robot.sendNavigationEndEvent(); } return; } // Check the "no approaching the target"-alarm: // ----------------------------------------------------------- if (targetDist < badNavAlarm_minDistTarget ) { badNavAlarm_minDistTarget = targetDist; badNavAlarm_lastMinDistTime = system::getCurrentTime(); } else { // Too much time have passed? if ( system::timeDifference( badNavAlarm_lastMinDistTime, system::getCurrentTime() ) > badNavAlarm_AlarmTimeout) { std::cout << "\n--------------------------------------------\nWARNING: Timeout for approaching toward the target expired!! Aborting navigation!! \n---------------------------------\n"; m_navigationState = NAV_ERROR; return; } } // Compute target location relative to current robot pose: // --------------------------------------------------------------------- const CPose2D relTarget = CPose2D(m_navigationParams->target.x,m_navigationParams->target.y,m_navigationParams->targetHeading) - curPose; // STEP1: Collision Grids Builder. // ----------------------------------------------------------------------------- STEP1_CollisionGridsBuilder(); // Will only recompute if "m_collisionGridsMustBeUpdated==true" // STEP2: Load the obstacles and sort them in height bands. // ----------------------------------------------------------------------------- if (! STEP2_SenseObstacles() ) { printf_debug("Error while loading and sorting the obstacles. Robot will be stopped.\n"); m_robot.stop(); m_navigationState = NAV_ERROR; return; } // Start timer executionTime.Tic(); m_infoPerPTG.resize(nPTGs); vector<THolonomicMovement> holonomicMovements(nPTGs); for (size_t indexPTG=0;indexPTG<nPTGs;indexPTG++) { CHolonomicLogFileRecordPtr HLFR; // STEP3(a): Transform target location into TP-Space for each PTG // ----------------------------------------------------------------------------- CParameterizedTrajectoryGenerator * ptg = getPTG(indexPTG); ASSERT_(ptg) TInfoPerPTG &ipf = m_infoPerPTG[indexPTG]; // The picked movement in TP-Space (to be determined by holonomic method below) THolonomicMovement &holonomicMovement = holonomicMovements[indexPTG]; holonomicMovement.PTG = ptg; // If the user doesn't want to use this PTG, just mark it as invalid: ipf.valid_TP = true; { const TNavigationParamsPTG * navp = dynamic_cast<const TNavigationParamsPTG*>(m_navigationParams); if (navp && !navp->restrict_PTG_indices.empty()) { bool use_this_ptg = false; for (size_t i=0;i<navp->restrict_PTG_indices.size() && !use_this_ptg;i++) { if (navp->restrict_PTG_indices[i]==indexPTG) use_this_ptg = true; } ipf.valid_TP = use_this_ptg; } } // Normal PTG validity filter: check if target falls into the PTG domain: ipf.valid_TP = ipf.valid_TP && ptg->PTG_IsIntoDomain( relTarget.x(),relTarget.y() ); if (ipf.valid_TP) { ptg->inverseMap_WS2TP(relTarget.x(),relTarget.y(),ipf.target_k,ipf.target_dist); ipf.target_alpha = ptg->index2alpha(ipf.target_k); ipf.TP_Target.x = cos(ipf.target_alpha) * ipf.target_dist; ipf.TP_Target.y = sin(ipf.target_alpha) * ipf.target_dist; // STEP3(b): Build TP-Obstacles // ----------------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP3_WSpaceToTPSpace"); // Initialize TP-Obstacles: const size_t Ki = ptg->getAlfaValuesCount(); ipf.TP_Obstacles.resize( Ki ); for (size_t k=0;k<Ki;k++) { ipf.TP_Obstacles[k] = ptg->refDistance; // if the robot ends the trajectory due to a >180deg turn, set that as the max. distance, not D_{max}: float phi = ptg->GetCPathPoint_phi(k,ptg->getPointsCountInCPath_k(k)-1); if (fabs(phi) >= M_PI* 0.95f ) ipf.TP_Obstacles[k]= ptg->GetCPathPoint_d(k,ptg->getPointsCountInCPath_k(k)-1); } // Implementation-dependent conversion: STEP3_WSpaceToTPSpace(indexPTG,ipf.TP_Obstacles); // Distances in TP-Space are normalized to [0,1]: const double _refD = 1.0/ptg->refDistance; for (size_t i=0;i<Ki;i++) ipf.TP_Obstacles[i] *= _refD; } // STEP4: Holonomic navigation method // ----------------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP4_HolonomicMethod"); ASSERT_(m_holonomicMethod[indexPTG]) m_holonomicMethod[indexPTG]->navigate( ipf.TP_Target, ipf.TP_Obstacles, ptg->getMax_V_inTPSpace(), holonomicMovement.direction, holonomicMovement.speed, HLFR); // Security: Scale down the velocity when heading towards obstacles, // such that it's assured that we never go thru an obstacle! const int kDirection = static_cast<int>( holonomicMovement.PTG->alpha2index( holonomicMovement.direction ) ); const double obsFreeNormalizedDistance = ipf.TP_Obstacles[kDirection]; double velScale = 1.0; ASSERT_(secureDistanceEnd>secureDistanceStart); if (obsFreeNormalizedDistance<secureDistanceEnd) { if (obsFreeNormalizedDistance<=secureDistanceStart) velScale = 0.0; // security stop else velScale = (obsFreeNormalizedDistance-secureDistanceStart)/(secureDistanceEnd-secureDistanceStart); } // Scale: holonomicMovement.speed *= velScale; } // STEP5: Evaluate each movement to assign them a "evaluation" value. // --------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP5_PTGEvaluator"); STEP5_PTGEvaluator( holonomicMovement, ipf.TP_Obstacles, TPose2D(relTarget), ipf.TP_Target, newLogRec.infoPerPTG[indexPTG]); } } // end "valid_TP" else { // Invalid PTG (target out of reachable space): // - holonomicMovement= Leave default values HLFR = CLogFileRecord_VFF::Create(); } // Logging: if (fill_log_record) { metaprogramming::copy_container_typecasting(ipf.TP_Obstacles, newLogRec.infoPerPTG[indexPTG].TP_Obstacles); newLogRec.infoPerPTG[indexPTG].PTG_desc = ptg->getDescription(); newLogRec.infoPerPTG[indexPTG].TP_Target = CPoint2D(ipf.TP_Target); newLogRec.infoPerPTG[indexPTG].HLFR = HLFR; newLogRec.infoPerPTG[indexPTG].desiredDirection = holonomicMovement.direction; newLogRec.infoPerPTG[indexPTG].desiredSpeed = holonomicMovement.speed; newLogRec.infoPerPTG[indexPTG].evaluation = holonomicMovement.evaluation; newLogRec.infoPerPTG[indexPTG].timeForTPObsTransformation = 0; // XXX newLogRec.infoPerPTG[indexPTG].timeForHolonomicMethod = 0; // XXX } } // end for each PTG // STEP6: After all PTGs have been evaluated, pick the best scored: // --------------------------------------------------------------------- int nSelectedPTG = 0; for (size_t indexPTG=0;indexPTG<nPTGs;indexPTG++) { if ( holonomicMovements[indexPTG].evaluation > holonomicMovements[nSelectedPTG].evaluation ) nSelectedPTG = indexPTG; } const THolonomicMovement & selectedHolonomicMovement = holonomicMovements[nSelectedPTG]; // STEP7: Get the non-holonomic movement command. // --------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP7_NonHolonomicMovement"); STEP7_GenerateSpeedCommands( selectedHolonomicMovement ); } // --------------------------------------------------------------------- // SEND MOVEMENT COMMAND TO THE ROBOT // --------------------------------------------------------------------- if ( new_cmd_v == 0.0 && new_cmd_w == 0.0 ) { m_robot.stop(); } else { if ( !m_robot.changeSpeeds( new_cmd_v, new_cmd_w ) ) { doEmergencyStop("\nERROR calling RobotMotionControl::changeSpeeds!! Stopping robot and finishing navigation\n"); return; } } // Statistics: // ---------------------------------------------------- float executionTimeValue = (float) executionTime.Tac(); meanExecutionTime= 0.3f * meanExecutionTime + 0.7f * executionTimeValue; meanTotalExecutionTime= 0.3f * meanTotalExecutionTime + 0.7f * ((float)totalExecutionTime.Tac() ); meanExecutionPeriod = 0.3f * meanExecutionPeriod + 0.7f * min(1.0f, (float)timerForExecutionPeriod.Tac()); timerForExecutionPeriod.Tic(); if (m_enableConsoleOutput) { printf_debug( "CMD:%.02fm/s,%.02fd/s \t" "T=%.01lfms Exec:%.01lfms|%.01lfms \t" "E=%.01lf PTG#%i\n", (double)new_cmd_v, (double)RAD2DEG( new_cmd_w ), 1000.0*meanExecutionPeriod, 1000.0*meanExecutionTime, 1000.0*meanTotalExecutionTime, (double)selectedHolonomicMovement.evaluation, nSelectedPTG ); } // --------------------------------------- // STEP8: Generate log record // --------------------------------------- if (fill_log_record) { m_timelogger.enter("navigationStep.populate_log_info"); this->loggingGetWSObstaclesAndShape(newLogRec); newLogRec.robotOdometryPose = curPose; newLogRec.WS_target_relative = CPoint2D(relTarget.x(), relTarget.y()); newLogRec.v = new_cmd_v; newLogRec.w = new_cmd_w; newLogRec.nSelectedPTG = nSelectedPTG; newLogRec.executionTime = executionTimeValue; newLogRec.actual_v = curVL; newLogRec.actual_w = curW; newLogRec.estimatedExecutionPeriod = meanExecutionPeriod; newLogRec.timestamp = tim_start_iteration; newLogRec.nPTGs = nPTGs; newLogRec.navigatorBehavior = nSelectedPTG; m_timelogger.leave("navigationStep.populate_log_info"); // Save to log file: // -------------------------------------- m_timelogger.enter("navigationStep.write_log_file"); if (m_logFile) (*m_logFile) << newLogRec; m_timelogger.leave("navigationStep.write_log_file"); // Set as last log record { mrpt::synch::CCriticalSectionLocker lock_log(&m_critZoneLastLog); // Lock lastLogRecord = newLogRec; // COPY } } // if (fill_log_record) } catch (std::exception &e) { std::cout << e.what(); std::cout << "[CAbstractPTGBasedReactive::performNavigationStep] Exceptions!!\n"; } catch (...) { std::cout << "[CAbstractPTGBasedReactive::performNavigationStep] Unexpected exception!!:\n"; } } void CAbstractPTGBasedReactive::STEP5_PTGEvaluator( THolonomicMovement & holonomicMovement, const vector_double & in_TPObstacles, const mrpt::math::TPose2D & WS_Target, const mrpt::math::TPoint2D & TP_Target, CLogFileRecord::TInfoPerPTG & log ) { const double refDist = holonomicMovement.PTG->refDistance; const double TargetDir = (TP_Target.x!=0 || TP_Target.y!=0) ? atan2( TP_Target.y, TP_Target.x) : 0.0; const int TargetSector = static_cast<int>( holonomicMovement.PTG->alpha2index( TargetDir ) ); const double TargetDist = TP_Target.norm(); // Picked movement direction: const int kDirection = static_cast<int>( holonomicMovement.PTG->alpha2index( holonomicMovement.direction ) ); // Coordinates of the trajectory end for the given PTG and "alpha": float x,y,phi,t,d; d = min( in_TPObstacles[ kDirection ], 0.90f*TargetDist); holonomicMovement.PTG->getCPointWhen_d_Is( d, kDirection,x,y,phi,t ); // Factor 1: Free distance for the chosen PTG and "alpha" in the TP-Space: // ---------------------------------------------------------------------- const double factor1 = in_TPObstacles[kDirection]; // Factor 2: Distance in sectors: // ------------------------------------------- double dif = std::abs(((double)( TargetSector - kDirection ))); const double nSectors = (float)in_TPObstacles.size(); if ( dif > (0.5*nSectors)) dif = nSectors - dif; const double factor2 = exp(-square( dif / (in_TPObstacles.size()/3.0f))) ; // Factor 3: Angle between the robot at the end of the chosen trajectory and the target // ------------------------------------------------------------------------------------- double t_ang = atan2( WS_Target.y - y, WS_Target.x - x ); t_ang -= phi; mrpt::math::wrapToPiInPlace(t_ang); const double factor3 = exp(-square( t_ang / (float)(0.5f*M_PI)) ); // Factor4: Decrease in euclidean distance between (x,y) and the target: // Moving away of the target is negatively valued // --------------------------------------------------------------------------- const double dist_eucl_final = std::sqrt(square(WS_Target.x-x)+square(WS_Target.y-y)); const double dist_eucl_now = std::sqrt(square(WS_Target.x)+square(WS_Target.y)); const double factor4 = min(2.0*refDist,max(0.0,((dist_eucl_now - dist_eucl_final)+refDist)))/(2*refDist); // --------- // float decrementDistanc = dist_eucl_now - dist_eucl_final; // if (dist_eucl_now>0) // factor4 = min(1.0,min(refDist*2,max(0,decrementDistanc + refDist)) / dist_eucl_now); // else factor4 = 0; // --------- // factor4 = min(2*refDist2,max(0,decrementDistanc + refDist2)) / (2*refDist2); // factor4= (refDist2 - min( refDist2, dist_eucl ) ) / refDist2; // Factor5: Hysteresis: // ----------------------------------------------------- float want_v,want_w; holonomicMovement.PTG->directionToMotionCommand( kDirection, want_v,want_w); float likely_v = exp( -fabs(want_v-last_cmd_v)/0.10f ); float likely_w = exp( -fabs(want_w-last_cmd_w)/0.40f ); const double factor5 = min( likely_v,likely_w ); // Factor6: Fast when free space // ----------------------------------------------------- float aver_obs = 0; for (int i=0; i<in_TPObstacles.size(); i++) aver_obs += in_TPObstacles[i]; aver_obs = aver_obs/in_TPObstacles.size(); const double factor6 = aver_obs*want_v; // SAVE LOG log.evalFactors.resize(6); log.evalFactors[0] = factor1; log.evalFactors[1] = factor2; log.evalFactors[2] = factor3; log.evalFactors[3] = factor4; log.evalFactors[4] = factor5; log.evalFactors[5] = factor6; if (holonomicMovement.speed == 0) { // If no movement has been found -> the worst evaluation: holonomicMovement.evaluation = 0; } else { // Sum: two cases: *************************************I'm not sure about this... if (dif<2 && // Heading the target in_TPObstacles[kDirection]*0.95f>TargetDist // and free space towards the target ) { // Direct path to target: // holonomicMovement.evaluation = 1.0f + (1 - TargetDist) + factor5 * weight5 + factor6*weight6; holonomicMovement.evaluation = 1.0f + (1 - t/15.0f) + factor5 * weights[4] + factor6*weights[5]; } else { // General case: holonomicMovement.evaluation = ( factor1 * weights[0] + factor2 * weights[1] + factor3 * weights[2] + factor4 * weights[3] + factor5 * weights[4] + factor6 * weights[5] ) / ( math::sum(weights)); } } } void CAbstractPTGBasedReactive::STEP7_GenerateSpeedCommands( const THolonomicMovement &in_movement) { try { last_cmd_v = new_cmd_v; last_cmd_w = new_cmd_w; if (in_movement.speed == 0) { // The robot will stop: new_cmd_v = new_cmd_w = 0; } else { // Take the normalized movement command: in_movement.PTG->directionToMotionCommand( in_movement.PTG->alpha2index( in_movement.direction ), new_cmd_v, new_cmd_w ); // Scale holonomic speeds to real-world one: //const double reduction = min(1.0, in_movement.speed / sqrt( square(new_cmd_v) + square(r*new_cmd_w))); double reduction = in_movement.speed; if (reduction < 0.5) reduction = 0.5; // To scale: new_cmd_v*=reduction; new_cmd_w*=reduction; // Assure maximum speeds: if (fabs(new_cmd_v) > robotMax_V_mps) { // Scale: float F = fabs(robotMax_V_mps / new_cmd_v); new_cmd_v *= F; new_cmd_w *= F; } if (fabs(new_cmd_w) > DEG2RAD(robotMax_W_degps)) { // Scale: float F = fabs((float)DEG2RAD(robotMax_W_degps) / new_cmd_w); new_cmd_v *= F; new_cmd_w *= F; } //First order low-pass filter float alfa = meanExecutionPeriod/( meanExecutionPeriod + SPEEDFILTER_TAU); new_cmd_v = alfa*new_cmd_v + (1-alfa)*last_cmd_v; new_cmd_w = alfa*new_cmd_w + (1-alfa)*last_cmd_w; } m_timelogger.leave("navigationStep.STEP7_NonHolonomicMovement"); } catch (std::exception &e) { m_timelogger.leave("navigationStep.STEP7_NonHolonomicMovement"); printf_debug("[CReactiveNavigationSystem::STEP7_NonHolonomicMovement] Exception:"); printf_debug((char*)(e.what())); } catch (...) { m_timelogger.leave("navigationStep.STEP7_NonHolonomicMovement"); printf_debug("[CReactiveNavigationSystem::STEP7_NonHolonomicMovement] Unexpected exception!\n"); } }
void CAbstractPTGBasedReactive::setHolonomicMethod( const THolonomicMethod method, const mrpt::utils::CConfigFileBase &ini) { this->deleteHolonomicObjects(); const size_t nPTGs = this->getPTG_count(); m_holonomicMethod.resize(nPTGs); for (size_t i=0; i<nPTGs; i++) { switch (method) { case hmSEARCH_FOR_BEST_GAP: m_holonomicMethod[i] = new CHolonomicND(); break; case hmVIRTUAL_FORCE_FIELDS: m_holonomicMethod[i] = new CHolonomicVFF(); break; default: THROW_EXCEPTION_CUSTOM_MSG1("Unknown Holonomic method: %u",static_cast<unsigned int>(method)) }; // Load params: m_holonomicMethod[i]->initialize( ini ); } } // The main method: executes one time-iteration of the reactive navigation algorithm. void CAbstractPTGBasedReactive::performNavigationStep() { // Security tests: if (m_closing_navigator) return; // Are we closing in the main thread? if (!m_init_done) THROW_EXCEPTION("Have you called loadConfigFile() before?") const size_t nPTGs = this->getPTG_count(); // Whether to worry about log files: const bool fill_log_record = (m_logFile!=NULL || m_enableKeepLogRecords); CLogFileRecord newLogRec; newLogRec.infoPerPTG.resize(nPTGs); // At the beginning of each log file, add an introductory block explaining which PTGs are we using: { static mrpt::utils::CStream *prev_logfile = NULL; if (m_logFile && m_logFile!=prev_logfile) // Only the first time { prev_logfile=m_logFile; for (size_t i=0;i<nPTGs;i++) { // If we make a direct copy (=) we will store the entire, heavy, collision grid. // Let's just store the parameters of each PTG by serializing it, so paths can be reconstructed // by invoking initialize() mrpt::utils::CMemoryStream buf; buf << *this->getPTG(i); buf.Seek(0); newLogRec.infoPerPTG[i].ptg = mrpt::nav::CParameterizedTrajectoryGeneratorPtr ( buf.ReadObject() ); } } } // Lock mrpt::synch::CCriticalSectionLocker lock( &m_critZoneNavigating ); CTimeLoggerEntry tle1(m_timelogger,"navigationStep"); try { totalExecutionTime.Tic(); // Start timer const mrpt::system::TTimeStamp tim_start_iteration = mrpt::system::now(); /* ---------------------------------------------------------------- Request current robot pose and velocities ---------------------------------------------------------------- */ mrpt::math::TPose2D curPose; mrpt::math::TTwist2D curVel; { CTimeLoggerEntry tle2(m_timelogger,"navigationStep.getCurrentPoseAndSpeeds"); if ( !m_robot.getCurrentPoseAndSpeeds(curPose, curVel) ) { doEmergencyStop("ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot and finishing navigation"); return; } } mrpt::math::TTwist2D curVelLocal = curVel; curVelLocal.rotate(-curPose.phi); /* ---------------------------------------------------------------- Have we reached the target location? ---------------------------------------------------------------- */ const double targetDist = mrpt::math::distance( mrpt::math::TPoint2D(curPose), mrpt::math::TPoint2D(m_navigationParams->target)); // Should "End of navigation" event be sent?? if (!navigationEndEventSent && targetDist < DIST_TO_TARGET_FOR_SENDING_EVENT) { navigationEndEventSent = true; m_robot.sendNavigationEndEvent(); } // Have we really reached the target? if ( targetDist < m_navigationParams->targetAllowedDistance ) { m_robot.stop(); m_navigationState = IDLE; if (m_enableConsoleOutput) printf_debug("Navigation target (%.03f,%.03f) was reached\n", m_navigationParams->target.x,m_navigationParams->target.y); if (!navigationEndEventSent) { navigationEndEventSent = true; m_robot.sendNavigationEndEvent(); } return; } // Check the "no approaching the target"-alarm: // ----------------------------------------------------------- if (targetDist < badNavAlarm_minDistTarget ) { badNavAlarm_minDistTarget = targetDist; badNavAlarm_lastMinDistTime = system::getCurrentTime(); } else { // Too much time have passed? if ( system::timeDifference( badNavAlarm_lastMinDistTime, system::getCurrentTime() ) > badNavAlarm_AlarmTimeout) { std::cout << "\n--------------------------------------------\nWARNING: Timeout for approaching toward the target expired!! Aborting navigation!! \n---------------------------------\n"; m_navigationState = NAV_ERROR; return; } } // Compute target location relative to current robot pose: // --------------------------------------------------------------------- const CPose2D relTarget = CPose2D(m_navigationParams->target) - curPose; STEP1_InitPTGs(); // Will only recompute if "m_PTGsMustBeReInitialized==true" // Update kinematic state in all PTGs: for (size_t i=0;i<nPTGs;i++) getPTG(i)->updateCurrentRobotVel(curVelLocal); // STEP2: Load the obstacles and sort them in height bands. // ----------------------------------------------------------------------------- if (! STEP2_SenseObstacles() ) { printf_debug("Error while loading and sorting the obstacles. Robot will be stopped.\n"); m_robot.stop(); m_navigationState = NAV_ERROR; return; } // Start timer executionTime.Tic(); m_infoPerPTG.resize(nPTGs); vector<THolonomicMovement> holonomicMovements(nPTGs); for (size_t indexPTG=0;indexPTG<nPTGs;indexPTG++) { CHolonomicLogFileRecordPtr HLFR; // STEP3(a): Transform target location into TP-Space for each PTG // ----------------------------------------------------------------------------- CParameterizedTrajectoryGenerator * ptg = getPTG(indexPTG); ASSERT_(ptg) TInfoPerPTG &ipf = m_infoPerPTG[indexPTG]; // The picked movement in TP-Space (to be determined by holonomic method below) THolonomicMovement &holonomicMovement = holonomicMovements[indexPTG]; holonomicMovement.PTG = ptg; // If the user doesn't want to use this PTG, just mark it as invalid: ipf.valid_TP = true; { const TNavigationParamsPTG * navp = dynamic_cast<const TNavigationParamsPTG*>(m_navigationParams); if (navp && !navp->restrict_PTG_indices.empty()) { bool use_this_ptg = false; for (size_t i=0;i<navp->restrict_PTG_indices.size() && !use_this_ptg;i++) { if (navp->restrict_PTG_indices[i]==indexPTG) use_this_ptg = true; } ipf.valid_TP = use_this_ptg; } } // Normal PTG validity filter: check if target falls into the PTG domain: if (ipf.valid_TP) { ipf.valid_TP = ptg->inverseMap_WS2TP(relTarget.x(),relTarget.y(),ipf.target_k,ipf.target_dist); } if (!ipf.valid_TP) { ipf.target_k=0; ipf.target_dist=0; { // Invalid PTG (target out of reachable space): // - holonomicMovement= Leave default values HLFR = CLogFileRecord_VFF::Create(); } } else { ipf.target_alpha = ptg->index2alpha(ipf.target_k); ipf.TP_Target.x = cos(ipf.target_alpha) * ipf.target_dist; ipf.TP_Target.y = sin(ipf.target_alpha) * ipf.target_dist; // STEP3(b): Build TP-Obstacles // ----------------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP3_WSpaceToTPSpace"); // Initialize TP-Obstacles: const size_t Ki = ptg->getAlphaValuesCount(); ipf.TP_Obstacles.assign( Ki, ptg->getRefDistance()); // Implementation-dependent conversion: STEP3_WSpaceToTPSpace(indexPTG,ipf.TP_Obstacles); // Distances in TP-Space are normalized to [0,1]: const double _refD = 1.0/ptg->getRefDistance(); for (size_t i=0;i<Ki;i++) ipf.TP_Obstacles[i] *= _refD; } // STEP4: Holonomic navigation method // ----------------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP4_HolonomicMethod"); ASSERT_(m_holonomicMethod[indexPTG]) m_holonomicMethod[indexPTG]->navigate( ipf.TP_Target, ipf.TP_Obstacles, 1.0, // Was: ptg->getMax_V_inTPSpace(), holonomicMovement.direction, holonomicMovement.speed, HLFR); // Security: Scale down the velocity when heading towards obstacles, // such that it's assured that we never go thru an obstacle! const int kDirection = static_cast<int>( holonomicMovement.PTG->alpha2index( holonomicMovement.direction ) ); const double obsFreeNormalizedDistance = ipf.TP_Obstacles[kDirection]; double velScale = 1.0; ASSERT_(secureDistanceEnd>secureDistanceStart); if (obsFreeNormalizedDistance<secureDistanceEnd) { if (obsFreeNormalizedDistance<=secureDistanceStart) velScale = 0.0; // security stop else velScale = (obsFreeNormalizedDistance-secureDistanceStart)/(secureDistanceEnd-secureDistanceStart); } // Scale: holonomicMovement.speed *= velScale; } // STEP5: Evaluate each movement to assign them a "evaluation" value. // --------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP5_PTGEvaluator"); STEP5_PTGEvaluator( holonomicMovement, ipf.TP_Obstacles, TPose2D(relTarget), ipf.TP_Target, newLogRec.infoPerPTG[indexPTG]); } } // end "valid_TP" // Logging: if (fill_log_record) { metaprogramming::copy_container_typecasting(ipf.TP_Obstacles, newLogRec.infoPerPTG[indexPTG].TP_Obstacles); CLogFileRecord::TInfoPerPTG &ipp = newLogRec.infoPerPTG[indexPTG]; ipp.PTG_desc = ptg->getDescription(); ipp.TP_Target = ipf.TP_Target; ipp.HLFR = HLFR; ipp.desiredDirection = holonomicMovement.direction; ipp.desiredSpeed = holonomicMovement.speed; ipp.evaluation = holonomicMovement.evaluation; ipp.timeForTPObsTransformation = 0; // XXX ipp.timeForHolonomicMethod = 0; // XXX } } // end for each PTG // STEP6: After all PTGs have been evaluated, pick the best scored: // --------------------------------------------------------------------- int nSelectedPTG = 0; for (size_t indexPTG=0;indexPTG<nPTGs;indexPTG++) { if ( holonomicMovements[indexPTG].evaluation > holonomicMovements[nSelectedPTG].evaluation ) nSelectedPTG = indexPTG; } const THolonomicMovement & selectedHolonomicMovement = holonomicMovements[nSelectedPTG]; // STEP7: Get the non-holonomic movement command. // --------------------------------------------------------------------- { CTimeLoggerEntry tle(m_timelogger,"navigationStep.STEP7_NonHolonomicMovement"); STEP7_GenerateSpeedCommands( selectedHolonomicMovement ); } // --------------------------------------------------------------------- // SEND MOVEMENT COMMAND TO THE ROBOT // --------------------------------------------------------------------- // All equal to 0 means "stop". if (std::find_if(m_new_vel_cmd.begin(), m_new_vel_cmd.end(), std::bind2nd(std::not_equal_to<double>(), 0.0) ) == m_new_vel_cmd.end() ) { m_robot.stop(); } else { if ( !m_robot.changeSpeeds(m_new_vel_cmd) ) { doEmergencyStop("\nERROR calling RobotMotionControl::changeSpeeds!! Stopping robot and finishing navigation\n"); return; } } // Statistics: // ---------------------------------------------------- const double executionTimeValue = executionTime.Tac(); meanExecutionTime= 0.3 * meanExecutionTime + 0.7 * executionTimeValue; meanTotalExecutionTime= 0.3 * meanTotalExecutionTime + 0.7 * totalExecutionTime.Tac(); meanExecutionPeriod = 0.3 * meanExecutionPeriod + 0.7 * min(1.0, timerForExecutionPeriod.Tac()); timerForExecutionPeriod.Tic(); if (m_enableConsoleOutput) { printf_debug( "CMD: %s \t" "T=%.01lfms Exec:%.01lfms|%.01lfms \t" "E=%.01lf PTG#%i\n", mrpt::utils::sprintf_vector("%.02f ",m_new_vel_cmd).c_str(), 1000.0*meanExecutionPeriod, 1000.0*meanExecutionTime, 1000.0*meanTotalExecutionTime, (double)selectedHolonomicMovement.evaluation, nSelectedPTG ); } // --------------------------------------- // STEP8: Generate log record // --------------------------------------- if (fill_log_record) { m_timelogger.enter("navigationStep.populate_log_info"); this->loggingGetWSObstaclesAndShape(newLogRec); newLogRec.robotOdometryPose = curPose; newLogRec.WS_target_relative = TPoint2D(relTarget.x(), relTarget.y()); newLogRec.cmd_vel = m_new_vel_cmd; newLogRec.nSelectedPTG = nSelectedPTG; newLogRec.executionTime = executionTimeValue; newLogRec.cur_vel = curVel; newLogRec.cur_vel_local = curVelLocal; newLogRec.estimatedExecutionPeriod = meanExecutionPeriod; newLogRec.timestamp = tim_start_iteration; newLogRec.nPTGs = nPTGs; m_timelogger.leave("navigationStep.populate_log_info"); // Save to log file: // -------------------------------------- { mrpt::utils::CTimeLoggerEntry tle(m_timelogger,"navigationStep.write_log_file"); if (m_logFile) (*m_logFile) << newLogRec; } // Set as last log record { mrpt::synch::CCriticalSectionLocker lock_log(&m_critZoneLastLog); // Lock lastLogRecord = newLogRec; // COPY } } // if (fill_log_record) } catch (std::exception &e) { std::cout << e.what(); std::cout << "[CAbstractPTGBasedReactive::performNavigationStep] Exceptions!!\n"; } catch (...) { std::cout << "[CAbstractPTGBasedReactive::performNavigationStep] Unexpected exception!!:\n"; } } void CAbstractPTGBasedReactive::STEP5_PTGEvaluator( THolonomicMovement & holonomicMovement, const std::vector<double> & in_TPObstacles, const mrpt::math::TPose2D & WS_Target, const mrpt::math::TPoint2D & TP_Target, CLogFileRecord::TInfoPerPTG & log ) { const double refDist = holonomicMovement.PTG->getRefDistance(); const double TargetDir = (TP_Target.x!=0 || TP_Target.y!=0) ? atan2( TP_Target.y, TP_Target.x) : 0.0; const int TargetSector = static_cast<int>( holonomicMovement.PTG->alpha2index( TargetDir ) ); const double TargetDist = TP_Target.norm(); // Picked movement direction: const int kDirection = static_cast<int>( holonomicMovement.PTG->alpha2index( holonomicMovement.direction ) ); // Coordinates of the trajectory end for the given PTG and "alpha": const double d = min( in_TPObstacles[ kDirection ], 0.90*TargetDist); uint16_t nStep; bool pt_in_range = holonomicMovement.PTG->getPathStepForDist(kDirection, d, nStep); mrpt::math::TPose2D pose; holonomicMovement.PTG->getPathPose(kDirection, nStep,pose); // Factor 1: Free distance for the chosen PTG and "alpha" in the TP-Space: // ---------------------------------------------------------------------- const double factor1 = in_TPObstacles[kDirection]; // Factor 2: Distance in sectors: // ------------------------------------------- double dif = std::abs(((double)( TargetSector - kDirection ))); const double nSectors = (float)in_TPObstacles.size(); if ( dif > (0.5*nSectors)) dif = nSectors - dif; const double factor2 = exp(-square( dif / (in_TPObstacles.size()/3.0f))) ; // Factor 3: Angle between the robot at the end of the chosen trajectory and the target // ------------------------------------------------------------------------------------- double t_ang = atan2( WS_Target.y - pose.y, WS_Target.x - pose.x ); t_ang -= pose.phi; mrpt::math::wrapToPiInPlace(t_ang); const double factor3 = exp(-square( t_ang / (float)(0.5f*M_PI)) ); // Factor4: Decrease in euclidean distance between (x,y) and the target: // Moving away of the target is negatively valued // --------------------------------------------------------------------------- const double dist_eucl_final = std::sqrt(square(WS_Target.x- pose.x)+square(WS_Target.y- pose.y)); const double dist_eucl_now = std::sqrt(square(WS_Target.x)+square(WS_Target.y)); const double factor4 = min(2.0*refDist,max(0.0,((dist_eucl_now - dist_eucl_final)+refDist)))/(2*refDist); // Factor5: Hysteresis: // ----------------------------------------------------- std::vector<double> desired_cmd; holonomicMovement.PTG->directionToMotionCommand( kDirection, desired_cmd); ASSERT_EQUAL_(m_last_vel_cmd.size(), desired_cmd.size()); double simil_score = 1.0; for (size_t i=0;i<desired_cmd.size();i++) { const double scr = exp(-std::abs(desired_cmd[i] - m_last_vel_cmd[i]) / 0.20); mrpt::utils::keep_min(simil_score, scr); } const double factor5 = simil_score; // Factor6: free space // ----------------------------------------------------- float aver_obs = 0; for (size_t i=0; i<in_TPObstacles.size(); i++) aver_obs += in_TPObstacles[i]; aver_obs = aver_obs/in_TPObstacles.size(); const double factor6 = aver_obs; // *want_v; // SAVE LOG log.evalFactors.resize(6); log.evalFactors[0] = factor1; log.evalFactors[1] = factor2; log.evalFactors[2] = factor3; log.evalFactors[3] = factor4; log.evalFactors[4] = factor5; log.evalFactors[5] = factor6; if (holonomicMovement.speed == 0) { // If no movement has been found -> the worst evaluation: holonomicMovement.evaluation = 0; } else { if (dif<2 /* heading almost exactly towards goal */ && in_TPObstacles[kDirection]*0.95f>TargetDist // and free space towards the target ) { // Direct path to target: const double normalizedDistAlongPTG = holonomicMovement.PTG->getPathDist(kDirection, nStep) / holonomicMovement.PTG->getRefDistance(); // Return a score >1.0 so we ensure this option is preferred over any other which does not reach the target: holonomicMovement.evaluation = 1.0f + (1.0 - normalizedDistAlongPTG) + factor5 * weights[4] + factor6*weights[5]; } else { // General case: holonomicMovement.evaluation = holonomicMovement.PTG->getScorePriority() *( factor1 * weights[0] + factor2 * weights[1] + factor3 * weights[2] + factor4 * weights[3] + factor5 * weights[4] + factor6 * weights[5] ) / ( math::sum(weights)); } } } void CAbstractPTGBasedReactive::STEP7_GenerateSpeedCommands( const THolonomicMovement &in_movement) { mrpt::utils::CTimeLoggerEntry tle(m_timelogger, "STEP7_GenerateSpeedCommands"); try { if (in_movement.speed == 0) { // The robot will stop: m_new_vel_cmd.assign(m_new_vel_cmd.size(), 0.0); } else { // Take the normalized movement command: in_movement.PTG->directionToMotionCommand( in_movement.PTG->alpha2index( in_movement.direction ), m_new_vel_cmd); // Scale holonomic speeds to real-world one: m_robot.cmdVel_scale(m_new_vel_cmd, in_movement.speed); // Honor user speed limits & "blending": const double beta = meanExecutionPeriod / (meanExecutionPeriod + SPEEDFILTER_TAU); m_robot.cmdVel_limits(m_new_vel_cmd, m_last_vel_cmd, beta); } m_last_vel_cmd = m_new_vel_cmd; // Save for filtering in next step } catch (std::exception &e) { printf_debug("[CReactiveNavigationSystem::STEP7_NonHolonomicMovement] Exception:"); printf_debug((char*)(e.what())); } }