forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
refactor(pure_pursuit): make lookahead distance calculation more tuna…
…ble (autowarefoundation#2127)
- Loading branch information
Showing
4 changed files
with
245 additions
and
38 deletions.
There are no files selected for viewing
114 changes: 114 additions & 0 deletions
114
control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,114 @@ | ||
| <?xml version='1.0' encoding='UTF-8'?> | ||
| <root> | ||
| <tabbed_widget parent="main_window" name="Main Window"> | ||
| <Tab containers="1" tab_name="tab1"> | ||
| <Container> | ||
| <DockSplitter sizes="0.5;0.5" orientation="-" count="2"> | ||
| <DockArea name="Lookahead Distance"> | ||
| <plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
| <range right="80.040302" top="4.460166" bottom="-0.166817" left="0.000000"/> | ||
| <limitY/> | ||
| <curve color="#1f77b4" name="/control/trajectory_follower/controller_node_exe/debug/ld_outputs/data.0"> | ||
| <transform alias="Velocity_LD" name="Scale/Offset"> | ||
| <options value_scale="1.0" time_offset="0" value_offset="0"/> | ||
| </transform> | ||
| </curve> | ||
| <curve color="#d62728" name="/control/trajectory_follower/controller_node_exe/debug/ld_outputs/data.1"> | ||
| <transform alias="Curvature_LD" name="Scale/Offset"> | ||
| <options value_scale="1.0" time_offset="0" value_offset="0"/> | ||
| </transform> | ||
| </curve> | ||
| <curve color="#1ac938" name="/control/trajectory_follower/controller_node_exe/debug/ld_outputs/data.2"> | ||
| <transform alias="Lateral_Error_LD" name="Scale/Offset"> | ||
| <options value_scale="1.0" time_offset="0" value_offset="0"/> | ||
| </transform> | ||
| </curve> | ||
| <curve color="#ff7f0e" name="/control/trajectory_follower/controller_node_exe/debug/ld_outputs/data.3"> | ||
| <transform alias="Total_LD" name="Scale/Offset"> | ||
| <options value_scale="1.0" time_offset="0" value_offset="0"/> | ||
| </transform> | ||
| </curve> | ||
| </plot> | ||
| </DockArea> | ||
| <DockSplitter sizes="0.333538;0.332925;0.333538" orientation="|" count="3"> | ||
| <DockArea name="Velocity"> | ||
| <plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
| <range right="80.040302" top="0.100000" bottom="-0.100000" left="0.000000"/> | ||
| <limitY/> | ||
| <curve color="#f14cc1" name="/control/trajectory_follower/controller_node_exe/debug/ld_outputs/data.6"> | ||
| <transform alias="Velocity" name="Scale/Offset"> | ||
| <options value_scale="1.0" time_offset="0" value_offset="0"/> | ||
| </transform> | ||
| </curve> | ||
| </plot> | ||
| </DockArea> | ||
| <DockArea name="Lateral Error"> | ||
| <plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
| <range right="80.040302" top="0.537336" bottom="0.537336" left="0.000000"/> | ||
| <limitY/> | ||
| <curve color="#9467bd" name="/control/trajectory_follower/controller_node_exe/debug/ld_outputs/data.5"> | ||
| <transform alias="Lateral_Error" name="Scale/Offset"> | ||
| <options value_scale="1.0" time_offset="0" value_offset="0"/> | ||
| </transform> | ||
| </curve> | ||
| </plot> | ||
| </DockArea> | ||
| <DockArea name="Curvature"> | ||
| <plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
| <range right="80.040302" top="0.000473" bottom="0.000426" left="0.000000"/> | ||
| <limitY/> | ||
| <curve color="#17becf" name="/control/trajectory_follower/controller_node_exe/debug/ld_outputs/data.4"> | ||
| <transform alias="Curvature" name="Scale/Offset"> | ||
| <options value_scale="1.0" time_offset="0" value_offset="0"/> | ||
| </transform> | ||
| </curve> | ||
| </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="" 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="true"/> | ||
| <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="UDP Server"/> | ||
| <plugin ID="WebSocket Server"/> | ||
| <plugin ID="ZMQ Subscriber"/> | ||
| <plugin ID="Fast Fourier Transform"/> | ||
| <plugin ID="Quaternion to RPY"/> | ||
| <plugin ID="Reactive Script Editor"> | ||
| <library code="--[[ Helper function to create a series from arrays

 new_series: a series previously created with ScatterXY.new(name)
 prefix: prefix of the timeseries, before the index of the array
 suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.
 suffix_Y: suffix to complete the name of the series containing the Y value
 timestamp: usually the tracker_time variable
 
 Example:
 
 Assuming we have multiple series in the form:
 
 /trajectory/node.{X}/position/x
 /trajectory/node.{X}/position/y
 
 where {N} is the index of the array (integer). We can create a reactive series from the array with:
 
 new_series = ScatterXY.new("my_trajectory") 
 CreateSeriesFromArray( new_series, "/trajectory/node", "position/x", "position/y", tracker_time );
--]]

function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )
 
 --- clear previous values
 new_series:clear()
 
 --- Append points to new_series
 index = 0
 while(true) do

 x = index;
 -- if not nil, get the X coordinate from a series
 if suffix_X ~= nil then 
 series_x = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_X) )
 if series_x == nil then break end
 x = series_x:atTime(timestamp)	 
 end
 
 series_y = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_Y) )
 if series_y == nil then break end 
 y = series_y:atTime(timestamp)
 
 new_series:push_back(x,y)
 index = index+1
 end
end

--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]

function GetSeriesNamesByPrefix(prefix)
 -- GetSeriesNames(9 is a built-in function
 all_names = GetSeriesNames()
 filtered_names = {}
 for i, name in ipairs(all_names) do
 -- check the prefix
 if name:find(prefix, 1, #prefix) then
 table.insert(filtered_names, name);
 end
 end
 return filtered_names
end

--[[ Modify an existing series, applying offsets to all their X and Y values

 series: an existing timeseries, obtained with TimeseriesView.find(name)
 delta_x: offset to apply to each x value
 delta_y: offset to apply to each y value 
 
--]]

function ApplyOffsetInPlace(series, delta_x, delta_y)
 -- use C++ indeces, not Lua indeces
 for index=0, series:size()-1 do
 x,y = series:at(index)
 series:set(index, x + delta_x, y + delta_y)
 end
end
"/> | ||
| <scripts/> | ||
| </plugin> | ||
| <plugin ID="CSV Exporter"/> | ||
| <plugin ID="ROS2 Topic Re-Publisher"/> | ||
| </Plugins> | ||
| <!-- - - - - - - - - - - - - - - --> | ||
| <previouslyLoaded_Datafiles/> | ||
| <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/> | ||
| <!-- - - - - - - - - - - - - - - --> | ||
| <customMathEquations/> | ||
| <snippets/> | ||
| <!-- - - - - - - - - - - - - - - --> | ||
| </root> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.