TODO: return optimized points to server

planning/static_centerline_optimizer/CMakeLists.txt

@@ -52,5 +52,6 @@ ament_auto_package(
 
 install(PROGRAMS
   scripts/optimize_centerline.sh
+  scripts/app.py
   DESTINATION lib/${PROJECT_NAME}
 )

planning/static_centerline_optimizer/README.md

@@ -16,16 +16,26 @@ 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.
+
+```sh
+ros2 run static_centerline_optimizer run_planning_server.sh vehicle_model:=lexus
+```
+
+Run http server with the following command.
+The port number is 5000 by default.
+
 ```sh
-ros2 run static_centerline_optimizer run_planning_server.sh vehicle_model:=<vehicle-name>
+ros2 run static_centerline_optimizer app.py
 ```
 
 ### Command Line Interface
 
 The optimized centerline is generated from the command line interface.
 
 ```sh
-ros2 run static_centerline_optimizer optimize_path.sh <osm-map-path> <start-lanelet-id> <end-lanelet-id> <vehicle-model>
+# 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
 ```
 
 image

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -34,14 +34,19 @@ using static_centerline_optimizer::srv::PlanRoute;
 class StaticCenterlineOptimizerNode : public rclcpp::Node
 {
 public:
+  enum class PlanPathResult {
+    SUCCESS = 0,
+    ROUTE_IS_NOT_READY = 1,
+  };
+
   explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options);
   void run();
 
 private:
   // load map
+  void load_map(const std::string & lanelet2_input_file_name);
   void on_load_map(
     const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response);
-  void load_map(const std::string & lanelet2_input_file_name);
 
   // plan route
   void on_plan_route(
@@ -51,12 +56,12 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
   // plan path
   void on_plan_path(
     const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
-  void plan_path(const int start_lanelet_id);
+  PlanPathResult plan_path(const int start_lanelet_id);
   void save_map(const std::string & lanelet2_output_file_name);
 
   HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
   std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
-  HADMapRoute::ConstSharedPtr route_ptr_{nullptr};
+  std::shared_ptr<lanelet::ConstLanelets> lanelets_ptr_{nullptr};
   std::vector<TrajectoryPoint> optimized_traj_points_{};
 
   // publisher

planning/static_centerline_optimizer/scripts/app.py

@@ -1,3 +1,19 @@
+#!/usr/bin/env python3
+
+# Copyright 2022 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# 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.
+
 from flask import Flask
 from flask import abort
 from flask import request
@@ -12,36 +28,38 @@
 node = Node("app")
 
 app = Flask(__name__)
-app.secret_key = "hogehoge"
-
+app.secret_key = "tmp_secret_key"
 
-def on_load_map(self, request, response):
-    response.sum = request.a + request.b
-    self.get_logger().info("Incoming request\na: %d b: %d" % (request.a, request.b))
 
-    return response
+def create_client(service_type, server_name):
+    # create client
+    cli = node.create_client(service_type, server_name)
+    while not cli.wait_for_service(timeout_sec=1.0):
+        print("{} service not available, waiting again...".format(server_name))
+    return cli
 
 
 @app.route("/load_map", methods=["POST"])
 def load_map_post():
     data = request.get_json()
     session["map_id"] = 1
 
+    print(data["map"])
+
     # create client
-    cli = node.create_client(LoadMap, "load_map")
-    while not cli.wait_for_service(timeout_sec=1.0):
-        print("Load map service not available, waiting again...")
+    cli = create_client(LoadMap, "load_map")
 
     # request map loading
-    req = LoadMap.Request()
-    req.map = data["map"]
-    cli.call_async(req)
+    req = LoadMap.Request(map=data["map"])
+    future = cli.call_async(req)
+    rclpy.spin_until_future_complete(node, future)
 
+    # TODO(murooka)
     error = False
-
     if error:
         abort(500, "error_message")
 
+    # TODO(murooka) generate uuid
     return {"mapId": "1"}
 
 
@@ -50,19 +68,16 @@ def plan_route_post():
     data = request.get_json()
 
     # create client
-    cli = node.create_client(PlanRoute, "plan_route")
-    while not cli.wait_for_service(timeout_sec=1.0):
-        print("Plan route service not available, waiting again...")
+    cli = create_client(PlanRoute, "plan_route")
 
     # request route planning
-    req = PlanRoute.Request()
-    req.map_id = 1
-    req.start_lane_id = data["start_lane_id"]
-    req.end_lane_id = data["end_lane_id"]
-    cli.call_async(req)
+    req = PlanRoute.Request(start_lane_id=data["start_lane_id"], end_lane_id=data["end_lane_id"])
+    future = cli.call_async(req)
+    rclpy.spin_until_future_complete(node, future)
+    print(future.result())
 
+    # TODO(murooka)
     error = False
-
     if error:
         abort(500, "error_message")
 
@@ -74,14 +89,12 @@ def plan_path_post():
     data = request.get_json()
 
     # create client
-    cli = node.create_client(PlanPath, "plan_path")
-    while not cli.wait_for_service(timeout_sec=1.0):
-        print("Plan path service not available, waiting again...")
+    cli = create_client(PlanPath, "plan_path")
 
     # request path planning
-    req = PlanPath.Request()
-    req.start_lane_id = data["start_lane_id"]
-    cli.call_async(req)
+    req = PlanPath.Request(start_lane_id=data["start_lane_id"])
+    future = cli.call_async(req)
+    rclpy.spin_until_future_complete(node, future)
 
     return {"po": "po"}