From 63f45ffb842d955ab6c01d2427a6fc28b8dce09f Mon Sep 17 00:00:00 2001 From: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> Date: Tue, 5 Jul 2022 02:32:45 +0900 Subject: [PATCH] fix(mission_planner): align goal pose to lanelet map (#1129) Signed-off-by: Xinyu Wang --- .../mission_planner_lanelet2.hpp | 1 + .../mission_planner_lanelet2.cpp | 22 +++++++++++++++++++ 2 files changed, 23 insertions(+) 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 index a623f45e432ba..03c60cd28ef03 100644 --- 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 index 8052ccd4d8e48..4853adada7c0b 100644 --- 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