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/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.cpp b/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.cpp index 8c991870e6..c2014d9a2e 100644 --- a/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.cpp +++ b/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.cpp @@ -110,7 +110,7 @@ END_EVENT_TABLE() holonomic_navigator_demoFrame::holonomic_navigator_demoFrame(wxWindow* parent, wxWindowID id) : m_gridMap(), - m_targetPoint(-5, -7), + m_targetPose(-5, -7, 0), m_robotPose(0, 0, 0), m_curCursorPos(0, 0), m_cursorPickState(cpsNone) @@ -667,9 +667,10 @@ void holonomic_navigator_demoFrame::simulateOneStep(double time_step) gl_scan2D->setScan(simulatedScan); // Draw scaled scan in right-hand view // Navigate: - mrpt::math::TPoint2D relTargetPose = - (mrpt::poses::CPoint2D(m_targetPoint) - mrpt::poses::CPose2D(m_robotPose)).asTPoint(); - relTargetPose *= 1.0 / simulatedScan.maxRange; // Normalized relative target: + mrpt::math::TPose2D relTargetPose = m_targetPose - m_robotPose; + // Normalized relative target: + relTargetPose.x *= 1.0 / simulatedScan.maxRange; + relTargetPose.y *= 1.0 / simulatedScan.maxRange; // tictac.Tic(); CAbstractHolonomicReactiveMethod::NavOutput no; @@ -757,14 +758,13 @@ void holonomic_navigator_demoFrame::updateViewsDynamicObjects() // Parabolic path const double s = 4 * t * (TARGET_BOUNCE_MAX - TARGET_BOUNCE_MIN) * (1 - t) + TARGET_BOUNCE_MIN; - gl_target->setLocation(m_targetPoint.x, m_targetPoint.y, 0); + gl_target->setLocation(mrpt::math::TPoint3D(m_targetPose.translation())); gl_target->setScale(s); } // Labels: StatusBar1->SetStatusText(mrpt::format("Robot: (%.03f,%.03f)", m_robotPose.x, m_robotPose.y), 0); - StatusBar1->SetStatusText( - mrpt::format("Target: (%.03f,%.03f)", m_targetPoint.x, m_targetPoint.y), 1); + StatusBar1->SetStatusText(mrpt::format("Target: %s", m_targetPose.asString().c_str())); // Show/hide: gl_robot_sensor_range->setVisibility(mnuViewMaxRange->IsChecked()); @@ -820,7 +820,7 @@ void holonomic_navigator_demoFrame::Onplot3DMouseClick(wxMouseEvent& event) { case cpsPickTarget: { - m_targetPoint = m_curCursorPos; + m_targetPose = {m_curCursorPos.x, m_curCursorPos.y, .0}; btnPlaceTarget->SetValue(false); btnPlaceTarget->Refresh(); diff --git a/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.h b/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.h index ed21f0378c..ef84e8f891 100644 --- a/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.h +++ b/apps/holonomic-navigator-demo/holonomic_navigator_demoMain.h @@ -169,7 +169,7 @@ class holonomic_navigator_demoFrame : public wxFrame std::unique_ptr m_holonomicMethod; mrpt::maps::COccupancyGridMap2D m_gridMap; - mrpt::math::TPoint2D m_targetPoint; + mrpt::math::TPose2D m_targetPose; mrpt::math::TPose2D m_robotPose; mrpt::system::CTicTac m_runtime; diff --git a/apps/navlog-viewer/navlog-viewer-ui.cpp b/apps/navlog-viewer/navlog-viewer-ui.cpp index cc0dbe60f5..315c071867 100644 --- a/apps/navlog-viewer/navlog-viewer-ui.cpp +++ b/apps/navlog-viewer/navlog-viewer-ui.cpp @@ -984,6 +984,10 @@ void NavlogViewerApp::updateVisualization() { // Target: + mrpt::opengl::CSetOfObjects::Ptr gl_trgCorners; + mrpt::opengl::CRenderizable::Ptr gl_trgC_r = + gl_robot_frame->getByName("target_corners"); // Get or create if new + mrpt::opengl::CPointCloud::Ptr gl_trg; mrpt::opengl::CRenderizable::Ptr gl_trg_r = gl_robot_frame->getByName("target"); // Get or create if new @@ -995,14 +999,22 @@ void NavlogViewerApp::updateVisualization() gl_trg->setPointSize(9.0); gl_trg->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0x00)); gl_robot_frame->insert(gl_trg); + + gl_trgCorners = mrpt::opengl::CSetOfObjects::Create(); + gl_trgCorners->setName("target_corners"); + gl_robot_frame->insert(gl_trgCorners); } else { gl_trg = std::dynamic_pointer_cast(gl_trg_r); + gl_trgCorners = std::dynamic_pointer_cast(gl_trgC_r); + ASSERT_(gl_trg); + ASSERT_(gl_trgCorners); } // Move the map & add a point at (0,0,0) so the name label // appears at the target: gl_trg->clear(); + gl_trgCorners->clear(); if (!log.WS_targets_relative.empty()) { const auto t0 = log.WS_targets_relative[0]; @@ -1011,6 +1023,9 @@ void NavlogViewerApp::updateVisualization() for (const auto& t : log.WS_targets_relative) { gl_trg->insertPoint(t.x - t0.x, t.y - t0.y, tz); + auto glCorner = mrpt::opengl::stock_objects::CornerXYZ(0.25f); + glCorner->setPose(t); + gl_trgCorners->insert(glCorner); } } } @@ -1264,10 +1279,8 @@ void NavlogViewerApp::updateVisualization() scene->insert(gl_obj); } { - auto gl_obj = mrpt::opengl::CPointCloud::Create(); + auto gl_obj = mrpt::opengl::CSetOfObjects::Create(); gl_obj->setName("tp_target"); - gl_obj->setPointSize(5.0f); - gl_obj->setColor_u8(mrpt::img::TColor(0x30, 0x30, 0x30, 0xff)); gl_obj->setLocation(0, 0, 0.02f); scene->insert(gl_obj); } @@ -1342,11 +1355,18 @@ void NavlogViewerApp::updateVisualization() // Target: { auto gl_obj = - std::dynamic_pointer_cast(scene->getByName("tp_target")); + std::dynamic_pointer_cast(scene->getByName("tp_target")); gl_obj->clear(); + auto glTargetPts = mrpt::opengl::CPointCloud::Create(); + glTargetPts->setPointSize(5); + glTargetPts->setColor_u8(0, 0, 0); + gl_obj->insert(glTargetPts); for (const auto& p : pI.TP_Targets) { - gl_obj->insertPoint(p.x, p.y, .0); + auto glCorner = mrpt::opengl::stock_objects::CornerXYZ(0.15f); + glCorner->setPose(p); + gl_obj->insert(glCorner); + glTargetPts->insertPoint(p.x, p.y, 0.02); } if (!pI.TP_Targets.empty()) diff --git a/appveyor.yml b/appveyor.yml index 311829ea1a..b59427ecfb 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 2.14.2-{branch}-build{build} +version: 2.14.3-{branch}-build{build} os: Visual Studio 2019 diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index c93b83bd78..1f0d3643d8 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,5 +1,17 @@ \page changelog Change Log +# Version 2.14.3: Released Oct 12th, 2024 +- Changes in libraries: + - \ref mrpt_img_grp: + - mrpt::img::CImage::rotateImage(): Special angles 90,-90, 180 are handled as expected with a quick image transformation and rotation. + - \ref mrpt_math_grp: + - mrpt::math::TPose2D and mrpt::math::TPose3D constructors from points are marked as explicit. + - \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. + - mrpt::nav::CHolonomicFullEval: Rewritten TP-Space data structures so the target heading is visible to holonomic evaluator algorithms. Added a new weight [7] related to correct alignment at target. All RNAV INI files have been updated accordingly. + - \ref mrpt_ros1bridge_grp and mrpt_ros2bridge_grp: + - Convert from MRPT occupancy grids to ROS: Add new optional parameter to interpret grid maps as cost maps. + # Version 2.14.2: Released Oct 5th, 2024 - Changes in libraries: - \ref mrpt_nav_grp: diff --git a/libs/containers/include/mrpt/containers/CDynamicGrid3D.h b/libs/containers/include/mrpt/containers/CDynamicGrid3D.h index 6a10c675de..216feec09b 100644 --- a/libs/containers/include/mrpt/containers/CDynamicGrid3D.h +++ b/libs/containers/include/mrpt/containers/CDynamicGrid3D.h @@ -203,7 +203,7 @@ class CDynamicGrid3D virtual void clear() { m_map.clear(); - m_map.resize(m_size_x * m_size_y * m_size_z); + if (const size_t N = m_size_x * m_size_y * m_size_z; N) m_map.resize(N); } /** Fills all the cells with the same value diff --git a/libs/img/include/mrpt/img/CImage.h b/libs/img/include/mrpt/img/CImage.h index 3a6f88a90d..71d1f384cb 100644 --- a/libs/img/include/mrpt/img/CImage.h +++ b/libs/img/include/mrpt/img/CImage.h @@ -261,8 +261,10 @@ class CImage : public mrpt::serialization::CSerializable, public CCanvas unsigned int height, TInterpolationMethod interp = IMG_INTERP_CUBIC) const; - /** Rotates the image by the given angle around the given center point, with + /** Rotates the image by the given angle (in radians) around the given center point, with * an optional scale factor. + * The output image will have the same size as the input, except if angle is exactly ±90 degrees, + * in which case a quick image rotation (switching height and widht) will be performed instead. * \sa resize, scaleImage */ void rotateImage( diff --git a/libs/img/src/CImage.cpp b/libs/img/src/CImage.cpp index 87b6d78359..0251307211 100644 --- a/libs/img/src/CImage.cpp +++ b/libs/img/src/CImage.cpp @@ -1797,6 +1797,22 @@ void CImage::rotateImage( // Detect in-place operation and make a deep copy if needed: if (out_img.m_impl->img.data == srcImg.data) srcImg = srcImg.clone(); + // quick rotation? + if (std::abs(M_PI * 0.5 - std::abs(ang)) < 1e-3 || std::abs(M_PI - std::abs(ang)) < 1e-3) + { + int rotCode = 0; + if (std::abs(M_PI * 0.5 - ang) < 1e-3) + rotCode = cv::ROTATE_90_COUNTERCLOCKWISE; + else if (std::abs(-M_PI * 0.5 - ang) < 1e-3) + rotCode = cv::ROTATE_90_CLOCKWISE; + else if (std::abs(M_PI - ang) < 1e-3) + rotCode = cv::ROTATE_180; + + cv::rotate(srcImg, out_img.m_impl->img, rotCode); + return; + } + // else: general rotation: + out_img.resize(getWidth(), getHeight(), getChannelCount()); // Based on the blog entry: diff --git a/libs/math/include/mrpt/math/TPose2D.h b/libs/math/include/mrpt/math/TPose2D.h index 2b6b53dd1e..7090181dde 100644 --- a/libs/math/include/mrpt/math/TPose2D.h +++ b/libs/math/include/mrpt/math/TPose2D.h @@ -36,10 +36,11 @@ struct TPose2D : public TPoseOrPoint, public internal::ProvideStaticResize -#include +#include #include #include #include @@ -33,6 +33,8 @@ class CAbstractHolonomicReactiveMethod : public mrpt::serialization::CSerializab /** Input parameters for CAbstractHolonomicReactiveMethod::navigate() */ struct NavInput { + NavInput() = default; + /** Distance to obstacles in polar coordinates, relative to the robot. * First index refers to -PI direction, and last one to +PI direction. * Distances can be dealed as "meters", although when used inside the @@ -40,37 +42,40 @@ class CAbstractHolonomicReactiveMethod : public mrpt::serialization::CSerializab * the range [0,1]. */ std::vector obstacles; + /** Relative location (x,y) of target point(s). In the same units than * `obstacles`. If many, last targets have higher priority. */ - std::vector targets; + std::vector targets; + /** Maximum robot speed, in the same units than `obstacles`, per second. */ - double maxRobotSpeed{1.0}; + double maxRobotSpeed = 1.0; + /** Maximum expected value to be found in `obstacles`. Typically, values * in `obstacles` larger or equal to this value mean there is no visible * obstacle in that direction. */ - double maxObstacleDist{1.0}; + double maxObstacleDist = 1.0; + /** The computed clearance for each direction (optional in some * implementations). Leave to default (NULL) if not needed. */ - const mrpt::nav::ClearanceDiagram* clearance{nullptr}; - - NavInput(); + const mrpt::nav::ClearanceDiagram* clearance = nullptr; }; /** Output for CAbstractHolonomicReactiveMethod::navigate() */ struct NavOutput { + NavOutput() = default; + /** The desired motion direction, in the range [-PI, PI] */ - double desiredDirection{0}; + double desiredDirection = 0; + /** The desired motion speed in that direction, from 0 up to * NavInput::maxRobotSpeed */ - double desiredSpeed{0}; + double desiredSpeed = 0; /** The navigation method will create a log record and store it here via * a smart pointer. Input value is ignored. */ CHolonomicLogFileRecord::Ptr logRecord; - - NavOutput(); }; static CAbstractHolonomicReactiveMethod::Ptr Factory(const std::string& className) noexcept; diff --git a/libs/nav/include/mrpt/nav/holonomic/CHolonomicFullEval.h b/libs/nav/include/mrpt/nav/holonomic/CHolonomicFullEval.h index 055ffeee92..8cda85979e 100644 --- a/libs/nav/include/mrpt/nav/holonomic/CHolonomicFullEval.h +++ b/libs/nav/include/mrpt/nav/holonomic/CHolonomicFullEval.h @@ -27,30 +27,7 @@ namespace mrpt::nav * CHolonomicFullEval::initialize() or directly in \a * CHolonomicFullEval::options * - * \code - * # Section name can be changed via setConfigFileSectionName() - * [FULL_EVAL_CONFIG] - * factorWeights = 1.0 1.0 1.0 0.05 1.0 - * factorNormalizeOrNot = 0 0 0 0 1 - * // 0: Clearness in direction - * // 1: Closest approach to target along straight line (Euclidean) - * // 2: Distance of end collision-free point to target (Euclidean) - * // 3: Hysteresis - * // 4: Clearness to nearest obstacle along path - * // 5: Like 2, but without being decimated if path to target is obstructed - * TARGET_SLOW_APPROACHING_DISTANCE = 0.20 // Start to reduce speed when - * closer than this to target [m] - * TOO_CLOSE_OBSTACLE = 0.02 // Directions with collision-free - * distances below this threshold are not elegible. - * HYSTERESIS_SECTOR_COUNT = 5 // Range of "sectors" (directions) - * for hysteresis over successive timesteps - * PHASE1_FACTORS = 0 1 2 // Indices of the factors above to - * be considered in phase 1 - * PHASE2_FACTORS = 3 4 // Indices of the factors above to - * be considered in phase 2 - * PHASE1_THRESHOLD = 0.75 // Phase1 scores must be above this - * relative range threshold [0,1] to be considered in phase 2 (Default:`0.75`) - * \endcode + * See [MRPT]/share/mrpt/config_files_navigation-ptgs for example configurations. * * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem */ diff --git a/libs/nav/include/mrpt/nav/holonomic/CHolonomicND.h b/libs/nav/include/mrpt/nav/holonomic/CHolonomicND.h index ec2dc79f6d..4c7319ee38 100644 --- a/libs/nav/include/mrpt/nav/holonomic/CHolonomicND.h +++ b/libs/nav/include/mrpt/nav/holonomic/CHolonomicND.h @@ -128,7 +128,7 @@ class CHolonomicND : public CAbstractHolonomicReactiveMethod /** Find gaps in the obtacles. */ void gapsEstimator( - const std::vector& obstacles, const mrpt::math::TPoint2D& in_target, TGapArray& gaps); + const std::vector& obstacles, const math::TPose2D& in_target, TGapArray& gaps); /** Search the best gap. */ @@ -136,7 +136,7 @@ class CHolonomicND : public CAbstractHolonomicReactiveMethod const std::vector& in_obstacles, const double in_maxObsRange, const TGapArray& in_gaps, - const mrpt::math::TPoint2D& in_target, + const math::TPose2D& in_target, unsigned int& out_selDirection, double& out_selEvaluation, TSituations& out_situation, @@ -146,7 +146,7 @@ class CHolonomicND : public CAbstractHolonomicReactiveMethod /** Fills in the representative sector field in the gap structure: */ void calcRepresentativeSectorForGap( - TGap& gap, const mrpt::math::TPoint2D& target, const std::vector& obstacles); + TGap& gap, const math::TPose2D& target, const std::vector& obstacles); /** Evaluate each gap: */ diff --git a/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h b/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h index 9b695cc8df..35c7d7071e 100644 --- a/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h +++ b/libs/nav/include/mrpt/nav/reactive/CAbstractPTGBasedReactive.h @@ -331,16 +331,20 @@ class CAbstractPTGBasedReactive : public CWaypointsNavigator struct PTGTarget { + PTGTarget() = default; + /** For each PTG, whether target falls into the PTG domain. */ - bool valid_TP{false}; - /** The Target, in TP-Space (x,y) */ - mrpt::math::TPoint2D TP_Target; + bool valid_TP = false; + + /** The Target, in TP-Space (x,y), plust its actual relative heading (phi) + * wrt to the current robot pose. */ + mrpt::math::TPose2D TP_Target; + /** TP-Target */ - double target_alpha, target_dist; - /** The discrete version of target_alpha */ - int target_k; + double target_alpha = 0, target_dist = 0; - PTGTarget() = default; + /** The discrete version of target_alpha */ + int target_k = 0; }; /** Scores \a holonomicMovement */ diff --git a/libs/nav/include/mrpt/nav/reactive/CLogFileRecord.h b/libs/nav/include/mrpt/nav/reactive/CLogFileRecord.h index 81c14923d8..f28f447e12 100644 --- a/libs/nav/include/mrpt/nav/reactive/CLogFileRecord.h +++ b/libs/nav/include/mrpt/nav/reactive/CLogFileRecord.h @@ -42,24 +42,33 @@ class CLogFileRecord : public mrpt::serialization::CSerializable { /** A short description for the applied PTG */ std::string PTG_desc; + /** Distances until obstacles, in "pseudometers", first index for -PI * direction, last one for PI direction. */ mrpt::math::CVectorFloat TP_Obstacles; + /** Target(s) location in TP-Space */ - std::vector TP_Targets; + std::vector TP_Targets; + /** Robot location in TP-Space: normally (0,0), except during "NOP cmd * vel" steps */ mrpt::math::TPoint2D TP_Robot; + /** Time, in seconds. */ double timeForTPObsTransformation, timeForHolonomicMethod; + /** The results from the holonomic method. */ double desiredDirection, desiredSpeed; + /** Final score of this candidate */ double evaluation; + /** Evaluation factors */ std::map evalFactors; + /** Other useful info about holonomic method execution. */ CHolonomicLogFileRecord::Ptr HLFR; + /** Only for the FIRST entry in a log file, this will contain a copy of * the PTG with trajectories, suitable to render trajectories, etc. */ mrpt::nav::CParameterizedTrajectoryGenerator::Ptr ptg; diff --git a/libs/nav/include/mrpt/nav/reactive/CWaypointsNavigator.h b/libs/nav/include/mrpt/nav/reactive/CWaypointsNavigator.h index d784fbfe72..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; @@ -164,11 +173,15 @@ class CWaypointsNavigator : public mrpt::nav::CAbstractNavigator /** The waypoints-specific part of navigationStep() */ virtual void waypoints_navigationStep(); - bool waypoints_isAligning() const { return m_is_aligning; } - /** Whether the last timestep was "is_aligning" in a waypoint with heading - */ - bool m_was_aligning{false}; - bool m_is_aligning{false}; + /// Sub-algorithms within waypoints_navigationStep() + void internal_select_next_waypoint(); + void internal_select_next_waypoint_default_policy( + std::list>& new_events); + void internal_select_next_waypoint_skip_policy(std::list>& new_events); + void internal_send_new_nav_cmd(const int prev_wp_index); + + bool waypoints_isAligning() const; + mrpt::system::TTimeStamp m_last_alignment_cmd; }; } // namespace mrpt::nav diff --git a/libs/nav/include/mrpt/nav/reactive/TWaypoint.h b/libs/nav/include/mrpt/nav/reactive/TWaypoint.h index ed7acbb435..6e1c964714 100644 --- a/libs/nav/include/mrpt/nav/reactive/TWaypoint.h +++ b/libs/nav/include/mrpt/nav/reactive/TWaypoint.h @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -173,19 +174,29 @@ struct TWaypointStatusSequence mrpt::system::TTimeStamp timestamp_nav_started = INVALID_TIMESTAMP; /** Whether the final waypoint has been reached successfuly. */ - bool final_goal_reached{false}; + bool final_goal_reached = false; /** Index in `waypoints` of the waypoint the navigator is currently trying * to reach. * This will point to the last waypoint after navigation ends successfully. * Its value is `-1` if navigation has not started yet */ - int waypoint_index_current_goal{-1}; + int waypoint_index_current_goal = -1; /** Robot pose at last time step (has INVALID_NUM fields upon * initialization) */ mrpt::math::TPose2D last_robot_pose{ TWaypoint::INVALID_NUM, TWaypoint::INVALID_NUM, TWaypoint::INVALID_NUM}; + 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; + bool is_aligning = false; + /** Ctor with default values */ /** Gets navigation params as a human-readable format */ std::string getAsText() const; diff --git a/libs/nav/src/holonomic/CAbstractHolonomicReactiveMethod.cpp b/libs/nav/src/holonomic/CAbstractHolonomicReactiveMethod.cpp index f8436f5e75..01619bc188 100644 --- a/libs/nav/src/holonomic/CAbstractHolonomicReactiveMethod.cpp +++ b/libs/nav/src/holonomic/CAbstractHolonomicReactiveMethod.cpp @@ -64,8 +64,3 @@ CAbstractHolonomicReactiveMethod::Ptr CAbstractHolonomicReactiveMethod::Factory( return CAbstractHolonomicReactiveMethod::Ptr(); } } - -CAbstractHolonomicReactiveMethod::NavInput::NavInput() : targets() {} -CAbstractHolonomicReactiveMethod::NavOutput::NavOutput() - - = default; diff --git a/libs/nav/src/holonomic/CHolonomicFullEval.cpp b/libs/nav/src/holonomic/CHolonomicFullEval.cpp index 79b890803f..dafd3c486f 100644 --- a/libs/nav/src/holonomic/CHolonomicFullEval.cpp +++ b/libs/nav/src/holonomic/CHolonomicFullEval.cpp @@ -18,6 +18,7 @@ #include // col(),... #include +#include using namespace mrpt; using namespace mrpt::math; @@ -27,7 +28,7 @@ using namespace std; IMPLEMENTS_SERIALIZABLE(CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav) IMPLEMENTS_SERIALIZABLE(CHolonomicFullEval, CAbstractHolonomicReactiveMethod, mrpt::nav) -const unsigned NUM_FACTORS = 7U; +constexpr unsigned NUM_FACTORS = 8U; CHolonomicFullEval::CHolonomicFullEval(const mrpt::config::CConfigFileBase* INI_FILE) : CAbstractHolonomicReactiveMethod("CHolonomicFullEval"), @@ -101,7 +102,7 @@ void CHolonomicFullEval::evalSingleTarget( for (unsigned int i = 0; i < nDirs; i++) { - double scores[NUM_FACTORS]; // scores for each criterion + std::array scores; // scores for each criterion // Too close to obstacles? (unless target is in between obstacles and // the robot) @@ -152,7 +153,7 @@ void CHolonomicFullEval::evalSingleTarget( // Range of attainable values: 0=passes thru target. 2=opposite // direction - double min_dist_target_along_path = sg.distance(target); + double min_dist_target_along_path = sg.distance(target.translation()); // Idea: if this segment is taking us *away* from target, don't make // the segment to start at (0,0), since all paths "running away" @@ -169,7 +170,7 @@ void CHolonomicFullEval::evalSingleTarget( // path takes us away or way blocked: sg.point1.x = x * 0.5; sg.point1.y = y * 0.5; - min_dist_target_along_path = sg.distance(target); + min_dist_target_along_path = sg.distance(target.translation()); } scores[1] = 1.0 / (1.0 + square(min_dist_target_along_path)); @@ -240,6 +241,13 @@ void CHolonomicFullEval::evalSingleTarget( scores[6] *= 0.1; } + // Factor [7]: Heading mismatch: 1.0=perfect phi aligment, 0.0=180deg error + // ------------------------------------------------------------------------- + uint32_t ptgStep = 0; + ptg->getPathStepForDist(i, d * ptg->getRefDistance(), ptgStep); + const auto ptgPose = ptg->getPathPose(i, ptgStep); + scores[7] = 1.0 - std::abs(mrpt::math::angDistance(target.phi, ptgPose.phi) / M_PI); + // Save stats for debugging: for (size_t l = 0; l < NUM_FACTORS; l++) m_dirs_scores(i, l) = scores[l]; diff --git a/libs/nav/src/holonomic/CHolonomicND.cpp b/libs/nav/src/holonomic/CHolonomicND.cpp index 7b3dde2824..7d00def064 100644 --- a/libs/nav/src/holonomic/CHolonomicND.cpp +++ b/libs/nav/src/holonomic/CHolonomicND.cpp @@ -123,7 +123,7 @@ void CHolonomicND::navigate(const NavInput& ni, NavOutput& no) Find gaps in the obtacles (Beta version) ---------------------------------------------------------------*/ void CHolonomicND::gapsEstimator( - const std::vector& obstacles, const mrpt::math::TPoint2D& target, TGapArray& gaps_out) + const std::vector& obstacles, const mrpt::math::TPose2D& target, TGapArray& gaps_out) { const size_t n = obstacles.size(); ASSERT_(n > 2); @@ -279,7 +279,7 @@ void CHolonomicND::searchBestGap( const std::vector& obstacles, const double maxObsRange, const TGapArray& in_gaps, - const mrpt::math::TPoint2D& target, + const mrpt::math::TPose2D& target, unsigned int& out_selDirection, double& out_selEvaluation, TSituations& out_situation, @@ -421,7 +421,7 @@ void CHolonomicND::searchBestGap( field in the gap structure: ---------------------------------------------------------------*/ void CHolonomicND::calcRepresentativeSectorForGap( - TGap& gap, const mrpt::math::TPoint2D& target, const std::vector& obstacles) + TGap& gap, const mrpt::math::TPose2D& target, const std::vector& obstacles) { int sector; const unsigned int sectors_to_be_wide = round(options.WIDE_GAP_SIZE_PERCENT * obstacles.size()); diff --git a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp index aabe248712..a8b4ac7a63 100644 --- a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp +++ b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp @@ -1525,11 +1525,9 @@ void CAbstractPTGBasedReactive::build_movement_candidate( ni.maxRobotSpeed = 1.0; // So, we use a normalized max speed here. ni.obstacles = ipf.TP_Obstacles; // Normalized [0,1] - ni.targets.clear(); // Normalized [0,1] - for (const auto& t : ipf.targets) - { + ni.targets.clear(); + for (const auto& t : ipf.targets) // ni.targets.push_back(t.TP_Target); - } CAbstractHolonomicReactiveMethod::NavOutput no; diff --git a/libs/nav/src/reactive/CLogFileRecord.cpp b/libs/nav/src/reactive/CLogFileRecord.cpp index 99603936ca..5edb5dd69c 100644 --- a/libs/nav/src/reactive/CLogFileRecord.cpp +++ b/libs/nav/src/reactive/CLogFileRecord.cpp @@ -21,7 +21,7 @@ using namespace mrpt::nav; IMPLEMENTS_SERIALIZABLE(CLogFileRecord, CSerializable, mrpt::nav) -uint8_t CLogFileRecord::serializeGetVersion() const { return 28; } +uint8_t CLogFileRecord::serializeGetVersion() const { return 29; } void CLogFileRecord::serializeTo(mrpt::serialization::CArchive& out) const { // Version 0 --------- @@ -35,7 +35,7 @@ void CLogFileRecord::serializeTo(mrpt::serialization::CArchive& out) const if (m) out.WriteBuffer((const void*)&(*ipp.TP_Obstacles.begin()), m * sizeof(ipp.TP_Obstacles[0])); - out << ipp.TP_Targets; // v8: CPoint2D -> TPoint2D. v26: vector + out << ipp.TP_Targets; // v8: CPoint2D -> TPoint2D. v26: vector. v29: TPose2D out << ipp.TP_Robot; // v17 out << ipp.timeForTPObsTransformation << ipp.timeForHolonomicMethod; // made double in v12 out << ipp.desiredDirection << ipp.desiredSpeed << ipp.evaluation; // made double in v12 @@ -144,6 +144,7 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve case 26: case 27: case 28: + case 29: { // Version 0 -------------- uint32_t i, n; @@ -167,20 +168,29 @@ void CLogFileRecord::serializeFrom(mrpt::serialization::CArchive& in, uint8_t ve { if (version >= 26) { - in >> ipp.TP_Targets; + if (version >= 29) + { + in >> ipp.TP_Targets; + } + else + { + std::vector trgs; + in >> trgs; + for (const auto& t : trgs) ipp.TP_Targets.push_back({t.x, t.y, .0}); + } } else { mrpt::math::TPoint2D trg; in >> trg; - ipp.TP_Targets.push_back(trg); + ipp.TP_Targets.push_back({trg.x, trg.y, .0}); } } else { mrpt::poses::CPoint2D pos; in >> pos; - ipp.TP_Targets.emplace_back(pos.x(), pos.y()); + ipp.TP_Targets.emplace_back(pos.x(), pos.y(), .0); } if (version >= 17) in >> ipp.TP_Robot; diff --git a/libs/nav/src/reactive/CWaypointsNavigator.cpp b/libs/nav/src/reactive/CWaypointsNavigator.cpp index 49cbcfd399..d86ff98a3e 100644 --- a/libs/nav/src/reactive/CWaypointsNavigator.cpp +++ b/libs/nav/src/reactive/CWaypointsNavigator.cpp @@ -10,7 +10,6 @@ #include "nav-precomp.h" // Precomp header // #include -#include #include #include #include @@ -55,17 +54,15 @@ void CWaypointsNavigator::onNavigateCommandReceived() std::lock_guard csl(m_nav_waypoints_cs); - m_was_aligning = false; - // This initializes all fields to initial values: - m_waypoint_nav_status = TWaypointStatusSequence(); + m_waypoint_nav_status = {}; } void CWaypointsNavigator::navigateWaypoints(const TWaypointSequence& nav_request) { MRPT_START - this->onNavigateCommandReceived(); + this->onNavigateCommandReceived(); // reset waypoint status to {} std::lock_guard csl(m_nav_waypoints_cs); @@ -113,305 +110,356 @@ void CWaypointsNavigator::waypoints_navigationStep() using mrpt::square; + // shortcut to save typing + TWaypointStatusSequence& wps = m_waypoint_nav_status; + // -------------------------------------- // Waypoint navigation algorithm // -------------------------------------- - m_is_aligning = false; // the robot is aligning into a waypoint with a desired heading + wps.is_aligning = false; // the robot is aligning into a waypoint with a desired heading + + mrpt::system::CTimeLoggerEntry tle(m_timlog_delays, "CWaypointsNavigator::navigationStep()"); + auto lck = mrpt::lockHelper(m_nav_waypoints_cs); + if (wps.waypoints.empty() || wps.final_goal_reached) { - mrpt::system::CTimeLoggerEntry tle(m_timlog_delays, "CWaypointsNavigator::navigationStep()"); - std::lock_guard csl(m_nav_waypoints_cs); + // No nav request is pending or it was canceled + } + else + { + // Get current robot pose: + CAbstractNavigator::updateCurrentPoseAndSpeeds(); + + internal_select_next_waypoint(); + } + + // Note: navigationStep() is called *after* this waypoints part, + // in order to get end-of-navigation events *after* waypoints-related events. + + wps.was_aligning = wps.is_aligning; // Let the next timestep know about this + + MRPT_END +} + +void CWaypointsNavigator::internal_select_next_waypoint() +{ + TWaypointStatusSequence& wps = m_waypoint_nav_status; + + // ------------------------------------------------------------------------- + // 1) default policy: go thru WPs one by one + // ------------------------------------------------------------------------- + const int prev_wp_index = wps.waypoint_index_current_goal; + + wps.robot_move_seg.point1.x = m_curPoseVel.pose.x; + wps.robot_move_seg.point1.y = m_curPoseVel.pose.y; + if (wps.last_robot_pose.x == TWaypoint::INVALID_NUM) + { + wps.robot_move_seg.point2 = wps.robot_move_seg.point1; + } + else + { + wps.robot_move_seg.point2.x = wps.last_robot_pose.x; + wps.robot_move_seg.point2.y = wps.last_robot_pose.y; + } + wps.last_robot_pose = m_curPoseVel.pose; // save for next iters + + decltype(m_pending_events) new_events; + + internal_select_next_waypoint_default_policy(new_events); - // shortcut to save typing - TWaypointStatusSequence& wps = m_waypoint_nav_status; + // ------------------------------------------------------------------------- + // 2) More advanced policy: if available, use children class methods + // to decide which is the best candidate for the next waypoint, if we can + // skip current one: + // ------------------------------------------------------------------------- + internal_select_next_waypoint_skip_policy(new_events); - if (wps.waypoints.empty() || wps.final_goal_reached) + // Insert at the beginning, for these events to be dispatched + // *before* any "end of nav" event: + m_pending_events.insert(m_pending_events.begin(), new_events.begin(), new_events.end()); + + // Still not started and no better guess? + if (wps.waypoint_index_current_goal < 0) + { + // Start with the first waypoint: + wps.waypoint_index_current_goal = 0; + } + + // ------------------------------------------------------------------------- + // 3) Should I request a new (single target) navigation command? + // Only if the temporary goal changed: + // ------------------------------------------------------------------------- + internal_send_new_nav_cmd(prev_wp_index); +} + +// ------------------------------------------------------------------------- +// default policy: go thru WPs one by one +// ------------------------------------------------------------------------- +void CWaypointsNavigator::internal_select_next_waypoint_default_policy( + std::list>& new_events) +{ + TWaypointStatusSequence& wps = m_waypoint_nav_status; + + if (wps.waypoint_index_current_goal < 0) return; // no active navigation + + // Check if waypoint (WP) was reached: + 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 && wps.prevDist2target > 0) + { + // We are approaching a WP, within |wp.allowed_distance| radius. + // 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; + + if (!wp.target_heading.has_value()) + { + // We reached a WP without any desired heading, so we are done: + consider_wp_reached = true; + } + else + { + // Handle pure-rotation robot interface to honor target_heading + const double ang_err = + mrpt::math::angDistance(m_curPoseVel.pose.phi, wp.target_heading.value()); + const double tim_since_last_align = + mrpt::system::timeDifference(m_last_alignment_cmd, mrpt::Clock::now()); + const double ALIGN_WAIT_TIME = 1.5; // seconds + + if (std::abs(ang_err) <= params_waypoints_navigator.waypoint_angle_tolerance && + /* give some time for the alignment (if supported in this robot) to finish */ + tim_since_last_align > ALIGN_WAIT_TIME) { - // No nav request is pending or it was canceled + consider_wp_reached = true; } else { - // 0) Get current robot pose: - CAbstractNavigator::updateCurrentPoseAndSpeeds(); - - // 1) default policy: go thru WPs one by one - const int prev_wp_index = wps.waypoint_index_current_goal; + wps.is_aligning = true; - mrpt::math::TSegment2D robot_move_seg; - robot_move_seg.point1.x = m_curPoseVel.pose.x; - robot_move_seg.point1.y = m_curPoseVel.pose.y; - if (wps.last_robot_pose.x == TWaypoint::INVALID_NUM) + if (!wps.was_aligning) { - robot_move_seg.point2 = robot_move_seg.point1; + // 1st time we are aligning: + // Send vel_cmd to the robot: + mrpt::kinematics::CVehicleVelCmd::Ptr align_cmd = m_robot.getAlignCmd(ang_err); + + MRPT_LOG_INFO_FMT( + "[CWaypointsNavigator::navigationStep] Trying to align to heading: %.02f deg. Relative " + "heading: %.02f deg. With motion cmd: %s", + mrpt::RAD2DEG(*wp.target_heading), mrpt::RAD2DEG(ang_err), + align_cmd ? align_cmd->asString().c_str() + : "nullptr (operation not supported by this robot)"); + + // In any case, do a "stop" + this->stop(false /*not emergency*/); + + if (align_cmd) + this->changeSpeeds(*align_cmd); + else + { + // this robot does not support "in place" alignment + consider_wp_reached = true; + } } else { - robot_move_seg.point2.x = wps.last_robot_pose.x; - robot_move_seg.point2.y = wps.last_robot_pose.y; + MRPT_LOG_THROTTLE_INFO_FMT( + 0.5, + "[CWaypointsNavigator::navigationStep] Waiting for the robot to get aligned: " + "current_heading=%.02f deg target_heading=%.02f deg", + mrpt::RAD2DEG(m_curPoseVel.pose.phi), mrpt::RAD2DEG(*wp.target_heading)); } - wps.last_robot_pose = m_curPoseVel.pose; // save for next iters + } + } - decltype(m_pending_events) new_events; + if (consider_wp_reached) + { + MRPT_LOG_DEBUG_STREAM( + "[CWaypointsNavigator::navigationStep] Waypoint " + << (wps.waypoint_index_current_goal + 1) << "/" << wps.waypoints.size() + << " reached." + " segment-to-target dist: " + << dist2target << ", allowed_dist: " << wp.allowed_distance); - if (wps.waypoint_index_current_goal >= 0) - { - auto& wp = wps.waypoints[wps.waypoint_index_current_goal]; - const double dist2target = robot_move_seg.distance(wp.target); - if (dist2target < wp.allowed_distance || - m_was_aligning /* we were already aligning at a WP */) - { - bool consider_wp_reached = false; - if (!wp.target_heading.has_value()) - { - // Any heading is ok: - consider_wp_reached = true; - } - else - { - // Handle pure-rotation robot interface to honor - // target_heading - const double ang_err = - mrpt::math::angDistance(m_curPoseVel.pose.phi, wp.target_heading.value()); - const double tim_since_last_align = - mrpt::system::timeDifference(m_last_alignment_cmd, mrpt::Clock::now()); - const double ALIGN_WAIT_TIME = 1.5; // seconds - - if (std::abs(ang_err) <= params_waypoints_navigator.waypoint_angle_tolerance && - /* give some time for the alignment (if supported in - this robot) to finish)*/ - tim_since_last_align > ALIGN_WAIT_TIME) - { - consider_wp_reached = true; - } - else - { - m_is_aligning = true; - - if (!m_was_aligning) - { - // 1st time we are aligning: - // Send vel_cmd to the robot: - mrpt::kinematics::CVehicleVelCmd::Ptr align_cmd = m_robot.getAlignCmd(ang_err); - - MRPT_LOG_INFO_FMT( - "[CWaypointsNavigator::navigationStep] " - "Trying to align to heading: %.02f deg. " - "Relative heading: %.02f deg. " - "With motion cmd: %s", - mrpt::RAD2DEG(*wp.target_heading), mrpt::RAD2DEG(ang_err), - align_cmd ? align_cmd->asString().c_str() - : "nullptr (operation not " - "supported by this robot)"); - - this->stop(false /*not emergency*/); // In any - // case, - // do a - // "stop" - if (align_cmd) - { - this->changeSpeeds(*align_cmd); - } - else - { - consider_wp_reached = true; // this robot does not support - // "in place" alignment - } - } - else - { - MRPT_LOG_THROTTLE_INFO_FMT( - 0.5, - "[CWaypointsNavigator::navigationStep] " - "Waiting for the robot to get aligned: " - "current_heading=%.02f deg " - "target_heading=%.02f deg", - mrpt::RAD2DEG(m_curPoseVel.pose.phi), mrpt::RAD2DEG(*wp.target_heading)); - } - } - } - - if (consider_wp_reached) - { - MRPT_LOG_DEBUG_STREAM( - "[CWaypointsNavigator::navigationStep] Waypoint " - << (wps.waypoint_index_current_goal + 1) << "/" << wps.waypoints.size() - << " reached." - " segment-to-target dist: " - << dist2target << ", allowed_dist: " << wp.allowed_distance); - - m_is_aligning = false; - - wp.reached = true; - wp.skipped = false; - wp.timestamp_reach = mrpt::Clock::now(); - - new_events.emplace_back(std::bind( - &CRobot2NavInterface::sendWaypointReachedEvent, std::ref(m_robot), - wps.waypoint_index_current_goal, true /*reason: really reached*/)); - - // Was this the final goal?? - if (wps.waypoint_index_current_goal < int(wps.waypoints.size() - 1)) - { - wps.waypoint_index_current_goal++; - } - else - { - wps.final_goal_reached = true; - // Make sure the end-navigation event is issued, - // navigation state switches to IDLE, etc: - this->performNavigationStepNavigating(false); - } - } - } - } + wps.is_aligning = false; - // 2) More advanced policy: if available, use children class methods - // to decide - // which is the best candidate for the next waypoint, if we can - // skip current one: - if (!wps.final_goal_reached && wps.waypoint_index_current_goal >= 0 && - wps.waypoints[wps.waypoint_index_current_goal].allow_skip) - { - const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose); - int most_advanced_wp = wps.waypoint_index_current_goal; - const int most_advanced_wp_at_begin = most_advanced_wp; + wp.reached = true; + wp.skipped = false; + wp.timestamp_reach = mrpt::Clock::now(); - for (int idx = wps.waypoint_index_current_goal; idx < (int)wps.waypoints.size(); idx++) - { - if (idx < 0) continue; - if (wps.waypoints[idx].reached) continue; - - // Is it reachable? - mrpt::math::TPoint2D wp_local_wrt_robot; - robot_pose.inverseComposePoint(wps.waypoints[idx].target, wp_local_wrt_robot); - - if (params_waypoints_navigator.max_distance_to_allow_skip_waypoint > 0 && - wp_local_wrt_robot.norm() > - params_waypoints_navigator.max_distance_to_allow_skip_waypoint) - continue; // Skip this one, it is too far away - - const bool is_reachable = this->impl_waypoint_is_reachable(wp_local_wrt_robot); - - if (is_reachable) - { - // Robustness filter: only skip to a future waypoint if - // it is seen as "reachable" during - // a given number of timesteps: - if (++wps.waypoints[idx].counter_seen_reachable > - params_waypoints_navigator.min_timesteps_confirm_skip_waypoints) - { - most_advanced_wp = idx; - } - } - - // Is allowed to skip it? - if (!wps.waypoints[idx].allow_skip) - break; // Do not keep trying, since we are now allowed - // to skip this one. - } + new_events.emplace_back(std::bind( + &CRobot2NavInterface::sendWaypointReachedEvent, std::ref(m_robot), + wps.waypoint_index_current_goal, true /*reason: really reached*/)); - if (most_advanced_wp >= 0) - { - wps.waypoint_index_current_goal = most_advanced_wp; - for (int k = most_advanced_wp_at_begin; k < most_advanced_wp; k++) - { - auto& wp = wps.waypoints[k]; - wp.reached = true; - wp.skipped = true; - wp.timestamp_reach = mrpt::Clock::now(); - - new_events.emplace_back(std::bind( - &CRobot2NavInterface::sendWaypointReachedEvent, std::ref(m_robot), k, - false /*reason: skipped*/)); - } - } - } + // Was this the final goal?? + if (wps.waypoint_index_current_goal < int(wps.waypoints.size() - 1)) + { + wps.waypoint_index_current_goal++; + } + else + { + wps.final_goal_reached = true; + // Make sure the end-navigation event is issued, + // navigation state switches to IDLE, etc: + this->performNavigationStepNavigating(false); + } + } +} + +void CWaypointsNavigator::internal_select_next_waypoint_skip_policy( + std::list>& new_events) +{ + TWaypointStatusSequence& wps = m_waypoint_nav_status; + + if (!wps.final_goal_reached && wps.waypoint_index_current_goal >= 0 && + wps.waypoints[wps.waypoint_index_current_goal].allow_skip) + { + const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose); + int most_advanced_wp = wps.waypoint_index_current_goal; + const int most_advanced_wp_at_begin = most_advanced_wp; + + for (int idx = wps.waypoint_index_current_goal; idx < (int)wps.waypoints.size(); idx++) + { + if (idx < 0) continue; + if (wps.waypoints[idx].reached) continue; + + // Is it reachable? + mrpt::math::TPoint2D wp_local_wrt_robot; + robot_pose.inverseComposePoint(wps.waypoints[idx].target, wp_local_wrt_robot); - // Insert at the beginning, for these events to be dispatched - // *before* any "end of nav" event: - m_pending_events.insert(m_pending_events.begin(), new_events.begin(), new_events.end()); + if (params_waypoints_navigator.max_distance_to_allow_skip_waypoint > 0 && + wp_local_wrt_robot.norm() > + params_waypoints_navigator.max_distance_to_allow_skip_waypoint) + continue; // Skip this one, it is too far away - // Still not started and no better guess? Start with the first - // waypoint: - if (wps.waypoint_index_current_goal < 0) wps.waypoint_index_current_goal = 0; + const bool is_reachable = this->impl_waypoint_is_reachable(wp_local_wrt_robot); - // 3) Should I request a new (single target) navigation command? - // Only if the temporary goal changed: - if (wps.waypoint_index_current_goal >= 0 && prev_wp_index != wps.waypoint_index_current_goal) + if (is_reachable) { - ASSERT_(wps.waypoint_index_current_goal < int(wps.waypoints.size())); - ASSERT_(params_waypoints_navigator.multitarget_look_ahead >= 0); - - // Notify we have a new "current waypoint" - // Push back so it's dispatched *after* the wp reached events: - m_pending_events.emplace_back(std::bind( - &CRobot2NavInterface::sendNewWaypointTargetEvent, std::ref(m_robot), - wps.waypoint_index_current_goal)); - - // Send the current targets + "multitarget_look_ahead" - // additional ones to help the local planner. - CWaypointsNavigator::TNavigationParamsWaypoints nav_cmd; - - // Check skippable flag while traversing from current wp forward - // "multitarget_look_ahead" steps: - int wp_last_idx = wps.waypoint_index_current_goal; - for (int nstep = 0; wp_last_idx < int(wps.waypoints.size()) - 1 && - nstep < params_waypoints_navigator.multitarget_look_ahead; - ++nstep) + // Robustness filter: only skip to a future waypoint if + // it is seen as "reachable" during + // a given number of timesteps: + if (++wps.waypoints[idx].counter_seen_reachable > + params_waypoints_navigator.min_timesteps_confirm_skip_waypoints) { - if (!m_waypoint_nav_status.waypoints[wp_last_idx].allow_skip) break; - wp_last_idx++; + most_advanced_wp = idx; } + } - for (int wp_idx = wps.waypoint_index_current_goal; wp_idx <= wp_last_idx; wp_idx++) - { - TWaypointStatus& wp = wps.waypoints[wp_idx]; - const bool is_final_wp = ((wp_idx + 1) == int(wps.waypoints.size())); - - CAbstractNavigator::TargetInfo ti; - - ti.target_coords.x = wp.target.x; - ti.target_coords.y = wp.target.y; - ti.target_coords.phi = (wp.target_heading.has_value() ? *wp.target_heading : .0); - ti.target_frame_id = wp.target_frame_id; - ti.targetAllowedDistance = wp.allowed_distance; - ti.targetIsRelative = false; - ti.targetIsIntermediaryWaypoint = !is_final_wp; - ti.targetDesiredRelSpeed = wp.speed_ratio; - - // For backwards compat. with single-target code, write - // single target info too for the first, next, waypoint: - if (wp_idx == wps.waypoint_index_current_goal) - { - nav_cmd.target = ti; - } - // Append to list of targets: - nav_cmd.multiple_targets.emplace_back(ti); - } - this->processNavigateCommand(&nav_cmd); + // Is allowed to skip it? + if (!wps.waypoints[idx].allow_skip) + break; // Do not keep trying, since we are now allowed + // to skip this one. + } - MRPT_LOG_DEBUG_STREAM( - "[CWaypointsNavigator::navigationStep] Active waypoint " - "changed. Current status:\n" - << this->getWaypointNavStatus().getAsText()); + if (most_advanced_wp >= 0) + { + wps.waypoint_index_current_goal = most_advanced_wp; + for (int k = most_advanced_wp_at_begin; k < most_advanced_wp; k++) + { + auto& wp = wps.waypoints[k]; + wp.reached = true; + wp.skipped = true; + wp.timestamp_reach = mrpt::Clock::now(); + + new_events.emplace_back(std::bind( + &CRobot2NavInterface::sendWaypointReachedEvent, std::ref(m_robot), k, + false /*reason: skipped*/)); } } } +} - // Note: navigationStep() called *after* waypoints part to get - // end-of-navigation events *after* - // waypoints-related events: +void CWaypointsNavigator::internal_send_new_nav_cmd(const int prev_wp_index) +{ + TWaypointStatusSequence& wps = m_waypoint_nav_status; + + // Should I request a new (single target) navigation command? + // Only if the temporary goal changed: + if (wps.waypoint_index_current_goal < 0 || prev_wp_index == wps.waypoint_index_current_goal) + return; // no changes + + ASSERT_(wps.waypoint_index_current_goal < int(wps.waypoints.size())); + ASSERT_(params_waypoints_navigator.multitarget_look_ahead >= 0); + + // Notify we have a new "current waypoint" + // Push back so it's dispatched *after* the wp reached events: + m_pending_events.emplace_back(std::bind( + &CRobot2NavInterface::sendNewWaypointTargetEvent, std::ref(m_robot), + wps.waypoint_index_current_goal)); + + // Send the current targets + "multitarget_look_ahead" + // additional ones to help the local planner. + CWaypointsNavigator::TNavigationParamsWaypoints nav_cmd; + + // Check skippable flag while traversing from current wp forward + // "multitarget_look_ahead" steps: + int wp_last_idx = wps.waypoint_index_current_goal; + for (int nstep = 0; wp_last_idx < int(wps.waypoints.size()) - 1 && + nstep < params_waypoints_navigator.multitarget_look_ahead; + ++nstep) + { + if (!m_waypoint_nav_status.waypoints[wp_last_idx].allow_skip) break; + wp_last_idx++; + } - m_was_aligning = m_is_aligning; // Let the next timestep know about this + for (int wp_idx = wps.waypoint_index_current_goal; wp_idx <= wp_last_idx; wp_idx++) + { + TWaypointStatus& wp = wps.waypoints[wp_idx]; + const bool is_final_wp = ((wp_idx + 1) == int(wps.waypoints.size())); + + CAbstractNavigator::TargetInfo ti; + + ti.target_coords.x = wp.target.x; + ti.target_coords.y = wp.target.y; + ti.target_coords.phi = (wp.target_heading.has_value() ? *wp.target_heading : .0); + ti.target_frame_id = wp.target_frame_id; + ti.targetAllowedDistance = wp.allowed_distance; + ti.targetIsRelative = false; + ti.targetIsIntermediaryWaypoint = !is_final_wp; + ti.targetDesiredRelSpeed = wp.speed_ratio; + + // For backwards compat. with single-target code, write + // single target info too for the first, next, waypoint: + if (wp_idx == wps.waypoint_index_current_goal) + { + nav_cmd.target = ti; + } + // Append to list of targets: + nav_cmd.multiple_targets.emplace_back(ti); + } + this->processNavigateCommand(&nav_cmd); - MRPT_END + MRPT_LOG_DEBUG_STREAM( + "[CWaypointsNavigator::navigationStep] Active waypoint " + "changed. Current status:\n" + << this->getWaypointNavStatus().getAsText()); +} + +bool CWaypointsNavigator::waypoints_isAligning() const +{ + auto lck = mrpt::lockHelper(m_nav_waypoints_cs); + return m_waypoint_nav_status.is_aligning; } void CWaypointsNavigator::navigationStep() { MRPT_START - // the robot is aligning into a waypoint with a desired heading - m_is_aligning = false; + + m_waypoint_nav_status.is_aligning = false; mrpt::system::CTimeLoggerEntry tle(m_navProfiler, "CWaypointsNavigator::navigationStep()"); @@ -422,10 +470,9 @@ void CWaypointsNavigator::navigationStep() // Call base navigation step to execute one-single waypoint navigation, as // usual: - if (!m_is_aligning) + if (!m_waypoint_nav_status.is_aligning) { - CAbstractNavigator::navigationStep(); // This internally locks - // "m_nav_cs" + CAbstractNavigator::navigationStep(); // This internally locks "m_nav_cs" } else { @@ -468,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( @@ -489,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/libs/ros1bridge/include/mrpt/ros1bridge/map.h b/libs/ros1bridge/include/mrpt/ros1bridge/map.h index c33667bd8b..d46e5b5e4a 100644 --- a/libs/ros1bridge/include/mrpt/ros1bridge/map.h +++ b/libs/ros1bridge/include/mrpt/ros1bridge/map.h @@ -96,16 +96,22 @@ bool fromROS(const nav_msgs::OccupancyGrid& src, mrpt::maps::COccupancyGridMap2D * @return true on sucessful conversion, false on any error. * @param src * @param header + * @param as_costmap If set to true, gridmap cell values will be copied without changes + * (interpreted as int8_t instead of Log-odds) */ bool toROS( const mrpt::maps::COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& msg, - const std_msgs::Header& header); + const std_msgs::Header& header, + bool as_costmap = false); /** * converts mrpt object to ros msg * @return true on sucessful conversion, false on any error. */ -bool toROS(const mrpt::maps::COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& msg); +bool toROS( + const mrpt::maps::COccupancyGridMap2D& src, + nav_msgs::OccupancyGrid& msg, + bool as_costmap = false); /** @} * @} diff --git a/libs/ros1bridge/src/map.cpp b/libs/ros1bridge/src/map.cpp index b3994ffaac..2153ae904f 100644 --- a/libs/ros1bridge/src/map.cpp +++ b/libs/ros1bridge/src/map.cpp @@ -107,13 +107,17 @@ bool mrpt::ros1bridge::fromROS(const nav_msgs::OccupancyGrid& src, COccupancyGri MRPT_END } bool mrpt::ros1bridge::toROS( - const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des, const std_msgs::Header& header) + const COccupancyGridMap2D& src, + nav_msgs::OccupancyGrid& des, + const std_msgs::Header& header, + bool as_costmap) { des.header = header; - return toROS(src, des); + return toROS(src, des, as_costmap); } -bool mrpt::ros1bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des) +bool mrpt::ros1bridge::toROS( + const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des, bool as_costmap) { des.info.width = src.getSizeX(); des.info.height = src.getSizeY(); @@ -133,10 +137,14 @@ bool mrpt::ros1bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::Occupancy for (unsigned int h = 0; h < des.info.height; h++) { const COccupancyGridMap2D::cellType* pSrc = src.getRow(h); + ASSERT_(pSrc); int8_t* pDes = &des.data[h * des.info.width]; for (unsigned int w = 0; w < des.info.width; w++) { - *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++); + if (as_costmap) + *pDes++ = static_cast(*pSrc++); + else + *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++); } } return true; diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/map.h b/libs/ros2bridge/include/mrpt/ros2bridge/map.h index 0b8717ce6b..78d9a7d17f 100644 --- a/libs/ros2bridge/include/mrpt/ros2bridge/map.h +++ b/libs/ros2bridge/include/mrpt/ros2bridge/map.h @@ -96,16 +96,22 @@ bool fromROS(const nav_msgs::msg::OccupancyGrid& src, mrpt::maps::COccupancyGrid * @return true on sucessful conversion, false on any error. * @param src * @param header + * @param as_costmap If set to true, gridmap cell values will be copied without changes + * (interpreted as int8_t instead of Log-odds) */ bool toROS( const mrpt::maps::COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& msg, - const std_msgs::msg::Header& header); + const std_msgs::msg::Header& header, + bool as_costmap = false); /** * converts mrpt object to ros msg * @return true on sucessful conversion, false on any error. */ -bool toROS(const mrpt::maps::COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& msg); +bool toROS( + const mrpt::maps::COccupancyGridMap2D& src, + nav_msgs::msg::OccupancyGrid& msg, + bool as_costmap = false); /** @} * @} diff --git a/libs/ros2bridge/src/map.cpp b/libs/ros2bridge/src/map.cpp index 95aae837bb..edd8bb475f 100644 --- a/libs/ros2bridge/src/map.cpp +++ b/libs/ros2bridge/src/map.cpp @@ -109,13 +109,15 @@ bool mrpt::ros2bridge::fromROS(const nav_msgs::msg::OccupancyGrid& src, COccupan bool mrpt::ros2bridge::toROS( const COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& des, - const std_msgs::msg::Header& header) + const std_msgs::msg::Header& header, + bool as_costmap) { des.header = header; - return toROS(src, des); + return toROS(src, des, as_costmap); } -bool mrpt::ros2bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& des) +bool mrpt::ros2bridge::toROS( + const COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& des, bool as_costmap) { des.info.width = src.getSizeX(); des.info.height = src.getSizeY(); @@ -135,10 +137,14 @@ bool mrpt::ros2bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::msg::Occu for (unsigned int h = 0; h < des.info.height; h++) { const COccupancyGridMap2D::cellType* pSrc = src.getRow(h); + ASSERT_(pSrc); int8_t* pDes = &des.data[h * des.info.width]; for (unsigned int w = 0; w < des.info.width; w++) { - *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++); + if (as_costmap) + *pDes++ = static_cast(*pSrc++); + else + *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++); } } return true; diff --git a/python/src/mrpt/img/CImage.cpp b/python/src/mrpt/img/CImage.cpp index 5fa2795907..b656f56e3d 100644 --- a/python/src/mrpt/img/CImage.cpp +++ b/python/src/mrpt/img/CImage.cpp @@ -342,7 +342,7 @@ void bind_mrpt_img_CImage(std::function< pybind11::module &(std::string const &n cl.def("scaleImage", [](mrpt::img::CImage const &o, class mrpt::img::CImage & a0, unsigned int const & a1, unsigned int const & a2) -> void { return o.scaleImage(a0, a1, a2); }, "", pybind11::arg("out_img"), pybind11::arg("width"), pybind11::arg("height")); cl.def("scaleImage", (void (mrpt::img::CImage::*)(class mrpt::img::CImage &, unsigned int, unsigned int, enum mrpt::img::TInterpolationMethod) const) &mrpt::img::CImage::scaleImage, "Scales this image to a new size, interpolating as needed, saving the new\n image in a different output object, or operating in-place if\n `out_img==this`. \n\n resize, rotateImage\n\nC++: mrpt::img::CImage::scaleImage(class mrpt::img::CImage &, unsigned int, unsigned int, enum mrpt::img::TInterpolationMethod) const --> void", pybind11::arg("out_img"), pybind11::arg("width"), pybind11::arg("height"), pybind11::arg("interp")); cl.def("rotateImage", [](mrpt::img::CImage const &o, class mrpt::img::CImage & a0, double const & a1, unsigned int const & a2, unsigned int const & a3) -> void { return o.rotateImage(a0, a1, a2, a3); }, "", pybind11::arg("out_img"), pybind11::arg("ang"), pybind11::arg("cx"), pybind11::arg("cy")); - cl.def("rotateImage", (void (mrpt::img::CImage::*)(class mrpt::img::CImage &, double, unsigned int, unsigned int, double) const) &mrpt::img::CImage::rotateImage, "Rotates the image by the given angle around the given center point, with\n an optional scale factor.\n \n\n resize, scaleImage\n\nC++: mrpt::img::CImage::rotateImage(class mrpt::img::CImage &, double, unsigned int, unsigned int, double) const --> void", pybind11::arg("out_img"), pybind11::arg("ang"), pybind11::arg("cx"), pybind11::arg("cy"), pybind11::arg("scale")); + cl.def("rotateImage", (void (mrpt::img::CImage::*)(class mrpt::img::CImage &, double, unsigned int, unsigned int, double) const) &mrpt::img::CImage::rotateImage, "Rotates the image by the given angle (in radians) around the given center point, with\n an optional scale factor.\n The output image will have the same size as the input, except if angle is exactly ±90 degrees,\n in which case a quick image rotation (switching height and widht) will be performed instead.\n \n\n resize, scaleImage\n\nC++: mrpt::img::CImage::rotateImage(class mrpt::img::CImage &, double, unsigned int, unsigned int, double) const --> void", pybind11::arg("out_img"), pybind11::arg("ang"), pybind11::arg("cx"), pybind11::arg("cy"), pybind11::arg("scale")); cl.def("setPixel", (void (mrpt::img::CImage::*)(int, int, size_t)) &mrpt::img::CImage::setPixel, "Changes the value of the pixel (x,y).\n Pixel coordinates starts at the left-top corner of the image, and start\n in (0,0).\n The meaning of the parameter \"color\" depends on the implementation: it\n will usually\n be a 24bit RGB value (0x00RRGGBB), but it can also be just a 8bit gray\n level.\n This method must support (x,y) values OUT of the actual image size\n without neither\n raising exceptions, nor leading to memory access errors.\n \n\n at, ptr\n\nC++: mrpt::img::CImage::setPixel(int, int, size_t) --> void", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("color")); cl.def("line", [](mrpt::img::CImage &o, int const & a0, int const & a1, int const & a2, int const & a3, const struct mrpt::img::TColor & a4) -> void { return o.line(a0, a1, a2, a3, a4); }, "", pybind11::arg("x0"), pybind11::arg("y0"), pybind11::arg("x1"), pybind11::arg("y1"), pybind11::arg("color")); cl.def("line", [](mrpt::img::CImage &o, int const & a0, int const & a1, int const & a2, int const & a3, const struct mrpt::img::TColor & a4, unsigned int const & a5) -> void { return o.line(a0, a1, a2, a3, a4, a5); }, "", pybind11::arg("x0"), pybind11::arg("y0"), pybind11::arg("x1"), pybind11::arg("y1"), pybind11::arg("color"), pybind11::arg("width")); diff --git a/python/src/mrpt/nav/holonomic/CHolonomicFullEval.cpp b/python/src/mrpt/nav/holonomic/CHolonomicFullEval.cpp index 862b8dc5c5..0ce75e34a7 100644 --- a/python/src/mrpt/nav/holonomic/CHolonomicFullEval.cpp +++ b/python/src/mrpt/nav/holonomic/CHolonomicFullEval.cpp @@ -33,7 +33,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::nav::CHolonomicFullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:57 +// mrpt::nav::CHolonomicFullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:34 struct PyCallBack_mrpt_nav_CHolonomicFullEval : public mrpt::nav::CHolonomicFullEval { using mrpt::nav::CHolonomicFullEval::CHolonomicFullEval; @@ -169,7 +169,7 @@ struct PyCallBack_mrpt_nav_CHolonomicFullEval : public mrpt::nav::CHolonomicFull } }; -// mrpt::nav::CHolonomicFullEval::TOptions file:mrpt/nav/holonomic/CHolonomicFullEval.h line:72 +// mrpt::nav::CHolonomicFullEval::TOptions file:mrpt/nav/holonomic/CHolonomicFullEval.h line:49 struct PyCallBack_mrpt_nav_CHolonomicFullEval_TOptions : public mrpt::nav::CHolonomicFullEval::TOptions { using mrpt::nav::CHolonomicFullEval::TOptions::TOptions; @@ -201,7 +201,7 @@ struct PyCallBack_mrpt_nav_CHolonomicFullEval_TOptions : public mrpt::nav::CHolo } }; -// mrpt::nav::CLogFileRecord_FullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:159 +// mrpt::nav::CLogFileRecord_FullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:136 struct PyCallBack_mrpt_nav_CLogFileRecord_FullEval : public mrpt::nav::CLogFileRecord_FullEval { using mrpt::nav::CLogFileRecord_FullEval::CLogFileRecord_FullEval; @@ -455,8 +455,8 @@ struct PyCallBack_mrpt_nav_CHolonomicND_TOptions : public mrpt::nav::CHolonomicN void bind_mrpt_nav_holonomic_CHolonomicFullEval(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::nav::CHolonomicFullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:57 - pybind11::class_, PyCallBack_mrpt_nav_CHolonomicFullEval, mrpt::nav::CAbstractHolonomicReactiveMethod> cl(M("mrpt::nav"), "CHolonomicFullEval", "Full evaluation of all possible directions within the discrete set of input\n directions.\n\n These are the optional parameters of the method which can be set by means of\n a configuration file passed to the constructor or to\n CHolonomicFullEval::initialize() or directly in \n\n \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n \n CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem"); + { // mrpt::nav::CHolonomicFullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:34 + pybind11::class_, PyCallBack_mrpt_nav_CHolonomicFullEval, mrpt::nav::CAbstractHolonomicReactiveMethod> cl(M("mrpt::nav"), "CHolonomicFullEval", "Full evaluation of all possible directions within the discrete set of input\n directions.\n\n These are the optional parameters of the method which can be set by means of\n a configuration file passed to the constructor or to\n CHolonomicFullEval::initialize() or directly in \n\n See [MRPT]/share/mrpt/config_files_navigation-ptgs for example configurations.\n\n \n CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem"); cl.def( pybind11::init( [](){ return new mrpt::nav::CHolonomicFullEval(); }, [](){ return new PyCallBack_mrpt_nav_CHolonomicFullEval(); } ), "doc"); cl.def( pybind11::init(), pybind11::arg("INI_FILE") ); @@ -474,7 +474,7 @@ void bind_mrpt_nav_holonomic_CHolonomicFullEval(std::function< pybind11::module cl.def("setTargetApproachSlowDownDistance", (void (mrpt::nav::CHolonomicFullEval::*)(const double)) &mrpt::nav::CHolonomicFullEval::setTargetApproachSlowDownDistance, "C++: mrpt::nav::CHolonomicFullEval::setTargetApproachSlowDownDistance(const double) --> void", pybind11::arg("dist")); cl.def("assign", (class mrpt::nav::CHolonomicFullEval & (mrpt::nav::CHolonomicFullEval::*)(const class mrpt::nav::CHolonomicFullEval &)) &mrpt::nav::CHolonomicFullEval::operator=, "C++: mrpt::nav::CHolonomicFullEval::operator=(const class mrpt::nav::CHolonomicFullEval &) --> class mrpt::nav::CHolonomicFullEval &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::nav::CHolonomicFullEval::TOptions file:mrpt/nav/holonomic/CHolonomicFullEval.h line:72 + { // mrpt::nav::CHolonomicFullEval::TOptions file:mrpt/nav/holonomic/CHolonomicFullEval.h line:49 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_nav_CHolonomicFullEval_TOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TOptions", "Algorithm options "); cl.def( pybind11::init( [](){ return new mrpt::nav::CHolonomicFullEval::TOptions(); }, [](){ return new PyCallBack_mrpt_nav_CHolonomicFullEval_TOptions(); } ) ); @@ -497,7 +497,7 @@ void bind_mrpt_nav_holonomic_CHolonomicFullEval(std::function< pybind11::module } } - { // mrpt::nav::CLogFileRecord_FullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:159 + { // mrpt::nav::CLogFileRecord_FullEval file:mrpt/nav/holonomic/CHolonomicFullEval.h line:136 pybind11::class_, PyCallBack_mrpt_nav_CLogFileRecord_FullEval, mrpt::nav::CHolonomicLogFileRecord> cl(M("mrpt::nav"), "CLogFileRecord_FullEval", "A class for storing extra information about the execution of\n CHolonomicFullEval navigation.\n \n\n CHolonomicFullEval, CHolonomicLogFileRecord"); cl.def( pybind11::init( [](){ return new mrpt::nav::CLogFileRecord_FullEval(); }, [](){ return new PyCallBack_mrpt_nav_CLogFileRecord_FullEval(); } ) ); cl.def_readwrite("selectedSector", &mrpt::nav::CLogFileRecord_FullEval::selectedSector); diff --git a/python/src/mrpt/nav/reactive/CLogFileRecord.cpp b/python/src/mrpt/nav/reactive/CLogFileRecord.cpp index b23c05004b..d1052620d4 100644 --- a/python/src/mrpt/nav/reactive/CLogFileRecord.cpp +++ b/python/src/mrpt/nav/reactive/CLogFileRecord.cpp @@ -242,7 +242,7 @@ void bind_mrpt_nav_reactive_CLogFileRecord(std::function< pybind11::module &(std } } - { // mrpt::nav::TWaypoint file:mrpt/nav/reactive/TWaypoint.h line:26 + { // mrpt::nav::TWaypoint file:mrpt/nav/reactive/TWaypoint.h line:27 pybind11::class_> cl(M("mrpt::nav"), "TWaypoint", "A single waypoint within TWaypointSequence. "); cl.def( pybind11::init( [](){ return new mrpt::nav::TWaypoint(); } ) ); cl.def( pybind11::init( [](mrpt::nav::TWaypoint const &o){ return new mrpt::nav::TWaypoint(o); } ) ); diff --git a/python/src/mrpt/nav/reactive/TWaypoint.cpp b/python/src/mrpt/nav/reactive/TWaypoint.cpp index ba0ac2ae43..c113d7ca33 100644 --- a/python/src/mrpt/nav/reactive/TWaypoint.cpp +++ b/python/src/mrpt/nav/reactive/TWaypoint.cpp @@ -421,7 +421,7 @@ struct PyCallBack_mrpt_nav_CWaypointsNavigator_TWaypointsNavigatorParams : publi void bind_mrpt_nav_reactive_TWaypoint(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::nav::TWaypointsRenderingParams file:mrpt/nav/reactive/TWaypoint.h line:92 + { // mrpt::nav::TWaypointsRenderingParams file:mrpt/nav/reactive/TWaypoint.h line:93 pybind11::class_> cl(M("mrpt::nav"), "TWaypointsRenderingParams", "used in getAsOpenglVisualization() "); cl.def( pybind11::init( [](){ return new mrpt::nav::TWaypointsRenderingParams(); } ) ); cl.def( pybind11::init( [](mrpt::nav::TWaypointsRenderingParams const &o){ return new mrpt::nav::TWaypointsRenderingParams(o); } ) ); @@ -438,7 +438,7 @@ void bind_mrpt_nav_reactive_TWaypoint(std::function< pybind11::module &(std::str cl.def_readwrite("show_labels", &mrpt::nav::TWaypointsRenderingParams::show_labels); cl.def("assign", (struct mrpt::nav::TWaypointsRenderingParams & (mrpt::nav::TWaypointsRenderingParams::*)(const struct mrpt::nav::TWaypointsRenderingParams &)) &mrpt::nav::TWaypointsRenderingParams::operator=, "C++: mrpt::nav::TWaypointsRenderingParams::operator=(const struct mrpt::nav::TWaypointsRenderingParams &) --> struct mrpt::nav::TWaypointsRenderingParams &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::nav::TWaypointSequence file:mrpt/nav/reactive/TWaypoint.h line:109 + { // mrpt::nav::TWaypointSequence file:mrpt/nav/reactive/TWaypoint.h line:110 pybind11::class_> cl(M("mrpt::nav"), "TWaypointSequence", "The struct for requesting navigation requests for a sequence of waypoints.\n Used in CWaypointsNavigator::navigateWaypoints().\n Users can directly fill in the list of waypoints manipulating the public\n field `waypoints`.\n \n"); cl.def( pybind11::init( [](){ return new mrpt::nav::TWaypointSequence(); } ) ); cl.def( pybind11::init( [](mrpt::nav::TWaypointSequence const &o){ return new mrpt::nav::TWaypointSequence(o); } ) ); @@ -451,7 +451,7 @@ void bind_mrpt_nav_reactive_TWaypoint(std::function< pybind11::module &(std::str cl.def("load", (void (mrpt::nav::TWaypointSequence::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::nav::TWaypointSequence::load, "Loads waypoints to a config file section \n\nC++: mrpt::nav::TWaypointSequence::load(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("c"), pybind11::arg("s")); cl.def("assign", (struct mrpt::nav::TWaypointSequence & (mrpt::nav::TWaypointSequence::*)(const struct mrpt::nav::TWaypointSequence &)) &mrpt::nav::TWaypointSequence::operator=, "C++: mrpt::nav::TWaypointSequence::operator=(const struct mrpt::nav::TWaypointSequence &) --> struct mrpt::nav::TWaypointSequence &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::nav::TWaypointStatus file:mrpt/nav/reactive/TWaypoint.h line:131 + { // mrpt::nav::TWaypointStatus file:mrpt/nav/reactive/TWaypoint.h line:132 pybind11::class_, mrpt::nav::TWaypoint> cl(M("mrpt::nav"), "TWaypointStatus", "A waypoint with an execution status. "); cl.def( pybind11::init( [](){ return new mrpt::nav::TWaypointStatus(); } ) ); cl.def( pybind11::init( [](mrpt::nav::TWaypointStatus const &o){ return new mrpt::nav::TWaypointStatus(o); } ) ); @@ -464,7 +464,7 @@ void bind_mrpt_nav_reactive_TWaypoint(std::function< pybind11::module &(std::str cl.def("getAsText", (std::string (mrpt::nav::TWaypointStatus::*)() const) &mrpt::nav::TWaypointStatus::getAsText, "Gets navigation params as a human-readable format \n\nC++: mrpt::nav::TWaypointStatus::getAsText() const --> std::string"); cl.def("assign", (struct mrpt::nav::TWaypointStatus & (mrpt::nav::TWaypointStatus::*)(const struct mrpt::nav::TWaypointStatus &)) &mrpt::nav::TWaypointStatus::operator=, "C++: mrpt::nav::TWaypointStatus::operator=(const struct mrpt::nav::TWaypointStatus &) --> struct mrpt::nav::TWaypointStatus &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::nav::TWaypointStatusSequence file:mrpt/nav/reactive/TWaypoint.h line:165 + { // mrpt::nav::TWaypointStatusSequence file:mrpt/nav/reactive/TWaypoint.h line:166 pybind11::class_> cl(M("mrpt::nav"), "TWaypointStatusSequence", "The struct for querying the status of waypoints navigation. Used in\n CWaypointsNavigator::getWaypointNavStatus().\n \n"); cl.def( pybind11::init( [](){ return new mrpt::nav::TWaypointStatusSequence(); } ) ); cl.def( pybind11::init( [](mrpt::nav::TWaypointStatusSequence const &o){ return new mrpt::nav::TWaypointStatusSequence(o); } ) ); @@ -473,6 +473,10 @@ void bind_mrpt_nav_reactive_TWaypoint(std::function< pybind11::module &(std::str cl.def_readwrite("final_goal_reached", &mrpt::nav::TWaypointStatusSequence::final_goal_reached); cl.def_readwrite("waypoint_index_current_goal", &mrpt::nav::TWaypointStatusSequence::waypoint_index_current_goal); cl.def_readwrite("last_robot_pose", &mrpt::nav::TWaypointStatusSequence::last_robot_pose); + cl.def_readwrite("robot_move_seg", &mrpt::nav::TWaypointStatusSequence::robot_move_seg); + cl.def_readwrite("prevDist2target", &mrpt::nav::TWaypointStatusSequence::prevDist2target); + cl.def_readwrite("was_aligning", &mrpt::nav::TWaypointStatusSequence::was_aligning); + cl.def_readwrite("is_aligning", &mrpt::nav::TWaypointStatusSequence::is_aligning); cl.def("getAsText", (std::string (mrpt::nav::TWaypointStatusSequence::*)() const) &mrpt::nav::TWaypointStatusSequence::getAsText, "Ctor with default values \n\n Gets navigation params as a human-readable format \n\nC++: mrpt::nav::TWaypointStatusSequence::getAsText() const --> std::string"); cl.def("getAsOpenglVisualization", [](mrpt::nav::TWaypointStatusSequence const &o, class mrpt::opengl::CSetOfObjects & a0) -> void { return o.getAsOpenglVisualization(a0); }, "", pybind11::arg("obj")); cl.def("getAsOpenglVisualization", (void (mrpt::nav::TWaypointStatusSequence::*)(class mrpt::opengl::CSetOfObjects &, const struct mrpt::nav::TWaypointsRenderingParams &) const) &mrpt::nav::TWaypointStatusSequence::getAsOpenglVisualization, "Renders the sequence of waypoints (previous contents of `obj` are\n cleared) \n\nC++: mrpt::nav::TWaypointStatusSequence::getAsOpenglVisualization(class mrpt::opengl::CSetOfObjects &, const struct mrpt::nav::TWaypointsRenderingParams &) const --> void", pybind11::arg("obj"), pybind11::arg("params")); @@ -515,6 +519,7 @@ void bind_mrpt_nav_reactive_TWaypoint(std::function< pybind11::module &(std::str cl.def_readwrite("min_timesteps_confirm_skip_waypoints", &mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::min_timesteps_confirm_skip_waypoints); cl.def_readwrite("waypoint_angle_tolerance", &mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::waypoint_angle_tolerance); cl.def_readwrite("multitarget_look_ahead", &mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::multitarget_look_ahead); + cl.def_readwrite("minimum_target_approach_per_step", &mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::minimum_target_approach_per_step); cl.def("loadFromConfigFile", (void (mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFile, "C++: mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFile(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("c"), pybind11::arg("s")); cl.def("saveToConfigFile", (void (mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::*)(class mrpt::config::CConfigFileBase &, const std::string &) const) &mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile, "C++: mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile(class mrpt::config::CConfigFileBase &, const std::string &) const --> void", pybind11::arg("c"), pybind11::arg("s")); cl.def("assign", (struct mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams & (mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::*)(const struct mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams &)) &mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::operator=, "C++: mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::operator=(const struct mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams &) --> struct mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams &", pybind11::return_value_policy::automatic, pybind11::arg("")); diff --git a/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp b/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp index ea3c56c281..c6d9e873ee 100644 --- a/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp +++ b/python/src/mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.cpp @@ -339,7 +339,7 @@ void bind_mrpt_nav_tpspace_CParameterizedTrajectoryGenerator(std::function< pybi cl.def("assign", (struct mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput & (mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::*)(const struct mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput &)) &mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::operator=, "C++: mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::operator=(const struct mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput &) --> struct mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput file:mrpt/nav/holonomic/CAbstractHolonomicReactiveMethod.h line:61 + { // mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput file:mrpt/nav/holonomic/CAbstractHolonomicReactiveMethod.h line:65 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "NavOutput", "Output for CAbstractHolonomicReactiveMethod::navigate() "); cl.def( pybind11::init( [](){ return new mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput(); } ) ); diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi index a8db2db8ff..3c0a4be454 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/nav.pyi @@ -23,7 +23,7 @@ class CAbstractHolonomicReactiveMethod(mrpt.pymrpt.mrpt.serialization.CSerializa maxObstacleDist: float maxRobotSpeed: float obstacles: List[float] - targets: Any + targets: List[mrpt.pymrpt.mrpt.math.TPose2D] @overload def __init__(self) -> None: ... @overload @@ -595,7 +595,7 @@ class CLogFileRecord(mrpt.pymrpt.mrpt.serialization.CSerializable): PTG_desc: str TP_Obstacles: Any TP_Robot: Any - TP_Targets: Any + TP_Targets: List[mrpt.pymrpt.mrpt.math.TPose2D] clearance: ClearanceDiagram desiredDirection: float desiredSpeed: float @@ -1536,6 +1536,7 @@ class CWaypointsNavigator(CAbstractNavigator): class TWaypointsNavigatorParams(mrpt.pymrpt.mrpt.config.CLoadableOptions): max_distance_to_allow_skip_waypoint: float min_timesteps_confirm_skip_waypoints: int + minimum_target_approach_per_step: float multitarget_look_ahead: int waypoint_angle_tolerance: float @overload @@ -1971,8 +1972,12 @@ class TWaypointStatus(TWaypoint): class TWaypointStatusSequence: final_goal_reached: bool + is_aligning: bool last_robot_pose: mrpt.pymrpt.mrpt.math.TPose2D + prevDist2target: float + robot_move_seg: mrpt.pymrpt.mrpt.math.TSegment2D timestamp_nav_started: mrpt.pymrpt.std.chrono.time_point_mrpt_Clock_std_chrono_duration_long_std_ratio_1_10000000_t + was_aligning: bool waypoint_index_current_goal: int waypoints: List[TWaypointStatus] @overload diff --git a/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini b/share/mrpt/config_files/navigation-ptgs/reactive2d_config.ini index 8254841c12..c818662165 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] @@ -63,15 +63,18 @@ max_deletion_ratio = 0.400000 // (Def [CHolonomicFullEval] -# [0]=Free space -# [1]=Dist. in sectors -# [2]=Closer to target (Euclidean) +# [0]=Collision-free space +# [1]=Nearnest path point to target (*=1/10 if way blocked by obstacles) +# [2]=Distance: end of path to target (*=1/10 if way blocked by obstacles) # [3]=Hysteresis -# [4]=clearance along path -# [5]=like [2] but without being decimated if path obstructed -# [6]=closeness of k_target -factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1 ] -factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0 ] +# [4]=clearance of nearby paths +# [5]=Identical to [2], without the *=1/10 +# [6]=Direct distance in PTG trajectories to target +# [7]=Match final heading with target heading. 1.0=perfect phi aligment, 0.0=180deg error (Added in MRPT 2.14.3) +# [0] [1] [2] [3] [4] [5] [6] [7] +factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1, 0.5 ] +factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0, 0 ] + TOO_CLOSE_OBSTACLE = 0.150000 // Directions with collision-free distances below this threshold are not elegible. TARGET_SLOW_APPROACHING_DISTANCE = 0.100000 // Start to reduce speed when closer than this to target. diff --git a/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini b/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini index eb30ea454c..b1725179ae 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini @@ -65,17 +65,20 @@ max_deletion_ratio = 0.400000 // (Def [CHolonomicFullEval] -# [0]=Free space -# [1]=Dist. in sectors -# [2]=Closer to target (Euclidean) +# [0]=Collision-free space +# [1]=Nearnest path point to target (*=1/10 if way blocked by obstacles) +# [2]=Distance: end of path to target (*=1/10 if way blocked by obstacles) # [3]=Hysteresis -# [5]=like [2] but without being decimated if path obstructed -# [6]=closeness to target_k -factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1 ] -factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0 ] +# [4]=clearance of nearby paths +# [5]=Identical to [2], without the *=1/10 +# [6]=Direct distance in PTG trajectories to target +# [7]=Match final heading with target heading. 1.0=perfect phi aligment, 0.0=180deg error (Added in MRPT 2.14.3) +# [0] [1] [2] [3] [4] [5] [6] [7] +factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1, 0.5 ] +factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0, 0 ] TOO_CLOSE_OBSTACLE = 0.15 // Directions with collision-free distances below this threshold are not elegible. -TARGET_SLOW_APPROACHING_DISTANCE = 0.300000 // Start to reduce speed when closer than this to target. +TARGET_SLOW_APPROACHING_DISTANCE = 0.600000 // Start to reduce speed when closer than this to target. OBSTACLE_SLOW_DOWN_DISTANCE = 0.150000 // Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max distance) HYSTERESIS_SECTOR_COUNT = 5.000000 // Range of `sectors` (directions) for hysteresis over successive timesteps LOG_SCORE_MATRIX = false // Save the entire score matrix in log files diff --git a/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini b/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini index 7a9c811bd5..100db60fde 100644 --- a/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini +++ b/share/mrpt/config_files/navigation-ptgs/reactivenav-app-config.ini @@ -67,18 +67,21 @@ min_dist = 0.100000 angle_tolerance = 5.000000 too_old_seconds = 1.000000 previous_keyframes = 1 // (Default: 1) How many previous keyframes will be compared with the latest pointcloud. -max_deletion_ratio = 0.400000 // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it'd be too suspicious and may indicate a failure of this filter. +max_deletion_ratio = 0.400000 // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it would be too suspicious and may indicate a failure of this filter. [DIFF_CHolonomicFullEval] -# [0]=Free space -# [1]=Dist. in sectors -# [2]=Closer to target (Euclidean) +# [0]=Collision-free space +# [1]=Nearnest path point to target (*=1/10 if way blocked by obstacles) +# [2]=Distance: end of path to target (*=1/10 if way blocked by obstacles) # [3]=Hysteresis -# [4]=clearance along path -# [6]=closeness to target_k -factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1 ] -factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0 ] +# [4]=clearance of nearby paths +# [5]=Identical to [2], without the *=1/10 +# [6]=Direct distance in PTG trajectories to target +# [7]=Match final heading with target heading. 1.0=perfect phi aligment, 0.0=180deg error (Added in MRPT 2.14.3) +# [0] [1] [2] [3] [4] [5] [6] [7] +factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1, 0.05 ] +factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0, 0 ] TOO_CLOSE_OBSTACLE = 0.150000 // Directions with collision-free distances below this threshold are not elegible. TARGET_SLOW_APPROACHING_DISTANCE = 1.0 // Start to reduce speed when closer than this to target. [m] @@ -92,7 +95,7 @@ gap_width_ratio_threshold = 0.20 // Rati PHASE_COUNT = 2 // Number of evaluation phases to run (params for each phase below) PHASE1_FACTORS = [1 2 3] // Indices of the factors above to be considered in this phase PHASE1_THRESHOLD = 0.5 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`) -PHASE2_FACTORS = [2 0 4] // Indices of the factors above to be considered in this phase +PHASE2_FACTORS = [2 0 4 7] // Indices of the factors above to be considered in this phase PHASE2_THRESHOLD = 0.5 // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`) @@ -270,18 +273,21 @@ min_dist = 0.100000 angle_tolerance = 5.000000 too_old_seconds = 1.000000 previous_keyframes = 1 // (Default: 1) How many previous keyframes will be compared with the latest pointcloud. -max_deletion_ratio = 0.400000 // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it'd be too suspicious and may indicate a failure of this filter. +max_deletion_ratio = 0.400000 // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it d be too suspicious and may indicate a failure of this filter. [HOLO_CHolonomicFullEval] -# [0]=Free space -# [1]=Dist. in sectors -# [2]=Closer to target (Euclidean) +# [0]=Collision-free space +# [1]=Nearnest path point to target (*=1/10 if way blocked by obstacles) +# [2]=Distance: end of path to target (*=1/10 if way blocked by obstacles) # [3]=Hysteresis -# [4]=clearance along path -# [6]=closeness to target_k -factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1 ] -factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0 ] +# [4]=clearance of nearby paths +# [5]=Identical to [2], without the *=1/10 +# [6]=Direct distance in PTG trajectories to target +# [7]=Match final heading with target heading. 1.0=perfect phi aligment, 0.0=180deg error (Added in MRPT 2.14.3) +# [0] [1] [2] [3] [4] [5] [6] [7] +factorWeights = [0.25 , 0.1 , 1 , 0.05 , 0.5, 1, 1, 0.5 ] +factorNormalizeOrNot = [0 , 0 , 0 , 0 , 1 , 1, 0, 0 ] TOO_CLOSE_OBSTACLE = 0.05 // Directions with collision-free distances below this threshold are not elegible. TARGET_SLOW_APPROACHING_DISTANCE = 2.0 // Start to reduce speed when closer than this to target. [m] diff --git a/version_prefix.txt b/version_prefix.txt index 6bbefbc2b6..84e2e29635 100644 --- a/version_prefix.txt +++ b/version_prefix.txt @@ -1,4 +1,4 @@ -2.14.2 +2.14.3 # IMPORTANT: This file is parsed by CMake, don't add any comment to # the first line. # This file is used in both Windows and Linux scripts to automatically