fix(mission_planner): align goal pose to lanelet map (#1129)

Signed-off-by: Xinyu Wang <[email protected]>
autowarefoundation · Jul 4, 2022 · 63f45ff · 63f45ff
1 parent 19aec38
commit 63f45ff
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp
@@ -58,6 +58,7 @@ class MissionPlannerLanelet2 : public MissionPlanner
 
   void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
   bool isGoalValid() const;
+  void refineGoalHeight(const RouteSections & route_sections);
 
   // virtual functions
   bool isRoutingGraphReady() const;

diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
@@ -122,6 +122,15 @@ bool isInParkingLot(
   return false;
 }
 
+double projectGoalToMap(
+  const lanelet::Lanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point)
+{
+  const lanelet::ConstLineString3d center_line =
+    lanelet::utils::generateFineCenterline(lanelet_component);
+  lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint());
+  return project.z();
+}
+
 }  // anonymous namespace
 
 namespace mission_planner
@@ -291,14 +300,27 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute(
       get_logger(), "Loop detected within route! Be aware that looped route is not debugged!");
   }
 
+  refineGoalHeight(route_sections);
+
   route_msg.header.stamp = this->now();
   route_msg.header.frame_id = map_frame_;
   route_msg.segments = route_sections;
   route_msg.goal_pose = goal_pose_.pose;
 
+  RCLCPP_DEBUG(get_logger(), "Goal Pose Z : %lf", goal_pose_.pose.position.z);
   return route_msg;
 }
 
+void MissionPlannerLanelet2::refineGoalHeight(const RouteSections & route_sections)
+{
+  const auto goal_lane_id = route_sections.back().preferred_primitive_id;
+  lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id);
+  const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position);
+  double goal_height = projectGoalToMap(goal_lanelet, goal_lanelet_pt);
+  goal_pose_.pose.position.z = goal_height;
+  checkpoints_.back().pose.position.z = goal_height;
+}
+
 }  // namespace mission_planner
 
 #include <rclcpp_components/register_node_macro.hpp>