Skip to content

Commit

Permalink
tmp
Browse files Browse the repository at this point in the history
  • Loading branch information
takayuki5168 committed Apr 1, 2022
1 parent d9ef2ed commit 7121ce0
Show file tree
Hide file tree
Showing 5 changed files with 251 additions and 43 deletions.
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
/**:
ros__parameters:
common:
# planning_method: "rule_base"
planning_method: "optimization_base"
planning_method: "rule_base"
# planning_method: "optimization_base"

max_accel: 1.0
min_accel: -1.0
Expand All @@ -12,7 +12,9 @@
min_object_accel: -1.0

obstacle_filtering:
detection_area_expand_width : 3.0
rough_detection_area_expand_width : 5.0
detection_area_expand_width : 0.0
decimate_step_length : 1.0
min_obstacle_velocity : 3.0
margin_for_collision_time : 3.0
max_ego_obj_overlap_time : 1.0
Expand Down
89 changes: 89 additions & 0 deletions planning/obstacle_velocity_planner/config/test.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<Tab containers="1" tab_name="slow down">
<Container>
<DockSplitter count="2" orientation="-" sizes="0.501124;0.498876">
<DockSplitter count="2" orientation="|" sizes="0.500321;0.499679">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range bottom="-0.100000" left="1648797252.376029" right="1648797352.367876" top="0.100000"/>
<limitY/>
<curve color="#1ac938" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.3"/>
<curve color="#f14cc1" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.5"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range bottom="-0.293527" left="1648797252.376029" right="1648797352.367876" top="12.034587"/>
<limitY/>
<curve color="#1f77b4" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.0"/>
<curve color="#ff7f0e" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.4"/>
<curve color="#9467bd" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.6"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter count="2" orientation="|" sizes="0.500321;0.499679">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range bottom="-0.100000" left="1648797252.376029" right="1648797352.367876" top="0.100000"/>
<limitY/>
<curve color="#d62728" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.1"/>
<curve color="#17becf" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.7"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range bottom="-0.100000" left="1648797252.376029" right="1648797352.367876" top="0.100000"/>
<limitY/>
<curve color="#bcbd22" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_planner/debug/values/data.9"/>
</plot>
</DockArea>
</DockSplitter>
</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>

Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ using nav_msgs::msg::Odometry;
using tier4_planning_msgs::msg::StopReasonArray;
using tier4_planning_msgs::msg::VelocityLimit;
using tier4_planning_msgs::msg::VelocityLimitClearCommand;
using tier4_debug_msgs::msg::Float32Stamped;
using vehicle_info_util::VehicleInfo;

namespace motion_planning
Expand Down Expand Up @@ -111,8 +112,8 @@ class ObstacleVelocityPlannerNode : public rclcpp::Node
rclcpp::Publisher<Trajectory>::SharedPtr trajectory_pub_;
rclcpp::Publisher<VelocityLimit>::SharedPtr vel_limit_pub_;
rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr clear_vel_limit_pub_;

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_pub_;

OnSetParametersCallbackHandle::SharedPtr set_param_res_;

Expand All @@ -123,11 +124,16 @@ class ObstacleVelocityPlannerNode : public rclcpp::Node
VehicleInfo vehicle_info_;

// Obstacle filtering
double detection_area_expand_width_;
double min_obstacle_velocity_;
double margin_for_collision_time_;
double max_ego_obj_overlap_time_;
double max_prediction_time_for_collision_check_;
struct ObstacleFilteringParam {
double rough_detection_area_expand_width;
double detection_area_expand_width;
double decimate_step_length;
double min_obstacle_velocity;
double margin_for_collision_time;
double max_ego_obj_overlap_time;
double max_prediction_time_for_collision_check;
};
ObstacleFilteringParam obstacle_filtering_param_;

// Mutex
std::mutex mutex_;
Expand Down
Loading

0 comments on commit 7121ce0

Please sign in to comment.