feat: add planning and control (#42)

* feat(aichallenge_submit_launch): replace from autoware launch to aichallenge submit launch * revert: this line to original * chore: remove unused * chore: move map dir according to launch * feat: add planning and control Signed-off-by: Masahiro Kubota <[email protected]> * fix: modify accel map based on rosba Signed-off-by: Masahiro Kubota <[email protected]> * fix: modify rosbag Signed-off-by: Masahiro Kubota <[email protected]> * chore: delete redundant file Signed-off-by: Masahiro Kubota <[email protected]> --------- Signed-off-by: Masahiro Kubota <[email protected]> Co-authored-by: taikitanaka3 <[email protected]>
booars · Aug 25, 2024 · 531cf90 · 531cf90
1 parent fb563af
commit 531cf90
Show file tree

Hide file tree

Showing 3 changed files with 147 additions and 7 deletions.
diff --git a/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_launch/data/accel_map.csv
@@ -1,7 +1,8 @@
-default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
-0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
-0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28
-0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03
-0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58
-0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1
-0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61
+default,0.0,1.0,2.0,3.0,4.0,5.0
+0.0,0.0,0.0,0.0,0.0,0.0,0.0
+0.1,0.0,0.0,0.0,0.0,0.0,0.0
+0.2,0.0,0.0,0.0,0.0,0.0,0.0
+0.3,0.05,0.05,0.05,0.05,0.05,0.05
+0.4,0.1,0.1,0.1,0.1,0.1,0.1
+0.5,0.175,0.175,0.175,0.175,0.175,0.175
+0.6,0.25,0.25,0.25,0.25,0.25,0.25
diff --git a/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml b/aichallenge/workspace/src/aichallenge_launch/launch/aichallenge.launch.xml
@@ -92,6 +92,127 @@
     </group>
 
   </group>
+  <!-- Dummy Perception -->
+  <node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
+    <remap from="~/output/objects" to="/perception/object_recognition/objects"/>
+  </node>
+
+  <!-- Planning -->
+  <group>
+    <push-ros-namespace namespace="planning"/>
+
+    <!-- mission_planning -->
+    <group>
+      <push-ros-namespace namespace="mission_planning"/>
+
+      <!-- mission_planner -->
+      <node pkg="mission_planner" exec="mission_planner" name="mission_planner" output="screen">
+        <remap from="input/modified_goal" to="/planning/scenario_planning/modified_goal"/>
+        <remap from="input/vector_map" to="/map/vector_map"/>
+        <!-- <remap from="/localization/kinematic_state" to="/awsim/ground_truth/localization/kinematic_state"/> -->
+        <remap from="debug/route_marker" to="/planning/mission_planning/route_marker"/>
+        <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
+      </node>
+
+      <!-- goal_pose_visualizer -->
+      <node pkg="mission_planner" exec="goal_pose_visualizer" name="goal_pose_visualizer" output="screen">
+        <remap from="input/route" to="/planning/mission_planning/route"/>
+        <remap from="output/goal_pose" to="/planning/mission_planning/echo_back_goal_pose"/>
+      </node>
+
+    </group> <!-- mission_planning -->
+
+    <!-- scenario_planning -->
+    <group>
+      <push-ros-namespace namespace="scenario_planning"/>
+
+      <!-- scenario_selector -->
+      <group>
+        <arg name="cmd" default="ros2 topic pub /planning/scenario_planning/scenario tier4_planning_msgs/msg/Scenario '{current_scenario: LaneDriving, activating_scenarios: [LaneDriving]}'"/>
+        <executable cmd="$(var cmd)" name="scenario_pub" shell="true"/>
+      </group> <!-- scenario_selector -->
+
+      <!-- operation_mode -->
+      <group>
+        <arg name="cmd" default="ros2 topic pub /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState '{
+          mode: 1,
+          is_autoware_control_enabled: true,
+          is_in_transition: false,
+          is_stop_mode_available: true,
+          is_autonomous_mode_available: true,
+          is_local_mode_available: true,
+          is_remote_mode_available: true
+          }'"/>
+        <executable cmd="$(var cmd)" name="operation_mode_pub" shell="true"/>
+      </group> <!-- operation_mode -->
+
+      <!-- lane_driving -->
+      <group>
+        <push-ros-namespace namespace="lane_driving"/>
+
+        <!-- behavior_planning -->
+        <group>
+          <push-ros-namespace namespace="behavior_planning"/>
+
+          <!-- behavior_planning_container -->
+          <node_container pkg="rclcpp_components" exec="component_container" name="behavior_planning_container" namespace="">
+
+            <!-- behavior_path_planner::BehaviorPathPlannerNode -->
+            <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
+              <remap from="~/input/route" to="/planning/mission_planning/route" />
+              <remap from="~/input/vector_map" to="/map/vector_map" />
+              <remap from="~/input/perception" to="/perception/object_recognition/objects" /> <!-- autoware_auto_perception_msgs/PredictedObjects -->
+              <remap from="~/input/occupancy_grid_map" to="/perception/occupancy_grid_map/map" />
+              <remap from="~/input/costmap" to="/planning/scenario_planning/parking/costmap_generator/occupancy_grid" />
+              <remap from="~/input/odometry" to="/localization/kinematic_state" />
+              <remap from="~/input/accel" to="/localization/acceleration" />
+              <remap from="~/input/scenario" to="/planning/scenario_planning/scenario" />
+              <remap from="~/output/path" to="path_with_lane_id" />
+              <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd" />
+              <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd" />
+              <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal" />
+              <param name="bt_tree_config_path" value="$(find-pkg-share aichallenge_submit_launch)/config/behavior_path_planner_tree.xml"/>
+              <param name="lane_change.enable_abort_lane_change" value="false"/>
+              <param name="lane_change.enable_collision_check_at_prepare_phase" value="false"/>
+              <param name="lane_change.use_predicted_path_outside_lanelet" value="false"/>
+              <param name="lane_change.use_all_predicted_path" value="false"/>
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/common/nearest_search.param.yaml" />
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml" />
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml" />
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml" />
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml" />
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml" />
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/behavior_path_planner.param.yaml" />
+              <param from="$(find-pkg-share aichallenge_submit_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml" />
+              <extra_arg name="use_intra_process_comms" value="false" />
+            </composable_node>
+
+          </node_container>
+        </group> <!-- behavior_planning -->
+      </group> <!-- lane_driving -->
+
+      <!-- Customizable -->
+      <node pkg="path_to_trajectory" exec="path_to_trajectory_node" name="path_to_trajectory" output="screen">
+        <remap from="input" to="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
+        <remap from="output" to="/planning/scenario_planning/trajectory"/>
+      </node>
+
+    </group>
+  </group>
+
+  <node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node" output="screen">
+    <param name="use_external_target_vel" value="true"/>
+    <param name="external_target_vel" value="8.0"/>
+    <param name="lookahead_gain" value="0.3"/>
+    <param name="lookahead_min_distance" value="4.0"/>
+    <param name="speed_proportional_gain" value="1.0"/>
+
+    <remap from="input/kinematics" to="/localization/kinematic_state"/>
+    <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
+    <remap from="output/control_cmd" to="/control/command/control_cmd"/>
+  </node>
+
+
   <!-- Map -->
   <group>
     <push-ros-namespace namespace="map"/>
@@ -146,4 +267,13 @@
     </include>
   </group>
 
+  <!-- API -->
+  <group>
+    <!-- default_ad_api -->
+    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py" />
+
+    <!-- ad_api_adaptors -->
+    <include file="$(find-pkg-share ad_api_adaptors)/launch/rviz_adaptors.launch.xml" />
+  </group>
+
 </launch>
diff --git a/aichallenge/workspace/src/aichallenge_launch/package.xml b/aichallenge/workspace/src/aichallenge_launch/package.xml
@@ -26,6 +26,15 @@
   <exec_depend>racing_kart_description</exec_depend>
   <exec_depend>racing_kart_sensor_kit_description</exec_depend>
 
+  <exec_depend>dummy_perception_publisher</exec_depend>
+  <exec_depend>mission_planner</exec_depend>
+  <exec_depend>behavior_path_planner</exec_depend>
+  <exec_depend>path_to_trajectory</exec_depend>
+  <exec_depend>default_ad_api</exec_depend>
+  <exec_depend>ad_api_adaptors</exec_depend>
+  <exec_depend>simple_pure_pursuit</exec_depend>
+
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>