add adaptive_cruise_controller package

autowarefoundation · Apr 1, 2022 · bfd51b7 · bfd51b7
1 parent de721d4
commit bfd51b7
Show file tree

Hide file tree

Showing 16 changed files with 2,325 additions and 0 deletions.
diff --git a/planning/adaptive_cruise_controller/CMakeLists.txt b/planning/adaptive_cruise_controller/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.5)
+project(adaptive_cruise_controller)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(OpenCV REQUIRED)
+
+ament_auto_add_library(adaptive_cruise_controller SHARED
+  src/adaptive_cruise_control.cpp
+  src/adaptive_cruise_control_core.cpp
+  src/acc_pid.cpp
+  src/debug_marker.cpp
+  src/utilities.cpp
+)
+
+target_include_directories(adaptive_cruise_controller
+  PUBLIC
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+target_link_libraries(adaptive_cruise_controller
+  ${OpenCV_LIBRARIES}
+)
+
+
+rclcpp_components_register_node(adaptive_cruise_controller
+  PLUGIN "motion_planning::AdaptiveCruiseControllerNode"
+  EXECUTABLE adaptive_cruise_controller_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)
diff --git a/planning/adaptive_cruise_controller/README.md b/planning/adaptive_cruise_controller/README.md
@@ -0,0 +1,25 @@
+# Adaptive Cruise Controller
+
+## Overview
+
+## Input topics
+
+| Name                        | Type                                            | Description       |
+| --------------------------- | ----------------------------------------------- | ----------------- |
+| `~/input/trajectory`        | autoware_auto_planning_msgs::Trajectory         | trajectory        |
+| `~/input/odometry`          | nav_msgs::Odometry                              | vehicle velocity  |
+| `~/input/predicted_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects   |
+
+## Output topics
+
+| Name                   | Type                                    | Description                            |
+| ---------------------- | --------------------------------------- | -------------------------------------- |
+| `~output/trajectory`   | autoware_auto_planning_msgs::Trajectory | trajectory to be followed              |
+| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray    | reasons that cause the vehicle to stop |
+
+
+## Role
+
+## Flowchart
+
+## Known Limits
diff --git a/planning/adaptive_cruise_controller/config/adaptive_cruise_control.param.yaml b/planning/adaptive_cruise_controller/config/adaptive_cruise_control.param.yaml
@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    decimate_step_length: 1.0
+    trajectory_extend_length: 5.0
+    rough_detection_area_expand_length: 3.0
+    detection_area_expand_length: 0.0
+    object_threshold_angle: 0.523
+    predicted_path_threshold_angle: 0.523
+    mininum_overlap_time_of_predicted_path: 2.0
+
+
+
+    object_low_velocity_thresh: 3.0 # if object velocity is lower than this value, adaptive cruise becomes disable.
+    object_stop_velocity_thresh: 1.0 # if object velocity is lower than this value, ignore the sign of object orientation
+    object_velocity_hysteresis_margin: 1.0 # hysteresis margin to change acc state for preventing chattering
+    reset_time_to_acc_state: 1.0
+    velocity_to_accleration_weight: 0.3 # target accleration is calculated by (target_velocity - current_velocity) * velocity_to_accleration_weight
+    acc_min_acceleration: -1.5
+    acc_min_jerk: -1.0
+    stop_min_acceleration: -3.0
+    object_min_acceleration: -3.0
+    minimum_margin_distance: 5.0
+    idling_time: 1.0
+    breaking_delay_time: 0.5
+    p_term_in_velocity_pid: 0.3
diff --git a/planning/adaptive_cruise_controller/config/plot_jugger_adaptive_cruise.control.xml b/planning/adaptive_cruise_controller/config/plot_jugger_adaptive_cruise.control.xml
@@ -0,0 +1,157 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root version="2.3.8">
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab containers="1" tab_name="tab1">
+   <Container>
+    <DockSplitter orientation="-" count="1" sizes="1">
+     <DockSplitter orientation="|" count="2" sizes="0.5;0.5">
+      <DockSplitter orientation="-" count="2" sizes="0.500286;0.499714">
+       <DockArea name="Object Velocity/Acceleration">
+        <plot mode="TimeSeries" style="Lines">
+         <range bottom="2.587438" top="3.256721" left="48.265130" right="51.387770"/>
+         <limitY/>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.2" color="#1f77b4">
+          <transform alias="object velocity" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.3" color="#d62728">
+         <transform alias="object velocity(estimated from position)" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.4" color="#1ac938">
+         <transform alias="object accleration" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+        </plot>
+       </DockArea>
+       <DockArea name="Distance To Forward Object">
+        <plot mode="TimeSeries" style="Lines">
+         <range bottom="21.768126" top="22.395437" left="48.265130" right="51.387770"/>
+         <limitY/>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.5" color="#ff7f0e">
+         <transform alias="current distance to object" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.6" color="#1ac938">
+         <transform alias="ideal distance to object" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+        </plot>
+       </DockArea>
+      </DockSplitter>
+      <DockSplitter orientation="-" count="2" sizes="0.500286;0.499714">
+       <DockArea name="Ego velocity/acceleration">
+        <plot mode="TimeSeries" style="Lines">
+         <range bottom="-0.045951" top="1.884001" left="45.299350" right="51.387770"/>
+         <limitY/>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.0" color="#f14cc1">
+         <transform alias="current velocity" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.13" color="#9467bd">
+         <transform alias="target velocity in acc" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.1" color="#17becf">
+         <transform alias="current accleration" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/control/trajectory_follower/longitudinal/diagnostic/diag_array/data.2" color="#bcbd22">
+         <transform alias="target velocity in velocity controller" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/control/trajectory_follower/longitudinal/diagnostic/diag_array/data.4" color="#1f77b4">
+         <transform alias="closest velocity" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+        </plot>
+       </DockArea>
+       <DockArea name="Flag">
+        <plot mode="TimeSeries" style="Lines">
+         <range bottom="-0.002602" top="0.106692" left="45.299350" right="49.966869"/>
+         <limitY/>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.7" color="#d62728">
+         <transform alias="cut in" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.2" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.8" color="#1ac938">
+         <transform alias="cut out" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.2" time_offset="0"/>
+          </transform>
+         </curve>           
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.9" color="#1f77b4">
+         <transform alias="target object" name="Scale/Offset">
+           <options value_offset="0" value_scale="0.8" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.10" color="#bcbd22">
+         <transform alias="acc mode" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.11" color="#17becf">
+         <transform alias="stop mode" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values/data.12" color="#9467bd">
+         <transform alias="none mode" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+        </plot>
+       </DockArea>
+      </DockSplitter>
+     </DockSplitter>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis=""/>
+  </plugin>
+  <plugin ID="DataLoad ROS2 bags">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+  </plugin>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="MQTT Subscriber"/>
+  <plugin ID="ROS2 Topic Subscriber">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="true"/>
+   <selected_topics>
+    <topic name="/planning/scenario_planning/lane_driving/motion_planning/adaptive_cruise_controller/debug/debug_values"/>
+    <topic name="/control/trajectory_follower/longitudinal/diagnostic"/>
+   </selected_topics>
+  </plugin>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin status="idle" ID="ROS2 Topic Re-Publisher"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
diff --git a/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/acc_pid.hpp b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/acc_pid.hpp
@@ -0,0 +1,53 @@
+// Copyright 2020 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.
+
+#ifndef ADAPTIVE_CRUISE_CONTROLLER__ACC_PID_HPP_
+#define ADAPTIVE_CRUISE_CONTROLLER__ACC_PID_HPP_
+
+#include <adaptive_cruise_controller/utilities.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace motion_planning
+{
+
+class AccPidNode
+{
+public:
+  explicit AccPidNode(const double baselink2front, const AccParam & acc_param);
+
+  void updateState(
+    const rclcpp::Time & current_time, const double ego_velocity, const double obstacle_velocity,
+    const double dist_to_obstacle);
+  State getState() { return current_state_; }
+  void calculate(const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion);
+
+private:
+  double calcStoppingDistFromCurrentVel(const double current_velocity);
+  double calcEmergencyDistFromVel(const double current_velocity, const double obj_velocity);
+  void calculateTargetMotion(const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion);
+  void calcTrajectoryWithStopPoints(
+    const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion);
+  TrajectoryPoints insertStopPoint(
+    const double stop_distance_from_ego, const TrajectoryPoints & trajectory_points,
+    const geometry_msgs::msg::Pose & ego_pose, geometry_msgs::msg::Pose & stop_pose);
+
+  double baselink2front_;
+  AccParam acc_param_;
+  State current_state_ = State::NONE;
+  std::shared_ptr<AdaptiveCruiseInformation> prev_acc_info_ptr_;
+};
+
+}  // namespace motion_planning
+
+#endif  // ADAPTIVE_CRUISE_CONTROLLER__ACC_PID_HPP_