Add autoware api (autowarefoundation#376)

* Add external api adaptor (autowarefoundation#267) * Add external api adaptor * Add api adaptor to logging simulator Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add engage status output Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add internal api adaptor (autowarefoundation#273) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add map hash generator (autowarefoundation#319) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add autoware api launch (autowarefoundation#326) * Add autoware api launch Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Apply autoware api launch Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add deprecated comment Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Remove unused parameter (autowarefoundation#325) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add api parameter (autowarefoundation#341) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Add start request API (autowarefoundation#321) * Add use start request option Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Feature external cmd selector heartbeat (autowarefoundation#356) * Rename external command topic Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Modify command topic name Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Rename remote_cmd_converter Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Remove gate mode from external command Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix latest external command name (autowarefoundation#361) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix merge conflict Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * change external traffic light topic name in behavior velocity planner (autowarefoundation#310) * fix topic * change internal topic name Co-authored-by: yabuta <makoto.yabuta@tier4.jp>
TomohitoAndo · Aug 31, 2021 · 46c3fda · 46c3fda
1 parent e32dc13
commit 46c3fda
Show file tree

Hide file tree

Showing 14 changed files with 137 additions and 65 deletions.
diff --git a/autoware_api_launch/CMakeLists.txt b/autoware_api_launch/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_api_launch)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE launch)
diff --git a/autoware_api_launch/launch/autoware_api.launch.xml b/autoware_api_launch/launch/autoware_api.launch.xml
@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- awapi (deprecated) -->
+  <group>
+    <include file="$(find-pkg-share awapi_awiv_adapter)/launch/awapi_awiv_adapter.launch.xml"/>
+  </group>
+
+  <!-- autoware api adaptor -->
+  <group>
+    <push-ros-namespace namespace="autoware_api"/>
+    <include file="$(find-pkg-share autoware_iv_external_api_adaptor)/launch/external_api_adaptor.launch.py"/>
+    <include file="$(find-pkg-share autoware_iv_internal_api_adaptor)/launch/internal_api_adaptor.launch.py"/>
+    <include file="$(find-pkg-share autoware_iv_internal_api_adaptor)/launch/internal_api_relay.launch.xml"/>
+  </group>
+
+  <!-- autoware api utils -->
+  <group>
+    <push-ros-namespace namespace="autoware_api/utils"/>
+    <include file="$(find-pkg-share path_distance_calculator)/launch/path_distance_calculator.launch.xml"/>
+  </group>
+
+</launch>
diff --git a/autoware_api_launch/package.xml b/autoware_api_launch/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+
+  <name>autoware_api_launch</name>
+  <version>0.0.0</version>
+  <description>The autoware_api_launch package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <exec_depend>autoware_iv_external_api_adaptor</exec_depend>
+  <exec_depend>autoware_iv_internal_api_adaptor</exec_depend>
+  <exec_depend>path_distance_calculator</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
@@ -60,8 +60,11 @@
     <arg name="lateral_controller_mode" value="mpc_follower" />
   </include>
 
-  <!-- AutowareAPI -->
-  <include file="$(find-pkg-share awapi_awiv_adapter)/launch/awapi_awiv_adapter.launch.xml" />
+  <!-- Autoware API -->
+  <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml">
+    <arg name="init_simulator_pose" value="false"/>
+    <arg name="init_localization_pose" value="true"/>
+  </include>
 
   <!-- Rviz -->
   <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_launch)/rviz/autoware.rviz -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png" if="$(var rviz)">

diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml
@@ -43,7 +43,6 @@
     </include>
   </group>
 
-
   <!-- Map -->
   <group>
     <include file="$(find-pkg-share map_launch)/launch/map.launch.py" if="$(var map)">
@@ -52,7 +51,6 @@
     </include>
   </group>
 
-
   <!-- Sensing -->
   <group>
     <include file="$(find-pkg-share sensing_launch)/launch/sensing.launch.xml" if="$(var sensing)">
@@ -63,14 +61,12 @@
     </include>
   </group>
 
-
   <!-- Localization -->
   <group>
     <include file="$(find-pkg-share localization_launch)/launch/localization.launch.xml" if="$(var localization)">
     </include>
   </group>
 
-
   <!-- Perception -->
   <group>
     <include file="$(find-pkg-share perception_launch)/launch/perception.launch.xml" if="$(var perception)">
@@ -93,6 +89,12 @@
     </include>
   </group>
 
+  <!-- Autoware API -->
+  <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml">
+    <arg name="init_simulator_pose" value="false"/>
+    <arg name="init_localization_pose" value="true"/>
+  </include>
+
   <!-- Rviz -->
   <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_launch)/rviz/autoware.rviz -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png" if="$(var rviz)">
   </node>

diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml
@@ -63,8 +63,11 @@
     <arg name="lateral_controller_mode" value="mpc_follower" />
   </include>
 
-  <!-- AutowareAPI -->
-  <include file="$(find-pkg-share awapi_awiv_adapter)/launch/awapi_awiv_adapter.launch.xml" />
+  <!-- Autoware API -->
+  <include file="$(find-pkg-share autoware_api_launch)/launch/autoware_api.launch.xml">
+    <arg name="init_simulator_pose" value="true"/>
+    <arg name="init_localization_pose" value="false"/>
+  </include>
 
   <!-- Rviz -->
   <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_config) -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png" if="$(var rviz)"/>

diff --git a/autoware_launch/package.xml b/autoware_launch/package.xml
@@ -30,6 +30,10 @@
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-bson</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-bson</exec_depend>
 
+  <!-- api adaptor -->
+  <exec_depend>awapi_awiv_adapter</exec_depend>
+  <exec_depend>autoware_iv_external_api_adaptor</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

diff --git a/control_launch/control_launch.drawio.svg b/control_launch/control_launch.drawio.svg
diff --git a/control_launch/launch/control.launch.py b/control_launch/launch/control.launch.py
@@ -172,20 +172,19 @@ def launch_setup(context, *args, **kwargs):
         plugin='VehicleCmdGate',
         name='vehicle_cmd_gate',
         remappings=[
-            ('input/engage', '/autoware/engage'),
             ('input/system_emergency', '/system/emergency/is_emergency'),
-            ('input/external_emergency_stop', '/remote/emergency_stop'),
             ('input/emergency', '/system/emergency/is_emergency'),
-            ('input/gate_mode', '/remote/gate_mode_cmd'),
             ('input/steering', '/vehicle/status/steering'),
 
             ('input/auto/control_cmd', 'trajectory_follower/control_cmd'),
             ('input/auto/turn_signal_cmd', '/planning/turn_signal_decider/turn_signal_cmd'),
             ('input/auto/shift_cmd', '/control/shift_decider/shift_cmd'),
 
-            ('input/remote/control_cmd', '/external/external_cmd_selector/control_cmd'),
-            ('input/remote/turn_signal_cmd', '/external/external_cmd_selector/turn_signal_cmd'),
-            ('input/remote/shift_cmd', '/external/external_cmd_selector/shift_cmd'),
+            ('input/external/control_cmd', '/external/selected/control_cmd'),
+            ('input/external/turn_signal_cmd', '/external/selected/turn_signal_cmd'),
+            ('input/external/shift_cmd', '/external/selected/shift_cmd'),
+            ('input/external_emergency_stop', '/external/selected/heartbeat'),
+            ('input/gate_mode', '/control/gate_mode_cmd'),
 
             ('input/emergency/control_cmd', '/system/emergency/control_cmd'),
             ('input/emergency/turn_signal_cmd', '/system/emergency/turn_signal_cmd'),
@@ -196,7 +195,13 @@ def launch_setup(context, *args, **kwargs):
             ('output/shift_cmd', '/control/shift_cmd'),
             ('output/turn_signal_cmd', '/control/turn_signal_cmd'),
             ('output/gate_mode', '/control/current_gate_mode'),
+            ('output/engage', '/api/autoware/get/engage'),
+
+            ('~/service/engage', '/api/autoware/set/engage'),
+            ('~/service/external_emergency', '/api/autoware/set/emergency'),
 
+            # TODO(Takagi, Isamu): deprecated
+            ('input/engage', '/autoware/engage'),
             ('~/service/external_emergency_stop', '~/external_emergency_stop'),
             ('~/service/clear_external_emergency_stop', '~/clear_external_emergency_stop'),
         ],
@@ -205,6 +210,7 @@ def launch_setup(context, *args, **kwargs):
             {
                 'use_emergency_handling': LaunchConfiguration('use_emergency_handling'),
                 'use_external_emergency_stop': LaunchConfiguration('use_external_emergency_stop'),
+                'use_start_request': LaunchConfiguration('use_start_request'),
             }
         ],
         extra_arguments=[{
@@ -218,18 +224,20 @@ def launch_setup(context, *args, **kwargs):
         plugin='ExternalCmdSelector',
         name='external_cmd_selector',
         remappings=[
+            ('~/service/select_external_command', '~/select_external_command'),
             ('~/input/local/control_cmd', '/external/local/control_cmd'),
             ('~/input/local/shift_cmd', '/external/local/shift_cmd'),
             ('~/input/local/turn_signal_cmd', '/external/local/turn_signal_cmd'),
+            ('~/input/local/heartbeat', '/external/local/heartbeat'),
             ('~/input/remote/control_cmd', '/external/remote/control_cmd'),
             ('~/input/remote/shift_cmd', '/external/remote/shift_cmd'),
             ('~/input/remote/turn_signal_cmd', '/external/remote/turn_signal_cmd'),
+            ('~/input/remote/heartbeat', '/external/remote/heartbeat'),
+            ('~/output/control_cmd', '/external/selected/external_control_cmd'),
+            ('~/output/shift_cmd', '/external/selected/shift_cmd'),
+            ('~/output/turn_signal_cmd', '/external/selected/turn_signal_cmd'),
+            ('~/output/heartbeat', '/external/selected/heartbeat'),
             ('~/output/current_selector_mode', '~/current_selector_mode'),
-            ('~/output/external_control_cmd',
-             '/external/external_cmd_selector/external_control_cmd'),
-            ('~/output/shift_cmd', '/external/external_cmd_selector/shift_cmd'),
-            ('~/output/turn_signal_cmd', '/external/external_cmd_selector/turn_signal_cmd'),
-            ('~/service/select_external_command', '~/select_external_command'),
         ],
         parameters=[
             {
@@ -241,19 +249,19 @@ def launch_setup(context, *args, **kwargs):
         }],
     )
 
-    # remote cmd converter
-    remote_cmd_converter_component = ComposableNode(
-        package='remote_cmd_converter',
-        plugin='RemoteCmdConverter',
-        name='remote_cmd_converter',
+    # external cmd converter
+    external_cmd_converter_component = ComposableNode(
+        package='external_cmd_converter',
+        plugin='external_cmd_converter::ExternalCmdConverterNode',
+        name='external_cmd_converter',
         remappings=[
-            ('in/external_control_cmd', '/external/external_cmd_selector/external_control_cmd'),
-            ('in/shift_cmd', '/external/external_cmd_selector/shift_cmd'),
+            ('in/external_control_cmd', '/external/selected/external_control_cmd'),
+            ('in/shift_cmd', '/external/selected/shift_cmd'),
             ('in/emergency_stop', '/remote/emergency_stop'),
             ('in/current_gate_mode', '/control/current_gate_mode'),
             ('in/twist', '/localization/twist'),
-            ('out/control_cmd', '/external/external_cmd_selector/control_cmd'),
-            ('out/latest_remote_control_cmd', '/remote/latest_remote_control_cmd'),
+            ('out/control_cmd', '/external/selected/control_cmd'),
+            ('out/latest_external_control_cmd', '/external/selected/latest_external_control_cmd'),
         ],
         parameters=[
             {
@@ -283,7 +291,7 @@ def launch_setup(context, *args, **kwargs):
             lane_departure_component,
             shift_decider_component,
             vehicle_cmd_gate_component,
-            remote_cmd_converter_component,
+            external_cmd_converter_component,
             external_cmd_selector_component,
         ],
     )
@@ -372,9 +380,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
     # vehicle cmd gate
     add_launch_arg('use_emergency_handling', 'false', 'use emergency handling')
     add_launch_arg('use_external_emergency_stop', 'true', 'use external emergency stop')
+    add_launch_arg('use_start_request', 'false', 'use start request service')
 
     # external cmd selector
-    add_launch_arg('initial_selector_mode', '1', '0: Local, 1: Remote')
+    add_launch_arg('initial_selector_mode', 'remote', 'local or remote')
 
     # remote cmd converter
     add_launch_arg(

diff --git a/control_launch/launch/control.launch.xml b/control_launch/launch/control.launch.xml
@@ -58,43 +58,24 @@
       <arg name="use_external_emergency_stop" value="true"/>
     </include>
 
-    <!-- For backward compatibility of external commands -->
-    <group>
-      <node pkg="topic_tools" exec="relay" name="relay_remote_remote_control_cmd">
-        <param name="input_topic" value="/remote/external_control_cmd" />
-        <param name="output_topic" value="/external/remote/external_control_cmd" />
-        <param name="type" value="autoware_vehicle_msgs/msg/RawControlCommandStamped" />
-      </node>
-      <node pkg="topic_tools" exec="relay" name="relay_remote_shift_cmd">
-        <param name="input_topic" value="/remote/shift_cmd" />
-        <param name="output_topic" value="/external/remote/shift_cmd" />
-        <param name="type" value="autoware_vehicle_msgs/msg/ShiftStamped" />
-      </node>
-      <node pkg="topic_tools" exec="relay" name="relay_remote_turn_signal_cmd">
-        <param name="input_topic" value="/remote/turn_signal_cmd" />
-        <param name="output_topic" value="/external/remote/turn_signal_cmd" />
-        <param name="type" value="autoware_vehicle_msgs/msg/TurnSignal" />
-      </node>
-    </group>
-
     <!-- external_cmd_selector -->
     <include file="$(find-pkg-share external_cmd_selector)/launch/external_cmd_selector.launch.xml">
-      <arg name="initial_selector_mode" value="1" />
-
+      <arg name="initial_selector_mode" value="remote" />
       <arg name="input/local/control_cmd" value="/external/local/control_cmd" />
       <arg name="input/local/shift_cmd" value="/external/local/shift_cmd" />
       <arg name="input/local/turn_signal_cmd" value="/external/local/turn_signal_cmd" />
-
+      <arg name="input/local/heartbeat" value="/external/local/heartbeat" />
       <arg name="input/remote/control_cmd" value="/external/remote/control_cmd" />
       <arg name="input/remote/shift_cmd" value="/external/remote/shift_cmd" />
       <arg name="input/remote/turn_signal_cmd" value="/external/remote/turn_signal_cmd" />
+      <arg name="input/remote/heartbeat" value="/external/remote/heartbeat" />
     </include>
 
-    <!-- remote_cmd_converter -->
-    <include file="$(find-pkg-share remote_cmd_converter)/launch/remote_cmd_converter.launch.xml">
-      <arg name="in/external_control_cmd" value="/external/external_cmd_selector/external_control_cmd" />
-      <arg name="in/shift_cmd" value="/external/external_cmd_selector/shift_cmd" />
-      <arg name="out/control_cmd" value="/external/external_cmd_selector/control_cmd" />
+    <!-- external_cmd_converter -->
+    <include file="$(find-pkg-share external_cmd_converter)/launch/external_cmd_converter.launch.xml">
+      <arg name="in/external_control_cmd" value="/external/selected/external_control_cmd" />
+      <arg name="in/shift_cmd" value="/external/selected/shift_cmd" />
+      <arg name="out/control_cmd" value="/external/selected/control_cmd" />
     </include>
   </group>
 </launch>
diff --git a/control_launch/package.xml b/control_launch/package.xml
@@ -14,7 +14,7 @@
   <exec_depend>latlon_muxer</exec_depend>
   <exec_depend>mpc_follower</exec_depend>
   <exec_depend>pure_pursuit</exec_depend>
-  <exec_depend>remote_cmd_converter</exec_depend>
+  <exec_depend>external_cmd_converter</exec_depend>
   <exec_depend>shift_decider</exec_depend>
   <exec_depend>vehicle_cmd_gate</exec_depend>
   <exec_depend>velocity_controller</exec_depend>

diff --git a/localization_launch/launch/util/util.launch.xml b/localization_launch/launch/util/util.launch.xml
@@ -17,10 +17,9 @@
   <arg name="use_intra_process" default="true" description="use ROS2 component container communication"/>
 
   <!-- pose_initializer -->
-  <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
-    <arg name="ndt_align_server_name" value="/localization/pose_estimator/ndt_align_srv" />
-  </include>
+  <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml" />
 
+  <!-- util -->
   <include file="$(find-pkg-share localization_launch)/launch/util/util.launch.py" />
 
 </launch>
diff --git a/map_launch/launch/map.launch.py b/map_launch/launch/map.launch.py
@@ -20,11 +20,23 @@
 from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import Node
 from launch_ros.actions import PushRosNamespace
 from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
+    map_hash_generator = Node(
+        package='map_loader',
+        executable='map_hash_generator',
+        name='map_hash_generator',
+        parameters=[
+            {
+                'lanelet2_map_path': LaunchConfiguration('lanelet2_map_path'),
+            }
+        ],
+    )
+
     lanelet2_map_loader = ComposableNode(
         package='map_loader',
         plugin='Lanelet2MapLoaderNode',
@@ -122,5 +134,6 @@ def add_launch_arg(name: str, default_value=None, description=None):
         GroupAction([
             PushRosNamespace('map'),
             container,
+            map_hash_generator,
         ])
     ])