fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/static_centerline_optimizer/CMakeLists.txt

@@ -23,7 +23,7 @@ rosidl_generate_interfaces(
 ament_auto_add_executable(main
   src/main.cpp
   src/static_centerline_optimizer_node.cpp
-  src/optimization_node.cpp
+  src/collision_free_optimizer_node.cpp
   src/utils.cpp
   )
 
@@ -54,7 +54,6 @@ ament_auto_package(
 )
 
 install(PROGRAMS
-  scripts/optimize_centerline.sh
   scripts/app.py
   DESTINATION lib/${PROJECT_NAME}
 )

planning/static_centerline_optimizer/README.md

@@ -2,51 +2,64 @@
 
 ## Purpose
 
-This package statically calcualtes the centerline satisfying footprints inside the drivable area.
+This package statically calcualtes the centerline satisfying path footprints inside the drivable area.
 
-On narrow-road driving, the default centerline, which is the middle line between lanelet's right and left bounds, often causes footprints outside the drivable area.
-With the static centerline optimization, we have the following advantages.
+On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area.
+To make path footprints inside the drivable area, we use online path shape optimization by [the obstacle_avoidance_planner package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/obstacle_avoidance_planner/).
+
+Instead of online path shape optimization, we introduce static ceneterline optimization.
+With this static centerline optimization, we have following advantages.
 
 - We can see the optimized centerline shape in advance.
-- We do not have to calculate a heavy and sometimes unstable path optimization since the footprints are already inside the drivable area.
+  - With the default autoware, path shape is not determined until the vehicle drives there.
+  - This enables offline path shape evaluation.
+- We do not have to calculate a heavy and sometimes unstable path optimization since the path footprints are already inside the drivable area.
 
 ## Usecases
 
 There are two interfaces to communicate with the centerline optimizer.
 
 ### Vector Map Builder Interface
 
-Run the autoware server with the following command by designating `vehicle_model` as lexus for example.
+Note: This function of Vector Map Builder has not been released. Please wait for a while.
+Currently there is no documentation about Vector Map Builder's operation for this function.
 
-```sh
-ros2 launch static_centerline_optimizer run_planning_server.launch.xml vehicle_model:=lexus
-```
+The optimized centerline can be generated from Vector Map Builder's operation.
 
-Run http server with the following command.
-The port number is 5000 by default.
+We can run
+
+- path planning server
+- http server to connect path planning server and Vector Map Builder
+
+with the following command by designating `<vehicle_model>`
 
 ```sh
-ros2 run static_centerline_optimizer app.py
+ros2 launch static_centerline_optimizer run_planning_server.launch.xml vehicle_model:=<vehicle-model>
 ```
 
+FYI, port ID of the http server is 4010 by default.
+
 ### Command Line Interface
 
-The optimized centerline is generated from the command line interface.
+The optimized centerline can be generated from the command line interface by designating
+
+- `<input-osm-path>`
+- (`<output-osm-path>` not mandatory)
+- `<start-lanelet-id>`
+- `<end-lanelet-id>`
+- `<vehicle-model>`
 
 ```sh
-# ros2 run static_centerline_optimizer optimize_path.sh <osm-map-path> <start-lanelet-id> <end-lanelet-id> <vehicle-model>
-$ ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml lanelet2_input_file_name:="$HOME"/AutonomousDrivingScenarios/map/kashiwanoha/lanelet2_map.osm start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus run_backgrond:=false
+ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_backgrond:=false lanelet2_input_file_path:=<input-osm-path> lanelet2_output_file_path:=<output-osm-path> start_lanelet_id:=<start-lane-id> end_lanelet_id:=<end-lane-id> vehicle_model:=<vehicle-model>
 ```
 
+## Visualization
+
 image
 
 The yellow footprints are the original ones in the map file.
 The red footprints are the optimized ones.
 You can see that the red footprints are inside the lane although the yellow ones are outside.
 
-The output map with the optimized centerline locates `/tmp/lanelet2_map.osm`
-If you want to change the output map path, you can remap the path
-
-```sh
-ros2 run static_centerline_optimizer optimize_centerline.sh <map-path> <start-lanelet-id> <end-lanelet-id> --ros-args --remap output:=<output-map-path>
-```
+The default output map with the optimized centerline locates `/tmp/lanelet2_map.osm`
+If you want to change the output map path, you can remap the path by adding the option `lanelet2_output_file_path:=<output-osm-path>`

planning/static_centerline_optimizer/include/static_centerline_optimizer/optimization_node.hpp → planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp

@@ -11,8 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef STATIC_CENTERLINE_OPTIMIZER__OPTIMIZATION_NODE_HPP_
-#define STATIC_CENTERLINE_OPTIMIZER__OPTIMIZATION_NODE_HPP_
+#ifndef STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_
+#define STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_
 
 #include "motion_utils/motion_utils.hpp"
 #include "obstacle_avoidance_planner/common_structs.hpp"
@@ -33,7 +33,7 @@
 
 namespace static_centerline_optimizer
 {
-class StaticCenterlineOptimizer : public rclcpp::Node
+class CollisionFreeOptimizerNode : public rclcpp::Node
 {
 private:
   rclcpp::Clock logger_ros_clock_;
@@ -84,7 +84,6 @@ class StaticCenterlineOptimizer : public rclcpp::Node
   std::unique_ptr<rclcpp::Time> latest_replanned_time_ptr_;
 
   // ROS
-  rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
   rclcpp::Subscription<Path>::SharedPtr path_sub_;
 
   // functions
@@ -96,11 +95,11 @@ class StaticCenterlineOptimizer : public rclcpp::Node
   //   const CVMaps & cv_maps);
 
 public:
-  explicit StaticCenterlineOptimizer(const rclcpp::NodeOptions & node_options);
+  explicit CollisionFreeOptimizerNode(const rclcpp::NodeOptions & node_options);
 
   // subscriber callback functions
-  std::vector<TrajectoryPoint> pathCallback(const Path::SharedPtr);
+  Trajectory pathCallback(const Path::SharedPtr);
 };
 }  // namespace static_centerline_optimizer
 
-#endif  // STATIC_CENTERLINE_OPTIMIZER__OPTIMIZATION_NODE_HPP_
+#endif  // STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -48,36 +48,38 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
 
 private:
   // load map
-  void load_map(const std::string & lanelet2_input_file_name);
+  void load_map(const std::string & lanelet2_input_file_path);
   void on_load_map(
     const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response);
 
   // plan route
+  std::vector<unsigned int> plan_route(const int start_lanelet_id, const int end_lanelet_id);
   void on_plan_route(
     const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
-  void plan_route(const int start_lanelet_id, const int end_lanelet_id);
 
   // plan path
   void on_plan_path(
     const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
-  PlanPathResult plan_path(const int start_lanelet_id);
-  void evaluate();
+  PlanPathResult plan_path(const std::vector<unsigned int> & route_lane_ids);
+  void evaluate(const std::vector<unsigned int> & route_lane_ids);
   MarkerArray createFootprintMarker(
     const LinearRing2d & footprint_poly, const std::array<double, 3> & marker_color,
     const size_t idx);
 
-  void save_map(const std::string & lanelet2_output_file_name);
+  void save_map(
+    const std::string & lanelet2_output_file_path,
+    const std::vector<unsigned int> & route_lane_ids);
 
   HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
   std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
-  std::shared_ptr<lanelet::ConstLanelets> lanelets_ptr_{nullptr};
   std::vector<TrajectoryPoint> optimized_traj_points_{};
 
   // publisher
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_{nullptr};
   rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
   rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_unsafe_footprints_{nullptr};
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_optimized_centerline_{nullptr};
 
   // service
   rclcpp::Service<LoadMap>::SharedPtr srv_load_map_;

planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp

@@ -26,16 +26,6 @@ namespace static_centerline_optimizer
 {
 namespace utils
 {
-HADMapBin::ConstSharedPtr create_map(
-  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);
-
-HADMapRoute plan_route(
-  const HADMapBin::ConstSharedPtr map_bin_msg_ptr,
-  const std::vector<geometry_msgs::msg::Pose> & check_points);
-
 geometry_msgs::msg::Pose get_center_pose(
   const RouteHandler & route_handler, const size_t lanelet_id);
 

planning/static_centerline_optimizer/launch/run_planning_server.launch.xml

@@ -6,4 +6,7 @@
     <arg name="vehicle_model" value="$(var vehicle_model)"/>
     <arg name="run_background" value="true"/>
   </include>
+
+  <!-- local server to connect path optimzier and cloud software -->
+  <node pkg="static_centerline_optimizer" exec="app.py" name="static_centerline_optimizer_http_server" output="screen"/>
 </launch>

planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml

@@ -7,8 +7,8 @@
   <arg name="rviz" default="true"/>
 
   <!-- mandatory arguments when run_background is true -->
-  <arg name="lanelet2_input_file_name" default=""/>
-  <arg name="lanelet2_output_file_name" default="/tmp/lanelet2_map.osm"/>
+  <arg name="lanelet2_input_file_path" default=""/>
+  <arg name="lanelet2_output_file_path" default="/tmp/lanelet2_map.osm"/>
   <arg name="start_lanelet_id" default=""/>
   <arg name="end_lanelet_id" default=""/>
 
@@ -45,13 +45,14 @@
   <!-- optimize path -->
   <node pkg="static_centerline_optimizer" exec="main" name="static_centerline_optimizer">
     <remap from="lanelet2_map_topic" to="$(var lanelet2_map_topic)"/>
+    <remap from="input_centerline" to="~/input_centerline"/>
+    <remap from="output_centerline" to="~/output_centerline"/>
     <remap from="debug/raw_centerline" to="~/debug/raw_centerline"/>
-    <remap from="debug/optimized_centerline" to="~/debug/optimized_centerline"/>
     <remap from="debug/unsafe_footprints" to="~/debug/unsafe_footprints"/>
 
     <param name="run_background" value="$(var run_background)"/>
-    <param name="lanelet2_input_file_name" value="$(var lanelet2_input_file_name)"/>
-    <param name="lanelet2_output_file_name" value="$(var lanelet2_output_file_name)"/>
+    <param name="lanelet2_input_file_path" value="$(var lanelet2_input_file_path)"/>
+    <param name="lanelet2_output_file_path" value="$(var lanelet2_output_file_path)"/>
     <param name="start_lanelet_id" value="$(var start_lanelet_id)"/>
     <param name="end_lanelet_id" value="$(var end_lanelet_id)"/>
     <!-- common param -->

planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz

@@ -5,9 +5,6 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /debug1
-        - /MarkerArray1
-        - /MarkerArray1/Topic1
-        - /MarkerArray1/Namespaces1
       Splitter Ratio: 0.550000011920929
     Tree Height: 388
   - Class: rviz_common/Selection
@@ -145,7 +142,7 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /raw_path_with_lane_id
+        Value: /static_centerline_optimizer/input_centerline
       Value: false
       View LaneId:
         Scale: 0.10000000149011612
@@ -158,16 +155,16 @@ Visualization Manager:
         Vehicle Length: 4.769999980926514
         Vehicle Width: 1.8300000429153442
     - Class: rviz_plugins/TrajectoryFootprint
-      Enabled: false
+      Enabled: true
       Name: Opimized Centerline
       Topic:
         Depth: 5
         Durability Policy: Transient Local
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /static_centerline_optimizer/debug/optimized_centerline
-      Value: false
+        Value: /static_centerline_optimizer/output_centerline
+      Value: true
       View Trajectory Footprint:
         Alpha: 0.5
         Color: 239; 41; 41
@@ -222,20 +219,20 @@ Visualization Manager:
             Value: true
             Vehicle Length: 4.769999980926514
             Vehicle Width: 1.8300000429153442
-      Enabled: false
-      Name: debug
-    - Class: rviz_default_plugins/MarkerArray
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: true
+          Name: MarkerArray
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /static_centerline_optimizer/debug/unsafe_footprints
+          Value: true
       Enabled: true
-      Name: MarkerArray
-      Namespaces:
-        unsafe_footprints: true
-      Topic:
-        Depth: 5
-        Durability Policy: Transient Local
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /static_centerline_optimizer/debug/unsafe_footprints
-      Value: true
+      Name: debug
   Enabled: true
   Global Options:
     Background Color: 10; 10; 10