Skip to content

Commit

Permalink
add obstacle_velocity_planner
Browse files Browse the repository at this point in the history 
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 committed May 25, 2022
1 parent d9cc4fe commit cd0c823
Show file tree
Hide file tree
Showing 33 changed files with 5,904 additions and 0 deletions.
44 changes: 44 additions & 0 deletions planning/obstacle_velocity_planner/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
cmake_minimum_required(VERSION 3.8)
project(obstacle_velocity_planner)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
# add_compile_options(-Wall -Wextra -Wpedantic -Werror)
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

find_package(Eigen3 REQUIRED)

ament_auto_add_library(obstacle_velocity_planner_core SHARED
src/node.cpp
src/utils.cpp
src/polygon_utils.cpp
src/optimization_based_planner/resample.cpp
src/optimization_based_planner/box2d.cpp
src/optimization_based_planner/velocity_optimizer.cpp
src/optimization_based_planner/optimization_based_planner.cpp
src/rule_based_planner/rule_based_planner.cpp
)

rclcpp_components_register_node(obstacle_velocity_planner_core
PLUGIN "motion_planning::ObstacleVelocityPlannerNode"
EXECUTABLE obstacle_velocity_planner
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(
INSTALL_TO_SHARE
launch
config
)

install(PROGRAMS
scripts/trajectory_visualizer.py
DESTINATION lib/${PROJECT_NAME}
)
113 changes: 113 additions & 0 deletions planning/obstacle_velocity_planner/config/obstacle_velocity_planner.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,113 @@
/**:
ros__parameters:
common:
planning_algorithm: "rule_base" # currently supported algorithm is "rule_base"

# parameters to calculate RSS distance
max_accel: 1.0
min_accel: -1.0
max_jerk: 1.0
min_jerk: -0.5
idling_time: 4.0
min_object_accel: -1.0

min_strong_stop_accel: -3.0

lpf_gain_for_accel: 0.2 # [-]

slow_down_obstacle_type:
unknown: true
car: true
truck: true
bus: true
trailer: true
motorcycle: true
bicycle: true
pedestrian: true

stop_obstacle_type:
unknown: true
car: false
truck: false
bus: false
trailer: false
motorcycle: true
bicycle: true
pedestrian: true

obstacle_filtering:
rough_detection_area_expand_width : 5.0
detection_area_expand_width : 0.0
decimate_trajectory_step_length : 2.0

min_obstacle_crossing_velocity : 3.0
crossing_obstacle_traj_angle_threshold : 0.698 # = 40 [deg]

margin_for_collision_time : 8.0
max_ego_obj_overlap_time : 1.0
max_prediction_time_for_collision_check : 20.0

rule_based_planner:
kp: 0.6 # 5.0 # 10.0 # 5.0 # 10.0 # 0.3 # 0.003
ki: 0.0
kd: 0.5 # 5.0 # 3.0 # 0.0 # 0.1
output_ratio_during_accel: 2.5 # 0.2 # 0.1

#kp: 10.0
#ki: 0.0
#kd: 3.0

#kp: 10.0
#ki: 0.0
#kd: 10.0
#output_ratio_during_accel: 0.03

vel_to_acc_weight: 8.0 # 1.0 # 0.3 # 30.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight

min_slow_down_target_vel: 3.0 # minimum target velocity during slow down

max_slow_down_obstacle_velocity_to_stop : 3.0
safe_distance_margin : 7.0

optimization_based_planner:
limit_min_accel: -3.0
resampling_s_interval: 2.0
dense_resampling_time_interval: 0.1
sparse_resampling_time_interval: 1.0
dense_time_horizon: 5.0
max_time_horizon: 25.0
max_trajectory_length: 200.0
velocity_margin: 0.1 #[m/s]

# Parameters for safe distance
safe_distance_margin: 5.0
t_dangerous: 0.5

# Parameters for collision detection
collision_time_threshold: 10.0
object_zero_velocity_threshold: 3.0 #[m/s]
object_low_velocity_threshold: 3.0 #[m/s]
external_velocity_limit: 20.0 #[m/s]
delta_yaw_threshold_of_object_and_ego: 90.0 # [deg]
delta_yaw_threshold_of_nearest_index: 60.0 # [deg]
collision_duration_threshold_of_object_and_ego: 1.0 # [s]

# Switch for each functions
enable_adaptive_cruise: true
use_object_acceleration: false

# For initial Motion
replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s]
engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement)
engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]

# Weights for optimization
max_s_weight: 100.0
max_v_weight: 1.0
over_s_safety_weight: 1000000.0
over_s_ideal_weight: 50.0
over_v_weight: 500000.0
over_a_weight: 5000.0
over_j_weight: 10000.0
150 changes: 150 additions & 0 deletions planning/obstacle_velocity_planner/config/plot_juggler_obstacle_velocity_planner.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,150 @@
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<Tab tab_name="slow down" containers="1">
<Container>
<DockSplitter sizes="1" orientation="-" count="1">
<DockSplitter sizes="0.5;0.5" orientation="|" count="2">
<DockSplitter sizes="0.500564;0.499436" orientation="-" count="2">
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-68.642152" left="1648801093.897904" right="1648801193.801909" top="94.063676"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.10" color="#1ac938"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.12" color="#17becf"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.16" color="#d62728"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-1.341975" left="1648801093.897904" right="1648801193.801909" top="1.170411"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.1" color="#f14cc1"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.14" color="#1f77b4"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter sizes="0.500564;0.499436" orientation="-" count="2">
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-0.293676" left="1648801093.897904" right="1648801193.801909" top="12.040726"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.0" color="#ff7f0e"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.11" color="#9467bd"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.13" color="#bcbd22"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-0.100000" left="1648801093.897904" right="1648801193.801909" top="0.100000"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.15" color="#1ac938"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.2" color="#ff7f0e"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<Tab tab_name="stop" containers="1">
<Container>
<DockSplitter sizes="1" orientation="-" count="1">
<DockSplitter sizes="0.5;0.5" orientation="|" count="2">
<DockSplitter sizes="0.500564;0.499436" orientation="-" count="2">
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-4.440820" left="1648801040.096838" right="1648801140.095224" top="182.073633"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.3" color="#17becf"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.5" color="#1f77b4"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.9" color="#d62728"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-1.411967" left="1648801040.096838" right="1648801140.095224" top="1.117231"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.1" color="#f14cc1"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.7" color="#1ac938"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter sizes="0.500564;0.499436" orientation="-" count="2">
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-0.364903" left="1648801040.096838" right="1648801140.095224" top="12.034235"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.0" color="#ff7f0e"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.4" color="#bcbd22"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.6" color="#d62728"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-0.025000" left="1648801040.096752" right="1648801140.095134" top="1.025000"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/output/stop_speed_exceeded/stop_speed_exceeded" color="#f14cc1"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<Tab tab_name="others" containers="1">
<Container>
<DockSplitter sizes="1" orientation="-" count="1">
<DockArea name="...">
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines">
<range bottom="-0.000303" left="1648801009.699198" right="1648801109.621976" top="0.051201"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/calculation_time/data" color="#9467bd"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<currentTabIndex index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="0"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad CSV">
<default time_axis="" delimiter="0"/>
</plugin>
<plugin ID="DataLoad ROS2 bags">
<use_header_stamp value="false"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
<boolean_strings_to_number value="true"/>
<remove_suffix_from_strings value="true"/>
</plugin>
<plugin ID="DataLoad ULog"/>
<plugin ID="ROS2 Topic Subscriber">
<use_header_stamp value="false"/>
<discard_large_arrays value="false"/>
<max_array_size value="10"/>
<boolean_strings_to_number value="true"/>
<remove_suffix_from_strings value="true"/>
</plugin>
<plugin ID="UDP Server"/>
<plugin ID="WebSocket Server"/>
<plugin ID="Fast Fourier Transform"/>
<plugin ID="Quaternion to RPY"/>
<plugin ID="Reactive Script Editor">
<library code="--[[ Helper function to create a ScatterXY series from arrays.&#xa;&#xa; series_name: name of the created ScatterXY series&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value.&#xa; if [nil], the index of the array will be used.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{N}/position/x&#xa; /trajectory/node.{N}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; CreateSeriesFromArray( &quot;my_trajectory&quot;, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;]]--&#xa;&#xa;function CreateSeriesFromArray( series_name, prefix, suffix_X, suffix_Y, timestamp )&#xa; --- create a new series or overwite the previous one&#xa; new_series = MutableScatterXY.new(series_name)&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_x == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;"/>
<scripts/>
</plugin>
<plugin ID="CSV Exporter"/>
<plugin ID="ROS2 Topic Re-Publisher"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>
Binary file added planning/obstacle_velocity_planner/image/collision_point.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added planning/obstacle_velocity_planner/image/detection_area.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added planning/obstacle_velocity_planner/image/obstacle_to_cruise.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added planning/obstacle_velocity_planner/image/obstacle_to_stop.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading

0 comments on commit cd0c823

Please sign in to comment.