Merge branch 'develop'

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))
       {

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();

apps/holonomic-navigator-demo/holonomic_navigator_demoMain.h

@@ -169,7 +169,7 @@ class holonomic_navigator_demoFrame : public wxFrame
 
   std::unique_ptr<mrpt::nav::CAbstractHolonomicReactiveMethod> 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;

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<mrpt::opengl::CPointCloud>(gl_trg_r);
+        gl_trgCorners = std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(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<mrpt::opengl::CPointCloud>(scene->getByName("tp_target"));
+          std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(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())

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
 

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:

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

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(

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:

libs/math/include/mrpt/math/TPose2D.h

@@ -36,10 +36,11 @@ struct TPose2D : public TPoseOrPoint, public internal::ProvideStaticResize<TPose
   /** Returns the identity transformation */
   static constexpr TPose2D Identity() { return TPose2D(); }
 
-  /** Implicit constructor from TPoint2D. Zeroes the phi coordinate.
+  /** Explicit constructor from TPoint2D. Zeroes the phi coordinate.
    * \sa TPoint2D
    */
-  TPose2D(const TPoint2D& p);
+  explicit TPose2D(const TPoint2D& p);
+
   /**
    * Constructor from TPoint3D, losing information. Zeroes the phi
    * coordinate.

libs/math/include/mrpt/math/TPose3D.h

@@ -46,27 +46,24 @@ struct TPose3D : public TPoseOrPoint, public internal::ProvideStaticResize<TPose
    * \sa TPoint2D
    */
   TPose3D(const TPoint2D& p);
-  /**
-   * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi,
+
+  /** Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi,
    * zeroing all the unprovided information.
    * \sa TPose2D
    */
   TPose3D(const TPose2D& p);
-  /**
-   * Implicit constructor from TPoint3D. Zeroes angular information.
+
+  /** Explicit  constructor from TPoint3D. Zeroes angular information.
    * \sa TPoint3D
    */
-  TPose3D(const TPoint3D& p);
-  /**
-   * Constructor from coordinates.
-   */
+  explicit TPose3D(const TPoint3D& p);
+
+  /** Constructor from coordinates */
   constexpr TPose3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll) :
       x(_x), y(_y), z(_z), yaw(_yaw), pitch(_pitch), roll(_roll)
   {
   }
-  /**
-   * Default fast constructor. Initializes to zeros.
-   */
+  /** Default fast constructor. Initializes to zeros. */
   constexpr TPose3D() = default;
 
   /** See fromString() for a description of the expected string format. */

libs/math/src/TLine2D.cpp

@@ -80,9 +80,8 @@ void TLine2D::getAsPose2D(TPose2D& outPose) const
 void TLine2D::getAsPose2DForcingOrigin(const TPoint2D& origin, TPose2D& outPose) const
 {
   if (!contains(origin)) throw std::logic_error("Base point is not contained in the line");
-  outPose = origin;
   // Line's director vector is (-coefs[1],coefs[0]).
-  outPose.phi = atan2(coefs[0], -coefs[1]);
+  outPose = {origin.x, origin.y, atan2(coefs[0], -coefs[1])};
 }
 TLine2D::TLine2D(const TPoint2D& p1, const TPoint2D& p2)
 {

libs/nav/include/mrpt/nav/holonomic/CAbstractHolonomicReactiveMethod.h

@@ -9,7 +9,7 @@
 #pragma once
 
 #include <mrpt/config/CConfigFileBase.h>
-#include <mrpt/math/TPoint2D.h>
+#include <mrpt/math/TPose2D.h>
 #include <mrpt/nav/holonomic/ClearanceDiagram.h>
 #include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
 #include <mrpt/serialization/CSerializable.h>
@@ -33,44 +33,49 @@ 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
      * PTG-based navigation system, they are "pseudometers", normalized to
      * the range [0,1].
      */
     std::vector<double> obstacles;
+
     /** Relative location (x,y) of target point(s). In the same units than
      * `obstacles`. If many, last targets have higher priority. */
-    std::vector<mrpt::math::TPoint2D> targets;
+    std::vector<mrpt::math::TPose2D> 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;

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
  */