Merge branch 'main' into revert-1276-point-cloud-msg-wrapper-pointclo…

…ud-preprocessor

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -35,11 +35,6 @@ 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();
 
@@ -55,18 +50,19 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
     const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
 
   // plan path
+  std::vector<TrajectoryPoint> plan_path(const std::vector<unsigned int> & route_lane_ids);
   void on_plan_path(
     const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
-  PlanPathResult plan_path(const std::vector<unsigned int> & route_lane_ids);
-  void evaluate(const std::vector<unsigned int> & route_lane_ids);
 
+  void evaluate(
+    const std::vector<unsigned int> & route_lane_ids,
+    const std::vector<TrajectoryPoint> & optimized_traj_points);
   void save_map(
-    const std::string & lanelet2_output_file_path,
-    const std::vector<unsigned int> & route_lane_ids);
+    const std::string & lanelet2_output_file_path, const std::vector<unsigned int> & route_lane_ids,
+    const std::vector<TrajectoryPoint> & optimized_traj_points);
 
   HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
   std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
-  std::vector<TrajectoryPoint> optimized_traj_points_{};
 
   // publisher
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_{nullptr};

planning/static_centerline_optimizer/scripts/app.py

@@ -15,11 +15,11 @@
 # limitations under the License.
 
 import json
+import uuid
 
 from flask import Flask
-from flask import abort
+from flask import jsonify
 from flask import request
-from flask import session
 from flask_cors import CORS
 import rclpy
 from rclpy.node import Node
@@ -31,7 +31,7 @@
 node = Node("static_centerline_optimizer_http_server")
 
 app = Flask(__name__)
-CORS(app, supports_credentials=True)
+CORS(app)
 
 
 def create_client(service_type, server_name):
@@ -44,30 +44,45 @@ def create_client(service_type, server_name):
 
 @app.route("/map", methods=["POST"])
 def get_map():
-    # TODO(murooka) use map_id
     data = request.get_json()
-    session["map_id"] = 1
+
+    map_uuid = str(uuid.uuid4())
+    global map_id
+    map_id = map_uuid
 
     # create client
     cli = create_client(LoadMap, "/planning/static_centerline_optimizer/load_map")
 
     # request map loading
     req = LoadMap.Request(map=data["map"])
     future = cli.call_async(req)
+
+    # get result
     rclpy.spin_until_future_complete(node, future)
+    res = future.result()
 
-    # TODO(murooka) error handling
-    error = False
-    if error:
-        abort(500, "error_message")
+    # error handling
+    if res.message == "InvalidMapFormat":
+        return jsonify(code=res.message, message="Map format is invalid."), 400
+    elif res.message != "":
+        return (
+            jsonify(
+                code="InternalServerError",
+                message="Error occurred on the server. Please check the terminal.",
+            ),
+            500,
+        )
 
-    # TODO(murooka) generate uuid
-    return "D61E1C81-784A-4C0A-8244-3AA65559D7C5"
+    return map_uuid
 
 
 @app.route("/planned_route", methods=["GET"])
 def post_planned_route():
     args = request.args.to_dict()
+    global map_id
+    if map_id != args.get("map_id"):
+        # TODO(murooka) error handling for map_id mismatch
+        print("map_id is not correct.")
 
     # create client
     cli = create_client(PlanRoute, "/planning/static_centerline_optimizer/plan_route")
@@ -77,39 +92,68 @@ def post_planned_route():
         start_lane_id=int(args.get("start_lane_id")), end_lane_id=int(args.get("end_lane_id"))
     )
     future = cli.call_async(req)
+
+    # get result
     rclpy.spin_until_future_complete(node, future)
     res = future.result()
 
-    # TODO(murooka) error handling
-    if res.message != "":
-        if res.message == "route_has_not_been_planned":
-            abort(404, "route not found")
-        else:
-            # invalid
-            pass
+    # error handling
+    if res.message == "MapNotFound":
+        return jsonify(code=res.message, message="Map is missing."), 404
+    elif res.message == "RouteNotFound":
+        return jsonify(code=res.message, message="Planning route failed."), 404
+    elif res.message != "":
+        return (
+            jsonify(
+                code="InternalServerError",
+                message="Error occurred on the server. Please check the terminal.",
+            ),
+            500,
+        )
 
     return json.dumps(tuple(res.lane_ids))
 
 
 @app.route("/planned_path", methods=["GET"])
 def post_planned_path():
+    args = request.args.to_dict()
+    global map_id
+    if map_id != args.get("map_id"):
+        # TODO(murooka) error handling for map_id mismatch
+        print("map_id is not correct.")
+
     # create client
     cli = create_client(PlanPath, "/planning/static_centerline_optimizer/plan_path")
 
     # request path planning
-    route_lane_ids = [eval(i) for i in request.args.getlist("route")]
+    route_lane_ids = [eval(i) for i in request.args.getlist("route[]")]
     req = PlanPath.Request(route=route_lane_ids)
     future = cli.call_async(req)
+
+    # get result
     rclpy.spin_until_future_complete(node, future)
     res = future.result()
 
-    # TODO(murooka) error handling
-    if res.message != "":
-        if True:
-            pass
-        else:
-            # invalid
-            pass
+    # error handling
+    if res.message == "MapNotFound":
+        return jsonify(code=res.message, message="Map is missing."), 404
+    elif res.message == "LaneletsNotConnected":
+        return (
+            jsonify(
+                code=res.message,
+                message="Lanelets are not connected.",
+                object_ids=tuple(res.unconnected_lane_ids),
+            ),
+            400,
+        )
+    elif res.message != "":
+        return (
+            jsonify(
+                code="InternalServerError",
+                message="Error occurred on the server. Please check the terminal.",
+            ),
+            500,
+        )
 
     # create output json
     result_json = []
@@ -120,7 +164,7 @@ def post_planned_path():
             current_lane_points.append(point)
 
         current_result_json = {}
-        current_result_json["laneId"] = int(points_with_lane_id.lane_id)
+        current_result_json["lane_id"] = int(points_with_lane_id.lane_id)
         current_result_json["points"] = current_lane_points
 
         result_json.append(current_result_json)