-
Notifications
You must be signed in to change notification settings - Fork 639
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(planning_validator): add planning validator package
Signed-off-by: Takamasa Horibe <[email protected]>
- Loading branch information
1 parent
64ec4cf
commit 759fe1f
Showing
22 changed files
with
1,868 additions
and
8 deletions.
There are no files selected for viewing
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,68 @@ | ||
cmake_minimum_required(VERSION 3.14) | ||
project(planning_validator) | ||
|
||
find_package(autoware_cmake REQUIRED) | ||
autoware_package() | ||
|
||
ament_auto_add_library(planning_validator_helpers SHARED | ||
src/utils.cpp | ||
src/debug_marker.cpp | ||
) | ||
|
||
# planning validator | ||
ament_auto_add_library(planning_validator_component SHARED | ||
include/planning_validator/planning_validator.hpp | ||
src/planning_validator.cpp | ||
) | ||
target_link_libraries(planning_validator_component planning_validator_helpers) | ||
rclcpp_components_register_node(planning_validator_component | ||
PLUGIN "planning_validator::PlanningValidator" | ||
EXECUTABLE planning_validator_node | ||
) | ||
|
||
# invalid trajectory publisher (for debug) | ||
ament_auto_add_library(invalid_trajectory_publisher_node SHARED | ||
src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp | ||
) | ||
rclcpp_components_register_node(invalid_trajectory_publisher_node | ||
PLUGIN "planning_validator::InvalidTrajectoryPublisherNode" | ||
EXECUTABLE invalid_trajectory_publisher | ||
) | ||
|
||
rosidl_generate_interfaces( | ||
${PROJECT_NAME} | ||
"msg/PlanningValidatorStatus.msg" | ||
DEPENDENCIES builtin_interfaces | ||
) | ||
|
||
# to use a message defined in the same package | ||
if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) | ||
rosidl_target_interfaces(planning_validator_component | ||
${PROJECT_NAME} "rosidl_typesupport_cpp") | ||
else() | ||
rosidl_get_typesupport_target( | ||
cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") | ||
target_link_libraries(planning_validator_component "${cpp_typesupport_target}") | ||
endif() | ||
|
||
if(BUILD_TESTING) | ||
ament_add_ros_isolated_gtest(test_planning_validator | ||
test/src/test_main.cpp | ||
test/src/test_planning_validator_functions.cpp | ||
test/src/test_planning_validator_helper.cpp | ||
test/src/test_planning_validator_pubsub.cpp | ||
) | ||
ament_target_dependencies(test_planning_validator | ||
rclcpp | ||
autoware_auto_planning_msgs | ||
) | ||
target_link_libraries(test_planning_validator | ||
planning_validator_component | ||
) | ||
endif() | ||
|
||
ament_auto_package( | ||
INSTALL_TO_SHARE | ||
config | ||
launch | ||
) |
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 @@ | ||
# Planning Validator |
16 changes: 16 additions & 0 deletions
16
planning/planning_validator/config/planning_validator.param.yaml
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,16 @@ | ||
/**: | ||
ros__parameters: | ||
publish_diag: true # if true, diagnostic msg is published | ||
use_previous_trajectory_on_invalid: true # if true, invalid trajectory is not published, previous valid trajectory is published instead. | ||
display_on_terminal: true # show error msg on terminal | ||
thresholds: | ||
interval: 100.0 | ||
relative_angle: 2.0 # (= 115 degree) | ||
curvature: 1.0 | ||
lateral_acc: 9.8 | ||
longitudinal_max_acc: 9.8 | ||
longitudinal_min_acc: -9.8 | ||
steering: 1.414 | ||
steering_rate: 10.0 | ||
velocity_deviation: 100.0 | ||
distance_deviation: 100.0 |
135 changes: 135 additions & 0 deletions
135
planning/planning_validator/config/planning_validator_plotjugler_config.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,135 @@ | ||
<?xml version='1.0' encoding='UTF-8'?> | ||
<root> | ||
<tabbed_widget name="Main Window" parent="main_window"> | ||
<Tab tab_name="tab1" containers="1"> | ||
<Container> | ||
<DockSplitter sizes="0.214447;0.211061;0.205418;0.369074" orientation="-" count="4"> | ||
<DockSplitter sizes="0.5;0.5" orientation="|" count="2"> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-0.049151" right="209.062739" top="2.183613"/> | ||
<limitY/> | ||
<curve color="#1f77b4" name="/planning_validator/output/validation_status/distance_deviation"/> | ||
</plot> | ||
</DockArea> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="0.074758" right="209.062739" top="1.134902"/> | ||
<limitY/> | ||
<curve color="#f14cc1" name="/planning_validator/output/validation_status/max_longitudinal_acc"/> | ||
</plot> | ||
</DockArea> | ||
</DockSplitter> | ||
<DockSplitter sizes="0.5;0.5" orientation="|" count="2"> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-0.044741" right="209.062739" top="1.864607"/> | ||
<limitY/> | ||
<curve color="#d62728" name="/planning_validator/output/validation_status/max_curvature"/> | ||
</plot> | ||
</DockArea> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-0.077270" right="209.062739" top="3.217721"/> | ||
<limitY/> | ||
<curve color="#9467bd" name="/planning_validator/output/validation_status/max_relative_angle"/> | ||
</plot> | ||
</DockArea> | ||
</DockSplitter> | ||
<DockSplitter sizes="0.5;0.5" orientation="|" count="2"> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="0.066734" right="209.062739" top="1.463922"/> | ||
<limitY/> | ||
<curve color="#1ac938" name="/planning_validator/output/validation_status/max_interval_distance"/> | ||
</plot> | ||
</DockArea> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-0.032905" right="209.062739" top="1.470008"/> | ||
<limitY/> | ||
<curve color="#17becf" name="/planning_validator/output/validation_status/max_steering"/> | ||
</plot> | ||
</DockArea> | ||
</DockSplitter> | ||
<DockSplitter sizes="0.5;0.5" orientation="|" count="2"> | ||
<DockSplitter sizes="0.5;0.5" orientation="-" count="2"> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-3.617326" right="209.062739" top="148.400163"/> | ||
<limitY/> | ||
<curve color="#ff7f0e" name="/planning_validator/output/validation_status/max_lateral_acc"/> | ||
</plot> | ||
</DockArea> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-0.106153" right="209.062739" top="4.352255"/> | ||
<limitY/> | ||
<curve color="#1f77b4" name="/planning_validator/output/validation_status/velocity_deviation"/> | ||
</plot> | ||
</DockArea> | ||
</DockSplitter> | ||
<DockSplitter sizes="0.5;0.5" orientation="-" count="2"> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-0.120561" right="209.062739" top="4.972480"/> | ||
<limitY/> | ||
<curve color="#bcbd22" name="/planning_validator/output/validation_status/max_steering_rate"/> | ||
</plot> | ||
</DockArea> | ||
<DockArea name="..."> | ||
<plot flip_y="false" mode="TimeSeries" flip_x="false" style="Lines"> | ||
<range left="116.478482" bottom="-1.137402" right="209.062739" top="0.027742"/> | ||
<limitY/> | ||
<curve color="#d62728" name="/planning_validator/output/validation_status/min_longitudinal_acc"/> | ||
</plot> | ||
</DockArea> | ||
</DockSplitter> | ||
</DockSplitter> | ||
</DockSplitter> | ||
</Container> | ||
</Tab> | ||
<currentTabIndex index="0"/> | ||
</tabbed_widget> | ||
<use_relative_time_offset enabled="1"/> | ||
<!-- - - - - - - - - - - - - - - --> | ||
<!-- - - - - - - - - - - - - - - --> | ||
<Plugins> | ||
<plugin ID="DataLoad CSV"> | ||
<default delimiter="0" 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="true"/> | ||
<max_array_size value="true"/> | ||
<selected_topics> | ||
<topic name="/planning_validator/output/validation_status"/> | ||
</selected_topics> | ||
</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> | ||
|
58 changes: 58 additions & 0 deletions
58
planning/planning_validator/include/planning_validator/debug_marker.hpp
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,58 @@ | ||
// Copyright 2022 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 PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ | ||
#define PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <autoware_auto_planning_msgs/msg/trajectory.hpp> | ||
#include <visualization_msgs/msg/marker.hpp> | ||
#include <visualization_msgs/msg/marker_array.hpp> | ||
|
||
#include <map> | ||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
class PlanningValidatorDebugPosePublisher | ||
{ | ||
public: | ||
explicit PlanningValidatorDebugPosePublisher(rclcpp::Node * node); | ||
|
||
void pushPoseMarker( | ||
const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, | ||
int id = 0); | ||
void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); | ||
void clearPoseMarker(const std::string & ns); | ||
void publish(); | ||
|
||
private: | ||
rclcpp::Node * node_; | ||
visualization_msgs::msg::MarkerArray marker_array_; | ||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_; | ||
std::map<std::string, int> marker_id_; | ||
|
||
int getMarkerId(const std::string & ns) | ||
{ | ||
if (marker_id_.count(ns) == 0) { | ||
marker_id_[ns] = 0.0; | ||
} | ||
return marker_id_[ns]++; | ||
} | ||
|
||
void clearMarkerId(const std::string & ns) { marker_id_[ns] = 0; } | ||
}; | ||
|
||
#endif // PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ |
115 changes: 115 additions & 0 deletions
115
planning/planning_validator/include/planning_validator/planning_validator.hpp
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,115 @@ | ||
// Copyright 2022 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 PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ | ||
#define PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ | ||
|
||
#include "planning_validator/debug_marker.hpp" | ||
#include "planning_validator/msg/planning_validator_status.hpp" | ||
#include "vehicle_info_util/vehicle_info_util.hpp" | ||
|
||
#include <diagnostic_updater/diagnostic_updater.hpp> | ||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <autoware_auto_planning_msgs/msg/trajectory.hpp> | ||
#include <diagnostic_msgs/msg/diagnostic_array.hpp> | ||
#include <nav_msgs/msg/odometry.hpp> | ||
|
||
#include <memory> | ||
#include <string> | ||
|
||
namespace planning_validator | ||
{ | ||
using autoware_auto_planning_msgs::msg::Trajectory; | ||
using autoware_auto_planning_msgs::msg::TrajectoryPoint; | ||
using diagnostic_updater::DiagnosticStatusWrapper; | ||
using diagnostic_updater::Updater; | ||
using nav_msgs::msg::Odometry; | ||
using planning_validator::msg::PlanningValidatorStatus; | ||
|
||
struct ValidationParams | ||
{ | ||
double interval_threshold; | ||
double relative_angle_threshold; | ||
double curvature_threshold; | ||
double lateral_acc_threshold; | ||
double longitudinal_max_acc_threshold; | ||
double longitudinal_min_acc_threshold; | ||
double steering_threshold; | ||
double steering_rate_threshold; | ||
double velocity_deviation_threshold; | ||
double distance_deviation_threshold; | ||
}; | ||
|
||
class PlanningValidator : public rclcpp::Node | ||
{ | ||
public: | ||
explicit PlanningValidator(const rclcpp::NodeOptions & options); | ||
|
||
void onTrajectory(const Trajectory::ConstSharedPtr msg); | ||
|
||
bool checkValidFiniteValue(const Trajectory & trajectory); | ||
bool checkValidInterval(const Trajectory & trajectory); | ||
bool checkValidRelativeAngle(const Trajectory & trajectory); | ||
bool checkValidCurvature(const Trajectory & trajectory); | ||
bool checkValidLateralAcceleration(const Trajectory & trajectory); | ||
bool checkValidMaxLongitudinalAcceleration(const Trajectory & trajectory); | ||
bool checkValidMinLongitudinalAcceleration(const Trajectory & trajectory); | ||
bool checkValidSteering(const Trajectory & trajectory); | ||
bool checkValidSteeringRate(const Trajectory & trajectory); | ||
bool checkValidVelocityDeviation(const Trajectory & trajectory); | ||
bool checkValidDistanceDeviation(const Trajectory & trajectory); | ||
|
||
private: | ||
void setupDiag(); | ||
|
||
void setupParameters(); | ||
|
||
bool isDataReady(); | ||
|
||
void validate(const Trajectory & trajectory); | ||
|
||
void publishTrajectory(); | ||
void publishDebugInfo(); | ||
void displayStatus(); | ||
|
||
rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_; | ||
rclcpp::Subscription<Trajectory>::SharedPtr sub_traj_; | ||
rclcpp::Publisher<Trajectory>::SharedPtr pub_traj_; | ||
rclcpp::Publisher<PlanningValidatorStatus>::SharedPtr pub_status_; | ||
|
||
// system parameters | ||
bool use_previous_trajectory_on_invalid_ = true; | ||
bool publish_diag_ = true; | ||
bool display_on_terminal_ = true; | ||
|
||
Updater diag_updater_{this}; | ||
|
||
PlanningValidatorStatus validation_status_; | ||
ValidationParams validation_params_; // for thresholds | ||
|
||
vehicle_info_util::VehicleInfo vehicle_info_; | ||
|
||
bool isAllValid(const PlanningValidatorStatus & status); | ||
|
||
Trajectory::ConstSharedPtr current_trajectory_; | ||
Trajectory::ConstSharedPtr previous_published_trajectory_; | ||
|
||
Odometry::ConstSharedPtr current_kinematics_; | ||
|
||
std::shared_ptr<PlanningValidatorDebugPosePublisher> debug_pose_publisher_; | ||
}; | ||
} // namespace planning_validator | ||
|
||
#endif // PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ |
Oops, something went wrong.