po

map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

@@ -32,7 +32,8 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
   explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options);
 
   static lanelet::LaneletMapPtr load_map(
-    const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type);
+    rclcpp::Node & node, const std::string & lanelet2_filename,
+    const std::string & lanelet2_map_projector_type);
   static HADMapBin create_map_bin_msg(
     const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
     const rclcpp::Time & now);

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -54,7 +54,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
   const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0);
 
   // load map from file
-  const auto map = load_map(lanelet2_filename, lanelet2_map_projector_type);
+  const auto map = load_map(*this, lanelet2_filename, lanelet2_map_projector_type);
   if (!map) {
     return;
   }
@@ -72,25 +72,19 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
 }
 
 lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
-  const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type)
+  rclcpp::Node & node, const std::string & lanelet2_filename,
+  const std::string & lanelet2_map_projector_type)
 {
   lanelet::ErrorMessages errors{};
   if (lanelet2_map_projector_type == "MGRS") {
-    /*
-    const double map_origin_lat = 40.8;  // declare_parameter("latitude", 0.0);
-    const double map_origin_lon = 29.3;  // declare_parameter("longitude", 0.0);
-    lanelet::GPSPoint position{map_origin_lat, map_origin_lon};
-    lanelet::Origin origin{position};
-    */
-
-    lanelet::projection::MGRSProjector projector{};  // origin};
+    lanelet::projection::MGRSProjector projector{};
     const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
     if (errors.empty()) {
       return map;
     }
   } else if (lanelet2_map_projector_type == "UTM") {
-    const double map_origin_lat = 40.8;  // declare_parameter("latitude", 0.0);
-    const double map_origin_lon = 29.3;  // declare_parameter("longitude", 0.0);
+    const double map_origin_lat = node.declare_parameter("latitude", 0.0);
+    const double map_origin_lon = node.declare_parameter("longitude", 0.0);
     lanelet::GPSPoint position{map_origin_lat, map_origin_lon};
     lanelet::Origin origin{position};
     lanelet::projection::UtmProjector projector{origin};

planning/static_centerline_optimizer/CMakeLists.txt

@@ -25,7 +25,6 @@ target_link_libraries(static_centerline_optimizer
 ament_auto_package(
   INSTALL_TO_SHARE
     launch
-    config
     rviz
 )
 

planning/static_centerline_optimizer/include/static_centerline_optimizer/functions.hpp

@@ -25,7 +25,7 @@
 namespace static_centerline_optimizer
 {
 HADMapBin::ConstSharedPtr create_map(
-  const std::string & lanelet2_file_name, const rclcpp::Time & current_time);
+  rclcpp::Node & node, const std::string & lanelet2_file_name, const rclcpp::Time & current_time);
 
 std::vector<geometry_msgs::msg::Pose> create_check_points(
   const RouteHandler & route_handler, const size_t start_lanelet_id, const size_t end_lanelet_id);

planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml

@@ -1,11 +1,19 @@
 <launch>
+  <!-- mandatory arguments for planning-->
   <arg name="lanelet2_file_name"/>
   <arg name="start_lanelet_id"/>
   <arg name="end_lanelet_id"/>
   <arg name="vehicle_model"/>
 
+  <!-- topic -->
   <arg name="lanelet2_map_topic" default="/map/vector_map"/>
   <arg name="lanelet2_map_marker_topic" default="/map/vector_map_marker"/>
+  <!-- param -->
+  <arg name="map_loader_param" default="$(find-pkg-share map_loader)/config/lanelet2_map_loader.param.yaml"/>
+  <arg
+    name="obstacle_avoidance_planner_param"
+    default="$(find-pkg-share planning_launch)/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml"
+  />
 
   <!-- Global parameters (for PathFootprint in tier4_planning_rviz_plugin) -->
   <!-- Do not add "group" in order to propagate global parameters -->
@@ -36,9 +44,12 @@
     <param name="lanelet2_file_name" value="$(var lanelet2_file_name)"/>
     <param name="start_lanelet_id" value="$(var start_lanelet_id)"/>
     <param name="end_lanelet_id" value="$(var end_lanelet_id)"/>
-    <param from="$(find-pkg-share planning_launch)/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml"/>
+    <!-- common param -->
     <param from="$(find-pkg-share planning_launch)/config/scenario_planning/common/common.param.yaml"/>
     <param from="$(find-pkg-share planning_launch)/config/scenario_planning/common/nearest_search.param.yaml"/>
+    <!-- component param -->
+    <param from="$(var map_loader_param)"/>
+    <param from="$(var obstacle_avoidance_planner_param)"/>
   </node>
 
   <!-- rviz -->

planning/static_centerline_optimizer/src/functions.cpp

@@ -73,11 +73,11 @@ lanelet::Point3d createPoint3d(const double x, const double y, const double z =
 }  // namespace
 
 HADMapBin::ConstSharedPtr create_map(
-  const std::string & lanelet2_file_name, const rclcpp::Time & current_time)
+  rclcpp::Node & node, const std::string & lanelet2_file_name, const rclcpp::Time & current_time)
 {
   // load map
   lanelet::LaneletMapPtr map_ptr;
-  map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_file_name, "MGRS");
+  map_ptr = Lanelet2MapLoaderNode::load_map(node, lanelet2_file_name, "MGRS");
   if (!map_ptr) {
     return nullptr;
   }

planning/static_centerline_optimizer/src/static_centerline_optimizer.cpp

@@ -88,12 +88,12 @@ int main(int argc, char * argv[])
     main_node->declare_parameter<double>("ego_nearest_dist_threshold");
   const double ego_nearest_yaw_threshold =
     main_node->declare_parameter<double>("ego_nearest_yaw_threshold");
-  const auto lanelet2_output_file_name = "/tmp/popo/lanelet2_map.osm";
+  const auto lanelet2_output_file_name = "/tmp/lanelet2_map.osm";
 
   // load map by the map_loader package
   // function0 starts
   const auto map_bin_msg_ptr =
-    static_centerline_optimizer::create_map(lanelet2_file_name, current_time);
+    static_centerline_optimizer::create_map(*main_node, lanelet2_file_name, current_time);
   if (!map_bin_msg_ptr) {
     RCLCPP_ERROR(main_node->get_logger(), "Loading map failed");
     return 0;