From acc731b2c5935ccb535976ddccb810808770a8fd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 5 Oct 2024 23:16:24 +0200 Subject: [PATCH] Waypoint approach is now more accurate --- .../reactive_navigator_demoMain.cpp | 2 +- doc/source/doxygen-docs/changelog.md | 4 +++- .../mrpt/nav/reactive/CWaypointsNavigator.h | 19 ++++++++++++----- .../nav/include/mrpt/nav/reactive/TWaypoint.h | 3 +++ libs/nav/src/reactive/CWaypointsNavigator.cpp | 21 ++++++++++++------- .../navigation-ptgs/reactive2d_config.ini | 4 ++-- 6 files changed, 36 insertions(+), 17 deletions(-) diff --git a/apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp b/apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp index 4bda3f6469..26544f92d3 100644 --- a/apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp +++ b/apps/ReactiveNavigationDemo/reactive_navigator_demoMain.cpp @@ -1587,7 +1587,7 @@ void reactive_navigator_demoframe::Onplot3DMouseClick(wxMouseEvent& event) heading *= M_PI / 180; } m_waypoints_clicked.waypoints.emplace_back( - m_curCursorPos.x, m_curCursorPos.y, 0.2 /* allowed dist */, allow_skip_wps, heading); + m_curCursorPos.x, m_curCursorPos.y, 0.35 /* allowed dist */, allow_skip_wps, heading); } if (event.ButtonIsDown(wxMOUSE_BTN_RIGHT)) { diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 1ffcee80b7..8dfed74d06 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,7 +1,9 @@ \page changelog Change Log # Version 2.14.3: UNRELEASED -(none yet) +- Changes in libraries: + - \ref mrpt_nav_grp: + - mrpt::nav::CWaypointsNavigator: New parameter "minimum_target_approach_per_step" and feature to keep approaching waypoints until no significant improvement is done. # Version 2.14.2: Released Oct 5th, 2024 - Changes in libraries: diff --git a/libs/nav/include/mrpt/nav/reactive/CWaypointsNavigator.h b/libs/nav/include/mrpt/nav/reactive/CWaypointsNavigator.h index 523d18489c..6ca7130ca1 100644 --- a/libs/nav/include/mrpt/nav/reactive/CWaypointsNavigator.h +++ b/libs/nav/include/mrpt/nav/reactive/CWaypointsNavigator.h @@ -119,22 +119,31 @@ class CWaypointsNavigator : public mrpt::nav::CAbstractNavigator struct TWaypointsNavigatorParams : public mrpt::config::CLoadableOptions { + TWaypointsNavigatorParams() = default; + /** In meters. <0: unlimited */ - double max_distance_to_allow_skip_waypoint{-1.0}; + double max_distance_to_allow_skip_waypoint = -1.0; + /** How many times shall a future waypoint be seen as reachable to skip * to it (Default: 1) */ - int min_timesteps_confirm_skip_waypoints{1}; + int min_timesteps_confirm_skip_waypoints = 1; + /** [rad] Angular error tolerance for waypoints with an assigned heading * (Default: 5 deg) */ - double waypoint_angle_tolerance; + double waypoint_angle_tolerance = mrpt::DEG2RAD(5.0); + /** >=0 number of waypoints to forward to the underlying navigation * engine, to ease obstacles avoidance when a waypoint is blocked * (Default=0 : none). */ - int multitarget_look_ahead{0}; + int multitarget_look_ahead = 0; + + /** While within the waypoint allowed_distance radius, this is the minimum distance [m] + * to be reduced for each time step. Once it is not, the waypoint will be marked as reached. + */ + double minimum_target_approach_per_step = 0.02; void loadFromConfigFile(const mrpt::config::CConfigFileBase& c, const std::string& s) override; void saveToConfigFile(mrpt::config::CConfigFileBase& c, const std::string& s) const override; - TWaypointsNavigatorParams(); }; TWaypointsNavigatorParams params_waypoints_navigator; diff --git a/libs/nav/include/mrpt/nav/reactive/TWaypoint.h b/libs/nav/include/mrpt/nav/reactive/TWaypoint.h index 167350b25a..6e1c964714 100644 --- a/libs/nav/include/mrpt/nav/reactive/TWaypoint.h +++ b/libs/nav/include/mrpt/nav/reactive/TWaypoint.h @@ -189,6 +189,9 @@ struct TWaypointStatusSequence mrpt::math::TSegment2D robot_move_seg; + /** Used to detect whether we peaked in closeness to a waypoint */ + double prevDist2target = .0; + /** Whether the last timestep was "is_aligning" in a waypoint with heading */ bool was_aligning = false; diff --git a/libs/nav/src/reactive/CWaypointsNavigator.cpp b/libs/nav/src/reactive/CWaypointsNavigator.cpp index bc8fa518af..88c4b65865 100644 --- a/libs/nav/src/reactive/CWaypointsNavigator.cpp +++ b/libs/nav/src/reactive/CWaypointsNavigator.cpp @@ -206,13 +206,22 @@ void CWaypointsNavigator::internal_select_next_waypoint_default_policy( auto& wp = wps.waypoints[wps.waypoint_index_current_goal]; const double dist2target = wps.robot_move_seg.distance(wp.target); + const double prev_dist2target = wps.prevDist2target; + wps.prevDist2target = dist2target; + if (dist2target > wp.allowed_distance && !wps.was_aligning /* we were already aligning at a WP */) return; // no need to check, we are not close enough. - if (!wps.was_aligning) + if (!wps.was_aligning && wps.prevDist2target > 0) { // We are approaching a WP, within |wp.allowed_distance| radius. - // XXX + // As long as we are getting closer and closer, let it keep going: + if (dist2target < + prev_dist2target - params_waypoints_navigator.minimum_target_approach_per_step) + { // ok, we are getting closer, do nothing: + return; + } + // Continue and accept the WP as reached (or perform the alignment there) } bool consider_wp_reached = false; @@ -506,6 +515,7 @@ void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFi MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s); MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s); MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s); + MRPT_LOAD_CONFIG_VAR(minimum_target_approach_per_step, double, c, s); } void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile( @@ -527,12 +537,7 @@ void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile ">=0 number of waypoints to forward to the underlying navigation " "engine, to ease obstacles avoidance when a waypoint is blocked " "(Default=0 : none)"); -} - -CWaypointsNavigator::TWaypointsNavigatorParams::TWaypointsNavigatorParams() : - waypoint_angle_tolerance(mrpt::DEG2RAD(5.0)) - -{ + MRPT_SAVE_CONFIG_VAR(minimum_target_approach_per_step, c, s); } /** \callergraph */ diff --git a/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini b/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini index 8254841c12..400e71e7fe 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini @@ -22,8 +22,8 @@ enable_time_profiler = false max_distance_to_allow_skip_waypoint = -1.000000 // Max distance to `foresee` waypoints [meters]. (<0: unlimited) min_timesteps_confirm_skip_waypoints = 1 // Min timesteps a `future` waypoint must be seen as reachable to become the active one. waypoint_angle_tolerance = 5.0 // Angular error tolerance for waypoints with an assigned heading [deg] -multitarget_look_ahead = 0 // >=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none). - +multitarget_look_ahead = 0 // >=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none). +minimum_target_approach_per_step = 0.02 // While within the waypoint allowed_distance radius, this is the minimum distance [m] to be reduced for each time step. Once it is not, the waypoint will be marked as reached. [CAbstractPTGBasedReactive] robotMax_V_mps = ${ROBOT_MAX_V} // Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set]