planner_cspace: start planning from expected robot pose #717

nhatao · 2023-09-05T10:21:14Z

This PR adds keep_a_part_of_previous_path option. As described in the diagram above, this option is useful when the robot's speed is high or the environment is dynamic.
In addition, the algorithm to preserve paths including switchback points is integrated. When it is expected the robot reaches a switchback point within sw_wait seconds, the path until the switchback point is preserved and the new path planning starts from the switchback point.

codecov-commenter · 2023-09-08T13:06:50Z

Codecov Report

Merging #717 (ea4bb6a) into master (600e7bb) will increase coverage by 0.06%.
Report is 1 commits behind head on master.
The diff coverage is 91.16%.

@@            Coverage Diff             @@
##           master     #717      +/-   ##
==========================================
+ Coverage   88.34%   88.41%   +0.06%     
==========================================
  Files          60       62       +2     
  Lines        4444     4582     +138     
==========================================
+ Hits         3926     4051     +125     
- Misses        518      531      +13

Files Changed	Coverage Δ
...clude/planner_cspace/planner_3d/grid_astar_model.h	`100.00% <ø> (ø)`
planner_cspace/src/start_pose_predictor.cpp	`87.35% <87.35%> (ø)`
planner_cspace/src/planner_3d.cpp	`85.47% <92.30%> (+0.21%)`	⬆️
...ectory_tracker/include/trajectory_tracker/path2d.h	`95.45% <96.87%> (-0.78%)`	⬇️
...e/planner_cspace/planner_3d/start_pose_predictor.h	`100.00% <100.00%> (ø)`
planner_cspace/src/grid_astar_model_3dof.cpp	`96.10% <100.00%> (+0.02%)`	⬆️

... and 2 files with indirect coverage changes

… add-start-pose-predictor

at-wat · 2023-09-14T03:27:56Z

planner_cspace/src/planner_3d.cpp

-                    range_limit,
-                    1.0f / freq_min_,
-                    find_best_))
+    bool is_goal_same_as_start = false;


Seems always be false

Fixed, and I added a test to cover this logic.

at-wat · 2023-09-14T03:39:50Z

planner_cspace/test/src/test_start_pose_predictor.cpp

+    paths_["switch_back"] = std::make_pair(path_grid_switch_back, getMetricPath(path_grid_switch_back));
+  }
+
+  nav_msgs::Path getMetricPath(const std::list<GridAstar<3, 2>::Vec>& path_grid) const


Looks like convert rather than get

at-wat · 2023-09-14T04:10:55Z

planner_cspace/test/src/test_start_pose_predictor.cpp

+      pose.pose.orientation.z = sin(yaw / 2);
+      pose.pose.orientation.w = cos(yaw / 2);


Suggested change

pose.pose.orientation.z = sin(yaw / 2);

pose.pose.orientation.w = cos(yaw / 2);

pose.pose.orientation.z = std::sin(yaw / 2);

pose.pose.orientation.w = std::cos(yaw / 2);

at-wat · 2023-09-14T04:14:19Z

planner_cspace/test/src/test_start_pose_predictor.cpp

+  EXPECT_EQ(start_grid[0], 5);
+  EXPECT_EQ(start_grid[1], 6);
+  EXPECT_EQ(start_grid[2], 1);


As there's == operator, I think it can be

Suggested change

EXPECT_EQ(start_grid[0], 5);

EXPECT_EQ(start_grid[1], 6);

EXPECT_EQ(start_grid[2], 1);

EXPECT_EQ(StartPosePredictor::Astar::Vec(5, 6, 1), start_grid);

neonavigation/planner_cspace/include/planner_cspace/cyclic_vec.h

Lines 163 to 170 in fdfea8e

template <typename T2>

bool operator==(const CyclicVecBase<DIM, NONCYCLIC, T2>& v) const

{

for (int i = 0; i < DIM; i++)

if (v.e_[i] != e_[i])

return false;

return true;

}

at-wat · 2023-09-14T08:48:14Z

planner_cspace/src/planner_3d.cpp

    const std::list<Astar::Vecf> path_interpolated =
        model_->path_interpolator_.interpolate(path_grid, 0.5, local_range_);
+    path.poses = start_pose_predictor_.getPreservedPath().poses;
    grid_metric_converter::grid2MetricPath(map_info_, path_interpolated, path);


I originally didn't expect to use grid2MetricPath to append poses to existing path and it looks confusing.
It would be better to rename it to something like grid2MetricPathAppended/grid2MetricPathWithAppend.

Renamed to appendGridPath2MetricPath

at-wat

LGTM

at-wat · 2023-09-14T09:38:24Z