mitsudome-r
diff --git a/‎planning/obstacle_stop_planner/CMakeLists.txt
+51 b/‎planning/obstacle_stop_planner/CMakeLists.txt
+51
diff --git a/‎planning/obstacle_stop_planner/README.md
+282 b/‎planning/obstacle_stop_planner/README.md
+282
diff --git a/‎planning/obstacle_stop_planner/config/adaptive_cruise_control.param.yaml
+40 b/‎planning/obstacle_stop_planner/config/adaptive_cruise_control.param.yaml
+40
diff --git a/‎planning/obstacle_stop_planner/config/common.param.yaml
+15 b/‎planning/obstacle_stop_planner/config/common.param.yaml
+15
diff --git a/‎planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
+25 b/‎planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
+25
diff --git a/‎planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml
+133 b/‎planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml
+133
diff --git a/‎planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml
+93 b/‎planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml
+93
@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.5)
+project(obstacle_stop_planner)
+
+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(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(PCL REQUIRED)
+
+ament_auto_add_library(obstacle_stop_planner SHARED
+  src/debug_marker.cpp
+  src/node.cpp
+  src/adaptive_cruise_control.cpp
+)
+
+target_include_directories(obstacle_stop_planner
+  PUBLIC
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+target_link_libraries(obstacle_stop_planner
+    ${OpenCV_LIBRARIES}
+    ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(obstacle_stop_planner
+  PLUGIN "motion_planning::ObstacleStopPlannerNode"
+  EXECUTABLE obstacle_stop_planner_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
+)
@@ -0,0 +1,40 @@
+/**:
+  ros__parameters:
+    adaptive_cruise_control:
+      # Adaptive Cruise Control (ACC) config
+      use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not
+      use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not
+      consider_obj_velocity: true # consider forward vehicle velocity to ACC or not
+
+      # general parameter for ACC
+      obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s]
+      obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s]
+      emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss]
+      emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s]
+      min_dist_stop: 4.0 # minimum distance of emergency stop [m]
+      obstacle_emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss]
+      max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss]
+      min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control
+      standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s]
+      min_dist_standard: 4.0 # minimum distance in active cruise control [m]
+      obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss]
+      margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-]
+      use_time_compensation_to_calc_distance: true # use time-compensation to calculate distance to forward vehicle
+
+      # pid parameter for ACC
+      p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-]
+      p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-]
+      d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-]
+      d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-]
+
+      # parameter for object velocity estimation
+      object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m]
+      object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m]
+      valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s]
+      valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s]
+      valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s]
+      valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s]
+      thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s]
+      lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity
+      use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####)
+      rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value
@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    # constraints param for normal driving
+    normal:
+      min_acc: -0.5         # min deceleration [m/ss]
+      max_acc: 1.0          # max acceleration [m/ss]
+      min_jerk: -0.5        # min jerk [m/sss]
+      max_jerk: 1.0         # max jerk [m/sss]
+
+    # constraints to be observed
+    limit:
+      min_acc: -2.5         # min deceleration limit [m/ss]
+      max_acc: 1.0          # max acceleration limit [m/ss]
+      min_jerk: -1.5        # min jerk limit [m/sss]
+      max_jerk: 1.5         # max jerk limit [m/sss]
@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    stop_planner:
+      stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
+      min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m]
+      step_length: 1.0 # step length for pointcloud search range [m]
+      extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
+      expand_stop_range: 0.0 # margin of vehicle footprint [m]
+
+    slow_down_planner:
+      # slow down planner parameters
+      forward_margin: 5.0 # margin distance from slow down point to vehicle front [m]
+      backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m]
+      expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
+      max_slow_down_vel: 1.38 # max slow down velocity [m/s]
+      min_slow_down_vel: 0.28 # min slow down velocity [m/s]
+
+      # slow down constraint parameters
+      consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel
+      forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s]
+      forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s]
+      jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss]
+      jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
+      jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
+      slow_down_vel: 1.38 # target slow down velocity [m/s]
@@ -0,0 +1,133 @@
+<?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="Estimated Object Velocity">
+        <plot mode="TimeSeries" style="Lines">
+         <range bottom="2.587438" top="3.256721" left="48.265130" right="51.387770"/>
+         <limitY/>
+         <curve name="" color="" />
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.0" color="#1f77b4">
+          <transform alias="pcl-based obj vel" 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/obstacle_stop_planner/debug_values/data.1" color="#d62728">
+         <transform alias="dynamic-obj-based obj vel" 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/obstacle_stop_planner/debug_values/data.2" color="#1ac938">
+         <transform alias="final estimated obj vel" 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/obstacle_stop_planner/debug_values/data.3" color="#ff7f0e">
+         <transform alias="dist" 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">
+        <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/obstacle_stop_planner/debug_values/data.4" 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/obstacle_stop_planner/debug_values/data.8" color="#9467bd">
+         <transform alias="Raw Upper 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/obstacle_stop_planner/debug_values/data.9" color="#17becf">
+         <transform alias="Upper Velocity" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/control/trajectory_follower/velocity_controller/debug_values/data.2" color="#bcbd22">
+         <transform alias="Target Velocity" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/control/trajectory_follower/velocity_controller/debug_values/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="Accel">
+        <plot mode="TimeSeries" style="Lines">
+         <range bottom="-0.002602" top="0.106692" left="45.299350" right="49.966869"/>
+         <limitY/>
+         <curve name="/control/trajectory_follower/velocity_controller/debug_values/data.25" color="#d62728">
+         <transform alias="Current Accel" name="Scale/Offset">
+           <options value_offset="0" value_scale="1.0" time_offset="0"/>
+          </transform>
+         </curve>
+         <curve name="/control/trajectory_follower/velocity_controller/debug_values/data.3" color="#1ac938">
+         <transform alias="Target Accel" 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/obstacle_stop_planner/debug_values"/>
+    <topic name="/control/trajectory_follower/velocity_controller/debug_values"/>
+   </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>
@@ -0,0 +1,93 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root version="2.3.8">
+ <tabbed_widget name="Main Window" parent="main_window">
+  <Tab containers="1" tab_name="tab1">
+   <Container>
+    <DockSplitter sizes="0.503972;0.496028" orientation="-" count="2">
+     <DockArea name="...">
+      <plot mode="TimeSeries" style="Lines">
+       <range top="8.000000" bottom="-2.500000" left="96.194865" right="196.174884"/>
+       <limitY min="-2.5" max="8"/>
+       <curve name="/planning/scenario_planning/motion_velocity_smoother/closest_acceleration/data" color="#fff700"/>
+       <curve name="/planning/scenario_planning/motion_velocity_smoother/closest_jerk/data" color="#ffffff"/>
+       <curve name="/planning/scenario_planning/motion_velocity_smoother/closest_velocity/data" color="#6117cf"/>
+       <curve name="/planning/scenario_planning/motion_velocity_smoother/closest_max_velocity/data" color="#949cfe"/>
+       <curve name="/localization/twist/twist/linear/x" color="#1f77b4"/>
+       <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.1" color="#d62728"/>
+       <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.4" color="#1ac938"/>
+       <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.5" color="#ff7f0e"/>
+       <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.7" color="#abfff2"/>
+       <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.6" color="#10001f"/>
+      </plot>
+     </DockArea>
+     <DockSplitter sizes="0.5;0.5" orientation="|" count="2">
+      <DockSplitter sizes="0.500891;0.499109" orientation="-" count="2">
+       <DockArea name="...">
+        <plot mode="TimeSeries" style="Dots">
+         <range top="46.453422" bottom="-2.151529" left="96.202174" right="196.174082"/>
+         <limitY/>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.3" color="#17becf"/>
+        </plot>
+       </DockArea>
+       <DockArea name="...">
+        <plot mode="TimeSeries" style="Lines">
+         <range top="0.100000" bottom="-0.100000" left="96.202174" right="196.174082"/>
+         <limitY/>
+         <curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.2" color="#bcbd22"/>
+        </plot>
+       </DockArea>
+      </DockSplitter>
+      <DockArea name="...">
+       <plot mode="XYPlot" style="Dots">
+        <range top="10.000000" bottom="0.000000" left="-2.151529" right="46.453422"/>
+        <limitY min="0" max="10"/>
+        <curve curve_y="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.0" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.[3;0]" curve_x="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.3" color="#5dff5f"/>
+       </plot>
+      </DockArea>
+     </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="ROS2 Topic Subscriber">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="false"/>
+   <max_array_size value="true"/>
+   <selected_topics>
+    <topic name="/planning/scenario_planning/max_velocity"/>
+    <topic name="/planning/scenario_planning/motion_velocity_smoother/closest_acceleration"/>
+    <topic name="/planning/scenario_planning/motion_velocity_smoother/closest_jerk"/>
+    <topic name="/planning/scenario_planning/motion_velocity_smoother/closest_max_velocity"/>
+    <topic name="/planning/scenario_planning/motion_velocity_smoother/closest_velocity"/>
+    <topic name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values"/>
+    <topic name="/localization/twist"/>
+   </selected_topics>
+  </plugin>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="CSV Exporter" status="idle"/>
+  <plugin ID="ROS2 Topic Re-Publisher" status="idle"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>