feat(costmap_generator, scenario_selector): improve freespace planning stability (#9579)

* discretize updating grid center position by size of grid resolution

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* modify logic for switching to lane driving in scenario selector

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* fix spelling

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

---------

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

Author: mkquda

planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp

@@ -139,6 +139,13 @@ class CostmapGenerator : public rclcpp::Node
 
   void set_current_pose();
 
+  /// \brief set position of grid center
+  /// \details computes relative position of ego from current grid center,
+  /// if offset is larger than grid resolution, grid center will be updated
+  /// by a multiple of the grid resolution
+  /// \param[in] tf costmap frame to vehicle frame transform
+  void set_grid_center(const geometry_msgs::msg::TransformStamped & tf);
+
   void onTimer();
 
   bool isActive();
@@ -148,7 +155,9 @@ class CostmapGenerator : public rclcpp::Node
 
   /// \brief publish ros msg: grid_map::GridMap, and nav_msgs::OccupancyGrid
   /// \param[in] gridmap with calculated cost
-  void publishCostmap(const grid_map::GridMap & costmap);
+  /// \param[in] tf costmap frame to vehicle frame transform
+  void publishCostmap(
+    const grid_map::GridMap & costmap, const geometry_msgs::msg::TransformStamped & tf);
 
   /// \brief fill a vector with road area polygons
   /// \param [in] lanelet_map input lanelet map

planning/autoware_costmap_generator/src/costmap_generator.cpp

@@ -286,11 +286,7 @@ void CostmapGenerator::onTimer()
   }
   time_keeper_->end_track("lookupTransform");
 
-  // Set grid center
-  grid_map::Position p;
-  p.x() = tf.transform.translation.x;
-  p.y() = tf.transform.translation.y;
-  costmap_.setPosition(p);
+  set_grid_center(tf);
 
   if ((param_->use_wayarea || param_->use_parkinglot) && lanelet_map_) {
     autoware::universe_utils::ScopedTimeTrack st("generatePrimitivesCostmap()", *time_keeper_);
@@ -312,7 +308,18 @@ void CostmapGenerator::onTimer()
     costmap_[LayerName::combined] = generateCombinedCostmap();
   }
 
-  publishCostmap(costmap_);
+  publishCostmap(costmap_, tf);
+}
+
+void CostmapGenerator::set_grid_center(const geometry_msgs::msg::TransformStamped & tf)
+{
+  const auto cur_pos = costmap_.getPosition();
+  const grid_map::Position ref_pos(tf.transform.translation.x, tf.transform.translation.y);
+  const auto disp = ref_pos - cur_pos;
+  const auto resolution = costmap_.getResolution();
+  const grid_map::Position offset(
+    std::round(disp.x() / resolution) * resolution, std::round(disp.y() / resolution) * resolution);
+  costmap_.setPosition(cur_pos + offset);
 }
 
 bool CostmapGenerator::isActive()
@@ -452,7 +459,8 @@ grid_map::Matrix CostmapGenerator::generateCombinedCostmap()
   return combined_costmap[LayerName::combined];
 }
 
-void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap)
+void CostmapGenerator::publishCostmap(
+  const grid_map::GridMap & costmap, const geometry_msgs::msg::TransformStamped & tf)
 {
   // Set header
   std_msgs::msg::Header header;
@@ -465,11 +473,13 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap)
     costmap, LayerName::combined, param_->grid_min_value, param_->grid_max_value,
     out_occupancy_grid);
   out_occupancy_grid.header = header;
+  out_occupancy_grid.info.origin.position.z = tf.transform.translation.z;
   pub_occupancy_grid_->publish(out_occupancy_grid);
 
   // Publish GridMap
   auto out_gridmap_msg = grid_map::GridMapRosConverter::toMessage(costmap);
   out_gridmap_msg->header = header;
+  out_gridmap_msg->info.pose.position.z = tf.transform.translation.z;
   pub_costmap_->publish(*out_gridmap_msg);
 
   // Publish ProcessingTime

planning/autoware_scenario_selector/src/node.cpp

@@ -14,6 +14,7 @@
 
 #include "autoware/scenario_selector/node.hpp"
 
+#include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
 
@@ -65,6 +66,22 @@ bool isInLane(
   return dist_to_nearest_lanelet < margin;
 }
 
+bool isAlongLane(
+  const std::shared_ptr<autoware::route_handler::RouteHandler> & route_handler,
+  const geometry_msgs::msg::Pose & current_pose)
+{
+  lanelet::ConstLanelet closest_lanelet;
+  if (!route_handler->getClosestLaneletWithConstrainsWithinRoute(
+        current_pose, &closest_lanelet, 0.0, M_PI_4)) {
+    return false;
+  }
+  const lanelet::BasicPoint2d src_point(current_pose.position.x, current_pose.position.y);
+  const auto dist_to_centerline =
+    lanelet::geometry::distanceToCenterline2d(closest_lanelet, src_point);
+  static constexpr double margin = 1.0;
+  return dist_to_centerline < margin;
+}
+
 bool isInParkingLot(
   const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
   const geometry_msgs::msg::Pose & current_pose)
@@ -215,10 +232,9 @@ bool ScenarioSelectorNode::isSwitchToParking(const bool is_stopped)
 
 bool ScenarioSelectorNode::isSwitchToLaneDriving()
 {
-  const auto is_in_lane =
-    isInLane(route_handler_->getLaneletMapPtr(), current_pose_->pose.pose.position);
+  const auto is_along_lane = isAlongLane(route_handler_, current_pose_->pose.pose);
 
-  if (!isEmptyParkingTrajectory() || !is_in_lane) {
+  if (!isEmptyParkingTrajectory() || !is_along_lane) {
     empty_parking_trajectory_time_ = {};
     return false;
   }