Implement preferred waypoint cost eval

MRPT · Aug 25, 2022 · 908709e · 908709e
1 parent cafc13f
commit 908709e
Show file tree

Hide file tree

Showing 5 changed files with 239 additions and 8 deletions.
diff --git a/apps/path-planner-cli/path-planner-cli.cpp b/apps/path-planner-cli/path-planner-cli.cpp
@@ -16,6 +16,7 @@
 #include <mrpt/system/os.h>  // plugins
 #include <mrpt/version.h>
 #include <selfdriving/algos/CostEvaluatorCostMap.h>
+#include <selfdriving/algos/CostEvaluatorPreferredWaypoint.h>
 #include <selfdriving/algos/TPS_RRTstar.h>
 #include <selfdriving/algos/viz.h>
 
@@ -216,8 +217,9 @@ static void do_plan_path()
     if (arg_costMap.isSet())
     {
         // cost map:
-        const auto costMapParams = selfdriving::CostEvaluatorCostMap::Parameters::FromYAML(
-            mrpt::containers::yaml::FromFile(arg_costMap.getValue()));
+        const auto costMapParams =
+            selfdriving::CostEvaluatorCostMap::Parameters::FromYAML(
+                mrpt::containers::yaml::FromFile(arg_costMap.getValue()));
 
         auto costmap =
             selfdriving::CostEvaluatorCostMap::FromStaticPointObstacles(
@@ -226,6 +228,19 @@ static void do_plan_path()
         planner->costEvaluators_.push_back(costmap);
     }
 
+    MRPT_TODO("WIP: add new cli flag!");
+#if 0
+    {
+        const auto p =
+            selfdriving::CostEvaluatorPreferredWaypoint::Parameters();
+
+        auto costEval = selfdriving::CostEvaluatorPreferredWaypoint::Create();
+        costEval->setPreferredWaypoints({{-1., 8.0}});
+
+        planner->costEvaluators_.push_back(costEval);
+    }
+#endif
+
     // verbosity level:
     planner->setMinLoggingLevel(
         mrpt::typemeta::TEnumType<mrpt::system::VerbosityLevel>::name2value(

diff --git a/libselfdriving/include/selfdriving/algos/CostEvaluatorCostMap.h b/libselfdriving/include/selfdriving/algos/CostEvaluatorCostMap.h
@@ -12,6 +12,9 @@
 
 namespace selfdriving
 {
+/** Defines higher costs to paths that pass closer to obstacles.
+ *
+ */
 class CostEvaluatorCostMap : public CostEvaluator
 {
     DEFINE_MRPT_OBJECT(CostEvaluatorCostMap, selfdriving)

diff --git a/libselfdriving/include/selfdriving/algos/CostEvaluatorPreferredWaypoint.h b/libselfdriving/include/selfdriving/algos/CostEvaluatorPreferredWaypoint.h
@@ -0,0 +1,66 @@
+/* -------------------------------------------------------------------------
+ *   SelfDriving C++ library based on PTGs and mrpt-nav
+ * Copyright (C) 2019-2022 Jose Luis Blanco, University of Almeria
+ * See LICENSE for license information.
+ * ------------------------------------------------------------------------- */
+
+#pragma once
+
+#include <mrpt/containers/CDynamicGrid.h>
+#include <mrpt/maps/CSimplePointsMap.h>
+#include <selfdriving/algos/CostEvaluator.h>
+
+namespace selfdriving
+{
+/** Defines lower (negative) costs to paths that pass closer to one or more
+ * "preferred waypoints" defined by 2D coordinates (x,y)
+ */
+class CostEvaluatorPreferredWaypoint : public CostEvaluator
+{
+    DEFINE_MRPT_OBJECT(CostEvaluatorPreferredWaypoint, selfdriving)
+
+   public:
+    CostEvaluatorPreferredWaypoint() = default;
+    ~CostEvaluatorPreferredWaypoint();
+
+    struct Parameters
+    {
+        Parameters();
+        ~Parameters();
+
+        static Parameters FromYAML(const mrpt::containers::yaml& c);
+
+        double waypointInfluenceRadius = 1.5;  //!< [m]
+        double costScale               = 10.0;
+
+        /** If enabled (default), the cost of a path segment (MoveEdgeSE2_TPS)
+         * will be the average of the set of pointwise evaluations along it.
+         * Otherwise the maximum (negative) cost will be kept instead.
+         */
+        bool useAverageOfPath = true;
+
+        mrpt::containers::yaml as_yaml();
+        void                   load_from_yaml(const mrpt::containers::yaml& c);
+    };
+
+    /** Method parameters. Can be freely modified at any time after
+     * construction. */
+    Parameters params_;
+
+    /** The preferred waypoints must be defined by means of this method. */
+    void setPreferredWaypoints(const std::vector<mrpt::math::TPoint2D>& pts);
+
+    /** Evaluate cost of move-tree edge */
+    double operator()(const MoveEdgeSE2_TPS& edge) const override;
+
+    mrpt::opengl::CSetOfObjects::Ptr get_visualization() const override;
+
+    const Parameters& params() const { return params_; }
+
+   private:
+    double eval_single_pose(const mrpt::math::TPose2D& p) const;
+
+    mrpt::maps::CSimplePointsMap waypoints_;
+};
+
+}  // namespace selfdriving
diff --git a/libselfdriving/src/algos/CostEvaluatorPreferredWaypoint.cpp b/libselfdriving/src/algos/CostEvaluatorPreferredWaypoint.cpp
@@ -0,0 +1,144 @@
+/* -------------------------------------------------------------------------
+ *   SelfDriving C++ library based on PTGs and mrpt-nav
+ * Copyright (C) 2019-2022 Jose Luis Blanco, University of Almeria
+ * See LICENSE for license information.
+ * ------------------------------------------------------------------------- */
+
+#include <mrpt/opengl/CDisk.h>
+#include <selfdriving/algos/CostEvaluatorPreferredWaypoint.h>
+
+using namespace selfdriving;
+
+IMPLEMENTS_MRPT_OBJECT(
+    CostEvaluatorPreferredWaypoint, CostEvaluator, selfdriving)
+
+CostEvaluatorPreferredWaypoint::Parameters::Parameters() = default;
+
+CostEvaluatorPreferredWaypoint::Parameters::~Parameters() = default;
+
+CostEvaluatorPreferredWaypoint::Parameters
+    CostEvaluatorPreferredWaypoint::Parameters::FromYAML(
+        const mrpt::containers::yaml& c)
+{
+    CostEvaluatorPreferredWaypoint::Parameters p;
+    p.load_from_yaml(c);
+    return p;
+}
+
+mrpt::containers::yaml CostEvaluatorPreferredWaypoint::Parameters::as_yaml()
+{
+    mrpt::containers::yaml c = mrpt::containers::yaml::Map();
+
+    MCP_SAVE(c, waypointInfluenceRadius);
+    MCP_SAVE(c, costScale);
+    MCP_SAVE(c, useAverageOfPath);
+
+    return c;
+}
+void CostEvaluatorPreferredWaypoint::Parameters::load_from_yaml(
+    const mrpt::containers::yaml& c)
+{
+    ASSERT_(c.isMap());
+
+    MCP_LOAD_REQ(c, waypointInfluenceRadius);
+    MCP_LOAD_REQ(c, costScale);
+    MCP_LOAD_REQ(c, useAverageOfPath);
+}
+
+CostEvaluatorPreferredWaypoint::~CostEvaluatorPreferredWaypoint() = default;
+
+void CostEvaluatorPreferredWaypoint::setPreferredWaypoints(
+    const std::vector<mrpt::math::TPoint2D>& pts)
+{
+    waypoints_.clear();
+    for (const auto& pt : pts) waypoints_.insertPoint(pt.x, pt.y);
+
+    // build 2D KD-tree now:
+    waypoints_.kdTreeEnsureIndexBuilt2D();
+}
+
+double CostEvaluatorPreferredWaypoint::operator()(
+    const MoveEdgeSE2_TPS& edge) const
+{
+    double cost = .0;
+    size_t n    = 0;
+
+    auto lambdaAddPose = [this, &cost, &n](const mrpt::math::TPose2D& p) {
+        const auto c = eval_single_pose(p);
+        ASSERT_GE_(c, .0);
+
+        if (params_.useAverageOfPath)
+        {
+            cost += c;
+            ++n;
+        }
+        else
+        {
+            if (c >= cost)
+            {
+                cost = c;
+                n    = 1;
+            }
+        }
+    };
+
+    // interpolated vs goal-end segments:
+    if (edge.interpolatedPath.has_value())
+    {
+        ASSERT_(!edge.interpolatedPath->empty());
+
+        for (const auto& p : *edge.interpolatedPath)
+            lambdaAddPose(edge.stateFrom.pose + p);
+    }
+    else
+    {
+        lambdaAddPose(edge.stateFrom.pose);
+        lambdaAddPose(edge.stateTo.pose);
+    }
+
+    ASSERT_(n);
+
+    return cost / n;
+}
+
+double CostEvaluatorPreferredWaypoint::eval_single_pose(
+    const mrpt::math::TPose2D& p) const
+{
+    std::vector<std::pair<size_t, float>> nearWps;  // indices-squaredDistances
+
+    const auto inflRadius = params_.waypointInfluenceRadius;
+
+    waypoints_.kdTreeRadiusSearch2D(
+        p.x, p.y, mrpt::square(inflRadius), nearWps);
+
+    double cost = params_.costScale;
+    for (const auto& wpIdxSqrDist : nearWps)
+    {
+        const float dist = sqrt(wpIdxSqrDist.second);
+        if (dist > inflRadius || dist < 0) continue;
+
+        // Decrease cost: prefer to pass thru these areas.
+        cost -= params_.costScale * (1.0f - sqrt(dist / inflRadius));
+    }
+
+    return std::max(.0, cost);
+}
+
+mrpt::opengl::CSetOfObjects::Ptr
+    CostEvaluatorPreferredWaypoint::get_visualization() const
+{
+    auto glObjs = mrpt::opengl::CSetOfObjects::Create();
+
+    for (size_t i = 0; i < waypoints_.size(); i++)
+    {
+        float x, y;
+        waypoints_.getPoint(i, x, y);
+
+        auto glDisk = mrpt::opengl::CDisk::Create();
+        glDisk->setColor_u8(0x00, 0x00, 0xff, 0x50);
+        glDisk->setDiskRadius(params_.waypointInfluenceRadius);
+        glDisk->setLocation(x, y, 0);
+        glObjs->insert(glDisk);
+    }
+    return glObjs;
+}
diff --git a/libselfdriving/src/registerClasses.cpp b/libselfdriving/src/registerClasses.cpp
@@ -6,23 +6,26 @@
 
 #include <mrpt/core/initializer.h>
 #include <selfdriving/algos/CostEvaluatorCostMap.h>
+#include <selfdriving/algos/CostEvaluatorPreferredWaypoint.h>
 #include <selfdriving/algos/TPS_Astar.h>
 #include <selfdriving/algos/TPS_RRTstar.h>
 #include <selfdriving/interfaces/VehicleMotionInterface.h>
 
 MRPT_INITIALIZER(selfdriving_register)
 {
     using namespace selfdriving;
+    using mrpt::rtti::registerClass;
 
     // Costs:
-    mrpt::rtti::registerClass(CLASS_ID(CostEvaluator));
-    mrpt::rtti::registerClass(CLASS_ID(CostEvaluatorCostMap));
+    registerClass(CLASS_ID(CostEvaluator));
+    registerClass(CLASS_ID(CostEvaluatorCostMap));
+    registerClass(CLASS_ID(CostEvaluatorPreferredWaypoint));
 
     // Planners:
-    mrpt::rtti::registerClass(CLASS_ID(Planner));
-    mrpt::rtti::registerClass(CLASS_ID(TPS_Astar));
-    mrpt::rtti::registerClass(CLASS_ID(TPS_RRTstar));
+    registerClass(CLASS_ID(Planner));
+    registerClass(CLASS_ID(TPS_Astar));
+    registerClass(CLASS_ID(TPS_RRTstar));
 
     // Interfaces:
-    mrpt::rtti::registerClass(CLASS_ID(VehicleMotionInterface));
+    registerClass(CLASS_ID(VehicleMotionInterface));
 }