adjust `getPoseFromPlan` method of the planner

[rayvburn]

The problem with the current approach is when the goal is placed on the other side of the wall that the robot is going along. In such a situation, the robot's global goal will be considered as a local goal; therefore, model-based trajectories will most likely cut through the wall.

Preliminary tests were performed with the method posted below, but the problem is that the global plan is not pruned properly. Therefore, the is an offset between the robot's newest pose pose_ and the first pose in the global plan global_plan_.at(0).

The main idea is to compute squared Eucl. distances between subsequent poses of the plan.

diff --git a/src/hubero_planner.cpp b/src/hubero_planner.cpp
index 72c3ba9..b828d24 100644
--- a/src/hubero_planner.cpp
+++ b/src/hubero_planner.cpp
@@ -865,18 +865,58 @@ Pose HuberoPlanner::getPoseFromPlan(const double& dist_from_current_pose, bool a
    // for faster computation - power once instead of square in each iteration
    double dist_target_sq = dist_from_current_pose * dist_from_current_pose;

+   double dist_sum_along_path = 0.0;
+
+   printf("Plan first pose {%8.4f, %8.4f},  robot pose {%8.4f, %8.4f}\r\n",
+       global_plan_.at(0).pose.position.x, global_plan_.at(0).pose.position.y,
+       pose_.getX(), pose_.getY()
+   );
+
+
+
+   // typically, the global plan is not updated as frequent as the pose, therefore there will be an offset at the start
+   double dist_sq_init_offset = computeSqDist(
+       global_plan_.at(0).pose.position.x,
+       global_plan_.at(0).pose.position.y,
+       pose_.getX(),
+       pose_.getY()
+   );
+   dist_sum_along_path = -dist_sq_init_offset;
+
+
+
    // find a point far enough so the social force can drive the robot towards instead of produce oscillations
    for (
-       std::vector<geometry_msgs::PoseStamped>::const_iterator it = global_plan_.begin();
+       // NOTE: starting from the [1] instead of [0]
+       std::vector<geometry_msgs::PoseStamped>::const_iterator it = std::next(global_plan_.begin());
        it != global_plan_.end();
        ++it
    ) {
-       double dist_sq = computeSqDist(pose_.getX(), pose_.getY(), it->pose.position.x, it->pose.position.y);
-       if (dist_sq > dist_target_sq) {
+       // the iterator is always valid as we start from the second element
+       auto it_prev = std::prev(it);
+       // additionally, the first pose will be
+       // TODO
+
+       double dist_sq = computeSqDist(
+           it_prev->pose.position.x,
+           it_prev->pose.position.y,
+           it->pose.position.x,
+           it->pose.position.y
+       );
+       dist_sum_along_path += dist_sq;
+       printf("\t(%3lu) {%8.4f, %8.4f}, newest %8.4f,  sum %8.4f, threshold %8.4f\r\n",
+           std::distance(global_plan_.begin(), it),
+           it->pose.position.x,
+           it->pose.position.y,
+           dist_sq,
+           dist_sum_along_path,
+           dist_target_sq
+       );
+       if (dist_sum_along_path > dist_target_sq) {
            // global path does not account orientation - interpolate
-           auto it_prev = std::prev(it);
+           // auto it_prev = std::prev(it);
            // make sure that iterator is valid (within vector range)
-           if (it_prev >= global_plan_.begin()) {
+           // if (it_prev >= global_plan_.begin()) {
                // orientation of the local goal is determined based on direction that allows to pass
                // through 2 consecutive path points (local goal and the previous one)
                double dir_v_angle = computeDirFromPositions(
@@ -885,25 +925,20 @@ Pose HuberoPlanner::getPoseFromPlan(const double& dist_from_current_pose, bool a
                    it->pose.position.x,
                    it->pose.position.y
                );
+               printf("Finishing (0) Safely\r\n");
+               // printf("\t%sPose: New method {%8.4f, %8.4f, %8.4f}, Old method {%8.4f, %8.4f, %8.4f}%s\r\n",
+               //  (
+               //      (std::abs(it->pose.position.x - pose_selected_old.getX()) > 1e-04)
+               //      || (std::abs(it->pose.position.y - pose_selected_old.getY()) > 1e-04)
+               //  ) ? "\x1B[31m" : "\x1B[32m",
+               //  it->pose.position.x, it->pose.position.y, dir_v_angle,
+               //  pose_selected_old.getX(), pose_selected_old.getY(), pose_selected_old.getYaw(),
+               //  "\x1B[0m"
+               // );
                return Pose(
                    Vector(it->pose.position.x, it->pose.position.y, it->pose.position.z),
                    Quaternion(dir_v_angle)
                );
-           }
-           // fallback - cannot determine orientation, global goal orientation is assumed
-           return Pose(
-               Vector(
-                   it->pose.position.x,
-                   it->pose.position.y,
-                   it->pose.position.z
-               ),
-               Quaternion(
-                   global_plan_.back().pose.orientation.x,
-                   global_plan_.back().pose.orientation.y,
-                   global_plan_.back().pose.orientation.z,
-                   global_plan_.back().pose.orientation.w
-               )
-           );
        }
    }
    // description of the conditions used below:

[rayvburn]

Fixed (see goal_initiation, goal_local, and goal)