PTG-base reactive nav: new option to select PTGs to employ

apps/ReactiveNavigationDemo/ReactiveNavigationDemoMain.cpp

@@ -444,7 +444,8 @@ ReactiveNavigationDemoFrame::ReactiveNavigationDemoFrame(wxWindow* parent,wxWind
 
 	// VC2003 complains about the too big string:
 	wxString	auxStr = wxT("; ---------------------------------------------------------------\n; FILE: CONFIG_ReactiveNavigator.ini\n;\n;  In this file there are parameters to the reactive navigation\n;   module.\n;\n;  JLBC @ OCT-2005\n; ---------------------------------------------------------------\n\n\n[GLOBAL_CONFIG]\n; 0: VFF,  1: ND\nHOLONOMIC_METHOD=0\nALARM_SEEMS_NOT_APPROACHING_TARGET_TIMEOUT=100\n\n; ----------------------------------------------------\n;\tParameters for the \"Nearness diagram\" Holonomic method\n; ----------------------------------------------------\n[ND_CONFIG]\nfactorWeights=1.0 0.5 2.0 0.4\n; 1: Free space\n; 2: Dist. in sectors\n; 3: Closer to target (euclidean)\n; 4: Hysteresis\nWIDE_GAP_SIZE_PERCENT=0.50\nMAX_SECTOR_DIST_FOR_D2_PERCENT=0.25\n");
-	auxStr << wxT("RISK_EVALUATION_SECTORS_PERCENT=0.25\nRISK_EVALUATION_DISTANCE=0.15\t\t; In normalized ps-meters [0,1]\nTARGET_SLOW_APPROACHING_DISTANCE=1.00\t; For stop gradually\nTOO_CLOSE_OBSTACLE=0.02\t\t\t; In normalized ps-meters\n\n\n; ----------------------------------------------------\n;\tParameters for the navigation on ROBOT: \"SYM_CAR\"\n; ----------------------------------------------------\n[SYMCAR]\nweights=0.5 0.05 0.5 2.0 0.5 0.1\n; 1: Free space\n; 2: Dist. in sectors\t\t\t\n; 3: Heading toward target\n; 4: Closer to target (euclidean)\n; 5: Hysteresis\n; 6: Security Distance\n\nDIST_TO_TARGET_FOR_SENDING_EVENT=1.25\t; Minimum. distance to target for sending the end event. Set to 0 to send it just on navigation end\n\nMinObstaclesHeight=0.0 \t\t; Minimum coordinate in the \"z\" axis for an obstacle to be taken into account.\nMaxObstaclesHeight=1.40 \t\t; Maximum coordinate in the \"z\" axis for an obstacle to be taken into account.\n\nrobotMax_V_mps=0.70\t\t\t; Speed limits\nrobotMax_W_degps=50\n\nMAX_REFERENCE_DISTANCE=3.50\nWIDE_GAP_SIZE_PERCENT=0.40\nRISK_EVALUATION_DISTANCE=0.5\nRISK_EVALUATION_SECTORS_PERCENT=0.20\nMAX_SECTOR_DIST_FOR_D2_PERCENT=0.25\nRESOLUCION_REJILLA_X=0.03\nRESOLUCION_REJILLA_Y=0.03\n\nPTG_COUNT=4\n\n; \t\tC-PTGs:\n; ------------------------------------\nPTG0_Type=1\nPTG0_nAlfas=300\nPTG0_v_max_mps=0.7\nPTG0_w_max_gps=50\nPTG0_K=1.0\n\nPTG1_Type=1\nPTG1_nAlfas=300\nPTG1_v_max_mps=0.7\nPTG1_w_max_gps=20\nPTG1_K=1.0\n\nPTG3_Type=1\nPTG3_nAlfas=300\nPTG3_v_max_mps=0.7\nPTG3_w_max_gps=40\nPTG3_K=-1.0\n\n\n; \t     a-A type PTGs:\n; ------------------------------------\nPTG2_Type=2\nPTG2_nAlfas=300\nPTG2_v_max_mps=0.7\nPTG2_w_max_gps=50\nPTG2_cte_a0v_deg=10\nPTG2_cte_a0w_deg=40\n\n\nRobotModel_shape2D_xs=-0.2 0.5 0.5 -0.2\nRobotModel_shape2D_ys=0.3 0.3 -0.3 -0.3\n");
+	auxStr << wxT("RISK_EVALUATION_SECTORS_PERCENT=0.25\nRISK_EVALUATION_DISTANCE=0.15\t\t; In normalized ps-meters [0,1]\nTARGET_SLOW_APPROACHING_DISTANCE=1.00\t; For stop gradually\nTOO_CLOSE_OBSTACLE=0.02\t\t\t; In normalized ps-meters\n\n\n; ----------------------------------------------------\n;\tParameters for the navigation on ROBOT: \"SYM_CAR\"\n; ----------------------------------------------------\n[SYMCAR]\nweights=0.5 0.05 0.5 2.0 0.5 0.1\n; 1: Free space\n; 2: Dist. in sectors\t\t\t\n; 3: Heading toward target\n; 4: Closer to target (euclidean)\n; 5: Hysteresis\n; 6: Security Distance\n\nDIST_TO_TARGET_FOR_SENDING_EVENT=1.25\t; Minimum. distance to target for sending the end event. Set to 0 to send it just on navigation end\n\nMinObstaclesHeight=0.0 \t\t; Minimum coordinate in the \"z\" axis for an obstacle to be taken into account.\nMaxObstaclesHeight=1.40 \t\t; Maximum coordinate in the \"z\" axis for an obstacle to be taken into account.\n\nrobotMax_V_mps=2.00\t\t\t; Speed limits\nrobotMax_W_degps=80\n\nMAX_REFERENCE_DISTANCE=3.50\nWIDE_GAP_SIZE_PERCENT=0.40\nRISK_EVALUATION_DISTANCE=0.5\nRISK_EVALUATION_SECTORS_PERCENT=0.20\nMAX_SECTOR_DIST_FOR_D2_PERCENT=0.25\nRESOLUCION_REJILLA_X=0.03\nRESOLUCION_REJILLA_Y=0.03\n\nPTG_COUNT=3\n\n; \t\tC-PTGs:\n; ------------------------------------\nPTG0_Type=1\nPTG0_nAlfas=300\nPTG0_v_max_mps=2.0\nPTG0_w_max_gps=80\nPTG0_K=1.0\n\nPTG1_Type=1\nPTG1_nAlfas=300\nPTG1_v_max_mps=2.0\nPTG1_w_max_gps=80\nPTG1_K=-1.0\n\n\n; \t     a-A type PTGs:\n; ------------------------------------\nPTG2_Type=2\nPTG2_nAlfas=300\nPTG2_v_max_mps=2.0\nPTG2_w_max_gps=80\nPTG2_cte_a0v_deg=40\nPTG2_cte_a0w_deg=50\n\n\nRobotModel_shape2D_xs=-0.2 0.5 0.5 -0.2\nRobotModel_shape2D_ys=0.3 0.3 -0.3 -0.3\n");
+
 	iniEditoreactivenav->edText->SetValue( auxStr );
 
 	EDIT_internalCfgRobot = string( iniEditorRobot->edText->GetValue().mb_str() );
@@ -624,12 +625,16 @@ void ReactiveNavigationDemoFrame::OnbtnNavigateClick(wxCommandEvent& event)
 
 	if (reacNavObj)
 	{
-		CAbstractReactiveNavigationSystem::TNavigationParams   navParams;
+		//CAbstractReactiveNavigationSystem::TNavigationParams   navParams;
+		CAbstractPTGBasedReactive::TNavigationParamsPTG   navParams;
 		navParams.target.x = x ;
 		navParams.target.y = y ;
 		navParams.targetAllowedDistance = 0.40f;
 		navParams.targetIsRelative = false;
 
+		// Optional: restrict the PTGs to use
+		//navParams.restrict_PTG_indices.push_back(1);
+
 		reacNavObj->navigate( &navParams );
 	}
 }

apps/navlog-viewer/navlog_viewer_GUI_designMain.cpp

@@ -485,7 +485,7 @@ void navlog_viewer_GUI_designDialog::OnslidLogCmdScroll(wxScrollEvent& event)
 		vector<float> xs,ys;
 
 		const size_t nAlphas = pI.TP_Obstacles.size();
-		ASSERT_(nAlphas>0)
+		//ASSERT_(nAlphas>0)  // In case of "invalid" PTGs during navigation, TP_Obstacles may be left uncomputed.
 
 		// Chosen direction:
 		xs.resize(2);

libs/reactivenav/include/mrpt/reactivenav/CAbstractPTGBasedReactive.h

@@ -73,7 +73,21 @@ namespace mrpt
 	class REACTIVENAV_IMPEXP CAbstractPTGBasedReactive: public CAbstractReactiveNavigationSystem
 	{
 	public:
-		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+		/** The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes. */
+		struct REACTIVENAV_IMPEXP TNavigationParamsPTG : public CAbstractReactiveNavigationSystem::TNavigationParams
+		{
+			/** (Default=empty) Optionally, a list of PTG indices can be sent such that 
+			 *  the navigator will restrict itself to only employ those PTGs. */
+			std::vector<size_t>    restrict_PTG_indices; 
+
+			TNavigationParamsPTG() { }
+			virtual ~TNavigationParamsPTG() { }
+			virtual std::string getAsText() const;
+			virtual TNavigationParams* clone() const { return new TNavigationParamsPTG(*this); }
+		};
+
 
 		/** Constructor. 
 		  * \param[in] react_iterf_impl An instance of an object that implement all the required interfaces to read from and control a robot.
@@ -104,14 +118,10 @@ namespace mrpt
 			const std::string &section );
 
 
-		/** Start navigation with the given parameters. The method returns immediately, and next calls to navigationStep() will effectivaly start the actual navigation. */
-		void  navigate( const TNavigationParams &params );
-
-		/** (*DEPRECATED method signature, use version with argument passed by reference*) 
-		    *Start navigation:
+		/** Start navigation:
 			* \param[in] params Pointer to structure with navigation info (its contents will be copied, so the original can be freely destroyed upon return.)
 			*/
-		void  navigate( const TNavigationParams *params ) { ASSERT_(params!=NULL); this->navigate(*params); }
+		virtual void navigate( const TNavigationParams *params );
 
 		/** Provides a copy of the last log record with information about execution.
 			* \param o An object where the log will be stored into.

libs/reactivenav/include/mrpt/reactivenav/CAbstractReactiveNavigationSystem.h

@@ -125,25 +125,27 @@ namespace mrpt
 	class REACTIVENAV_IMPEXP CAbstractReactiveNavigationSystem : public mrpt::utils::CDebugOutputCapable
 	{
 	public:
-		/** The struct for configuring navigation requests. */
+		/** The struct for configuring navigation requests. See also: CAbstractPTGBasedReactive::TNavigationParamsPTG */
 		struct REACTIVENAV_IMPEXP TNavigationParams
 		{
 			mrpt::poses::TPoint2D  target;  //!< Coordinates of desired target location.
-			double                 targetHeading; //!< Target location (heading, in radians)
+			double                 targetHeading; //!< Target location (heading, in radians).
 
 			float                  targetAllowedDistance;    //!< Allowed distance to target in order to end the navigation.
 			bool                   targetIsRelative;  //!< (Default=false) Whether the \a target coordinates are in global coordinates (false) or are relative to the current robot pose (true).
 
 			TNavigationParams(); //!< Ctor with default values
-			std::string getAsText() const; //!< Gets navigation params as a human-readable format
+			virtual ~TNavigationParams() {}
+			virtual std::string getAsText() const; //!< Gets navigation params as a human-readable format
+			virtual TNavigationParams* clone() const { return new TNavigationParams(*this); }
 		};
 
 
 		/** Constructor */
 		CAbstractReactiveNavigationSystem( CReactiveInterfaceImplementation &react_iterf_impl );
 
         /** Destructor */
-        virtual ~CAbstractReactiveNavigationSystem() { }
+        virtual ~CAbstractReactiveNavigationSystem();
 
 		/** Cancel current navegacion. */
 		void cancel();
@@ -182,7 +184,7 @@ namespace mrpt
 		virtual void  performNavigationStep( )=0;
 
 		TState             m_navigationState;  //!< Current internal state of navigator:
-		TNavigationParams  m_navigationParams;  //!< Current navigation parameters
+		TNavigationParams  *m_navigationParams;  //!< Current navigation parameters
 
 
 		CReactiveInterfaceImplementation   &m_robot; //!< The navigator-robot interface.

libs/reactivenav/src/CAbstractPTGBasedReactive.cpp

@@ -44,6 +44,18 @@ using namespace mrpt::utils;
 using namespace mrpt::reactivenav;
 using namespace std;
 
+// ------ CAbstractPTGBasedReactive::TNavigationParamsPTG -----
+std::string CAbstractPTGBasedReactive::TNavigationParamsPTG::getAsText() const
+{
+
+	std::string s = TNavigationParams::getAsText();
+	s += "restrict_PTG_indices: ";
+	s += mrpt::utils::sprintf_vector("%u ",this->restrict_PTG_indices);
+	s += "\n";
+	return s;
+}
+
+// Ctor:
 CAbstractPTGBasedReactive::CAbstractPTGBasedReactive(CReactiveInterfaceImplementation &react_iterf_impl, bool enableConsoleOutput, bool enableLogFile):
 	CAbstractReactiveNavigationSystem(react_iterf_impl),
 	m_holonomicMethod            (),
@@ -161,15 +173,16 @@ void CAbstractPTGBasedReactive::getLastLogRecord( CLogFileRecord &o )
 	o = lastLogRecord;
 }
 
-void CAbstractPTGBasedReactive::navigate(const CReactiveNavigationSystem3D::TNavigationParams &params )
+void CAbstractPTGBasedReactive::navigate(const CReactiveNavigationSystem3D::TNavigationParams *params )
 {
 	navigationEndEventSent = false;
 
 	// Copy data:
-	m_navigationParams = params;
+	mrpt::utils::delete_safe(m_navigationParams);
+	m_navigationParams = params->clone();
 
 	// Transform: relative -> absolute, if needed.
-	if ( m_navigationParams.targetIsRelative )
+	if ( m_navigationParams->targetIsRelative )
 	{
 		poses::CPose2D currentPose;
 		float velLineal_actual,velAngular_actual;
@@ -180,14 +193,14 @@ void CAbstractPTGBasedReactive::navigate(const CReactiveNavigationSystem3D::TNav
 			return;
 		}
 
-		const poses::CPose2D relTarget(m_navigationParams.target.x,m_navigationParams.target.y,m_navigationParams.targetHeading);
+		const poses::CPose2D relTarget(m_navigationParams->target.x,m_navigationParams->target.y,m_navigationParams->targetHeading);
 		poses::CPose2D absTarget;
 		absTarget.composeFrom(currentPose, relTarget);
 
-		m_navigationParams.target = mrpt::math::TPoint2D(absTarget.x(),absTarget.y());
-		m_navigationParams.targetHeading = absTarget.phi();
+		m_navigationParams->target = mrpt::math::TPoint2D(absTarget.x(),absTarget.y());
+		m_navigationParams->targetHeading = absTarget.phi();
 
-		m_navigationParams.targetIsRelative = false; // Now it's not relative
+		m_navigationParams->targetIsRelative = false; // Now it's not relative
 	}
 
 	// new state:
@@ -289,7 +302,7 @@ void CAbstractPTGBasedReactive::performNavigationStep()
 		/* ----------------------------------------------------------------
 		 	  Have we reached the target location?
 		   ---------------------------------------------------------------- */
-		const double targetDist = curPose.distance2DTo( m_navigationParams.target.x, m_navigationParams.target.y );
+		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)
@@ -299,11 +312,11 @@ void CAbstractPTGBasedReactive::performNavigationStep()
 		}
 
 		// Have we really reached the target?
-		if ( targetDist < m_navigationParams.targetAllowedDistance )
+		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 (m_enableConsoleOutput) printf_debug("Navigation target (%.03f,%.03f) was reached\n", m_navigationParams->target.x,m_navigationParams->target.y);
 
 			if (!navigationEndEventSent)
 			{
@@ -335,7 +348,7 @@ void CAbstractPTGBasedReactive::performNavigationStep()
 
 		// Compute target location relative to current robot pose:
 		// ---------------------------------------------------------------------
-		const CPose2D relTarget = CPose2D(m_navigationParams.target.x,m_navigationParams.target.y,m_navigationParams.targetHeading) - curPose;
+		const CPose2D relTarget = CPose2D(m_navigationParams->target.x,m_navigationParams->target.y,m_navigationParams->targetHeading) - curPose;
 
 		// STEP1: Collision Grids Builder.
 		// -----------------------------------------------------------------------------
@@ -372,9 +385,23 @@ void CAbstractPTGBasedReactive::performNavigationStep()
 			THolonomicMovement &holonomicMovement = holonomicMovements[indexPTG];
 			holonomicMovement.PTG = ptg;
 
-			// Firstly, check if target falls into the PTG domain:
-			ipf.valid_TP = ptg->PTG_IsIntoDomain( relTarget.x(),relTarget.y() );
+			// 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)
 			{
@@ -448,7 +475,6 @@ void CAbstractPTGBasedReactive::performNavigationStep()
 			// Logging:
 			if (fill_log_record)
 			{
-				newLogRec.infoPerPTG.resize(nPTGs);
 				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);

libs/reactivenav/src/CAbstractReactiveNavigationSystem.cpp

@@ -63,10 +63,17 @@ std::string CAbstractReactiveNavigationSystem::TNavigationParams::getAsText() co
 							Constructor
   ---------------------------------------------------------------*/
 CAbstractReactiveNavigationSystem::CAbstractReactiveNavigationSystem(CReactiveInterfaceImplementation &react_iterf_impl) :
-	m_robot(react_iterf_impl)
+	m_robot               ( react_iterf_impl ),
+	m_lastNavigationState ( IDLE ),
+	m_navigationState     ( IDLE ),
+	m_navigationParams    ( NULL )
 {
-	m_navigationState =
-	m_lastNavigationState = IDLE;
+}
+
+// Dtor:
+CAbstractReactiveNavigationSystem::~CAbstractReactiveNavigationSystem()
+{
+	mrpt::utils::delete_safe( m_navigationParams );
 }
 
 /*---------------------------------------------------------------
@@ -148,7 +155,8 @@ void CAbstractReactiveNavigationSystem::navigationStep()
 			if ( m_lastNavigationState != NAVIGATING )
 			{
 				printf_debug("\n[CAbstractReactiveNavigationSystem::navigationStep()] Starting Navigation. Watchdog initiated...\n");
-				printf_debug("[CAbstractReactiveNavigationSystem::navigationStep()] Navigation Params:\n%s\n", m_navigationParams.getAsText().c_str() );
+				if (m_navigationParams)
+					printf_debug("[CAbstractReactiveNavigationSystem::navigationStep()] Navigation Params:\n%s\n", m_navigationParams->getAsText().c_str() );
 
 				m_robot.startWatchdog( 1000 );	// Watchdog = 1 seg
 			}