enable service

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

planning/static_centerline_optimizer/README.md

@@ -16,6 +16,10 @@ There are two interfaces to communicate with the centerline optimizer.
 
 ### Vector Map Builder Interface
 
+```sh
+ros2 run static_centerline_optimizer run_planning_server.sh vehicle_model:=<vehicle-name>
+```
+
 ### Command Line Interface
 
 The optimized centerline is generated from the command line interface.

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -66,6 +66,8 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
 
   // service
   rclcpp::Service<LoadMap>::SharedPtr srv_load_map_;
+  rclcpp::Service<PlanRoute>::SharedPtr srv_plan_route_;
+  rclcpp::Service<PlanPath>::SharedPtr srv_plan_path_;
 
   // callback group for service
   rclcpp::CallbackGroup::SharedPtr callback_group_;

planning/static_centerline_optimizer/launch/run_planning_server.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <!-- mandatory arguments for planning-->
+  <arg name="vehicle_model"/>
+
+  <include file="$(find-pkg-share static_centerline_optimizer)/launch/static_centerline_optimizer.launch.xml">
+    <arg name="vehicle_model" value="$(var vehicle_model)"/>
+    <arg name="run_background" value="true"/>
+  </include>
+</launch>

planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml

@@ -1,10 +1,15 @@
 <launch>
   <!-- mandatory arguments for planning-->
-  <arg name="lanelet2_input_file_name"/>
-  <arg name="start_lanelet_id"/>
-  <arg name="end_lanelet_id"/>
   <arg name="vehicle_model"/>
+
+  <!-- flag -->
   <arg name="run_background" default="false"/>
+  <arg name="rviz" default="true"/>
+
+  <!-- mandatory arguments when run_background is true -->
+  <arg name="lanelet2_input_file_name" default=""/>
+  <arg name="start_lanelet_id" default=""/>
+  <arg name="end_lanelet_id" default=""/>
 
   <!-- topic -->
   <arg name="lanelet2_map_topic" default="/map/vector_map"/>
@@ -55,5 +60,5 @@
   </node>
 
   <!-- rviz -->
-  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share static_centerline_optimizer)/rviz/static_centerline_optimizer.rviz"/>
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share static_centerline_optimizer)/rviz/static_centerline_optimizer.rviz" if="$(var rviz)"/>
 </launch>

planning/static_centerline_optimizer/scripts/app.py

@@ -1,22 +1,42 @@
 from flask import Flask
 from flask import abort
-from flask import jsonify
-from flask import render_template
 from flask import request
 from flask import session
+import rclpy
+from rclpy.node import Node
+from static_centerline_optimizer.srv import LoadMap
+from static_centerline_optimizer.srv import PlanPath
+from static_centerline_optimizer.srv import PlanRoute
+
+rclpy.init()
+node = Node("app")
 
 app = Flask(__name__)
 app.secret_key = "hogehoge"
 
+
+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
+
+
 @app.route("/load_map", methods=["POST"])
 def load_map_post():
     data = request.get_json()
-    map = data["map"]
     session["map_id"] = 1
-    print(session["map_id"])
 
-    # load map(map)
-    ros2 service call /load_map static_centerline_optimizer/srv/LoadMap 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...")
+
+    # request map loading
+    req = LoadMap.Request()
+    req.map = data["map"]
+    cli.call_async(req)
+
     error = False
 
     if error:
@@ -29,31 +49,41 @@ def load_map_post():
 def plan_route_post():
     data = request.get_json()
 
-    print(session["map_id"])
-    # map_id = data['mapId']
-    # start_lanelet_id = data['startLaneletId']
-    # end_lanelet_id = data['endLaneletId']
-    # plan route(start_lanelet_id, end_lanelet_id)
+    # 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...")
+
+    # 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)
 
     error = False
 
     if error:
         abort(500, "error_message")
 
-    return render_template("index.html")
+    return {"po": "po"}
 
 
 @app.route("/plan_path", methods=["POST"])
 def plan_path_post():
     data = request.get_json()
 
-    map_id = data["mapId"]
-    vehicle_model = data["vehicleModel"]
-    route = data["route"]
+    # 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...")
 
-    # plan path(vehicle_model, route)
+    # request path planning
+    req = PlanPath.Request()
+    req.start_lane_id = data["start_lane_id"]
+    cli.call_async(req)
 
-    return render_template("index.html")
+    return {"po": "po"}
 
 
 if __name__ == "__main__":