Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 6 commits into from
Jul 4, 2022
Merged

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

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e3287cf
fix(mission_planner): align goal pose to lanelet map
Jun 20, 2022
e93e945
ci(pre-commit): autofix
pre-commit-ci[bot] Jun 20, 2022
2e5277d
add vector to header
Jun 20, 2022
4d37e8c
refactor calculation
Jul 1, 2022
7b181c1
ci(pre-commit): autofix
pre-commit-ci[bot] Jul 1, 2022
d1731e1
remove vector header and warn msg
Jul 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
refactor calculation 
Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp>
  • Loading branch information
Xinyu Wang authored and M. Fatih Cırıt committed Jul 4, 2022
commit 4d37e8c12fb96669f72e1a189e0a72911d3a806c
3 changes: 0 additions & 3 deletions map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,9 +82,6 @@ double getLaneletAngle(
bool isInLanelet(
const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
const double radius = 0.0);
template <class T>
double getAverageProjectionHeight(
const std::vector<T> & linestrings, const lanelet::ConstPoint3d & point);

} // namespace utils
} // namespace lanelet
Expand Down
18 changes: 0 additions & 18 deletions map/lanelet2_extension/lib/utilities.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -685,23 +685,5 @@ bool isInLanelet(
return false;
}

template <class T>
double getAverageProjectionHeight(
const std::vector<T> & linestrings, const lanelet::ConstPoint3d & point)
{
double height = 0;
for (const auto & it : linestrings) {
lanelet::ConstLineString3d line(it);
auto p_point = lanelet::geometry::project(line, point);
height += p_point.z();
}
return height / linestrings.size();
}

template double getAverageProjectionHeight<lanelet::ConstLineString3d>(
const std::vector<lanelet::ConstLineString3d> &, const lanelet::ConstPoint3d &);
template double getAverageProjectionHeight<lanelet::ConstPolygon3d>(
const std::vector<lanelet::ConstPolygon3d> &, const lanelet::ConstPoint3d &);

} // namespace utils
} // namespace lanelet
4 changes: 2 additions & 2 deletions planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,6 @@ class MissionPlannerLanelet2 : public MissionPlanner

private:
bool is_graph_ready_;
double goal_height;
lanelet::LaneletMapPtr lanelet_map_ptr_;
lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
Expand All @@ -58,7 +57,8 @@ class MissionPlannerLanelet2 : public MissionPlanner
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_subscriber_;

void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
bool isGoalValid();
bool isGoalValid() const;
void refineGoalHeight(const RouteSections & route_sections);

// virtual functions
bool isRoutingGraphReady() const;
Expand Down
39 changes: 18 additions & 21 deletions planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,17 +122,13 @@ bool isInParkingLot(
return false;
}

template <class T>
double projectGoalToMap(T lanelet_component, const lanelet::ConstPoint3d & goal_point)
{
return lanelet::utils::getAverageProjectionHeight(lanelet_component, goal_point);
}

double projectGoalToMap(
const lanelet::ConstLineString3d & lanelet_component, const lanelet::ConstPoint3d & goal_point)
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(lanelet_component, goal_point.basicPoint());
lanelet::geometry::project(center_line, goal_point.basicPoint());
return project.z();
}

Expand Down Expand Up @@ -211,7 +207,7 @@ void MissionPlannerLanelet2::visualizeRoute(
marker_publisher_->publish(route_marker_array);
}

bool MissionPlannerLanelet2::isGoalValid()
bool MissionPlannerLanelet2::isGoalValid() const
{
lanelet::Lanelet closest_lanelet;
if (!lanelet::utils::query::getClosestLanelet(
Expand All @@ -229,24 +225,19 @@ bool MissionPlannerLanelet2::isGoalValid()
constexpr double th_angle = M_PI / 4;

if (std::abs(angle_diff) < th_angle) {
const lanelet::ConstLineString3d closest_center_line =
lanelet::utils::generateFineCenterline(closest_lanelet);
goal_height = projectGoalToMap(closest_center_line, goal_lanelet_pt);
return true;
}
}

// check if goal is in parking space
const auto parking_spaces = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_);
if (isInParkingSpace(parking_spaces, goal_lanelet_pt)) {
goal_height = projectGoalToMap(parking_spaces, goal_lanelet_pt);
return true;
}

// check if goal is in parking lot
const auto parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_);
if (isInParkingLot(parking_lots, goal_lanelet_pt)) {
goal_height = projectGoalToMap(parking_lots, goal_lanelet_pt);
return true;
}

Expand All @@ -265,9 +256,6 @@ bool MissionPlannerLanelet2::isGoalValid()

constexpr double th_angle = M_PI / 4;
if (std::abs(angle_diff) < th_angle) {
const lanelet::ConstLineString3d closest_center_line =
lanelet::utils::generateFineCenterline(closest_shoulder_lanelet);
goal_height = projectGoalToMap(closest_center_line, goal_lanelet_pt);
return true;
}
}
Expand All @@ -294,9 +282,6 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute(
return route_msg;
}

goal_pose_.pose.position.z = goal_height;
checkpoints_.back().pose.position.z = goal_height;

for (std::size_t i = 1; i < checkpoints_.size(); i++) {
const auto start_checkpoint = checkpoints_.at(i - 1);
const auto goal_checkpoint = checkpoints_.at(i);
Expand All @@ -316,15 +301,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_WARN(get_logger(), "Goal Pose Z : %lf", goal_height);
RCLCPP_WARN(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>
Expand Down