feat(planning_validator): add planning validator package

Signed-off-by: Takamasa Horibe <[email protected]>
autowarefoundation · Jan 2, 2023 · 759fe1f · 759fe1f
1 parent 64ec4cf
commit 759fe1f
Show file tree

Hide file tree

Showing 22 changed files with 1,868 additions and 8 deletions.
diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt
@@ -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
+)
diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md
@@ -0,0 +1 @@
+# Planning Validator
diff --git a/planning/planning_validator/config/planning_validator.param.yaml b/planning/planning_validator/config/planning_validator.param.yaml
@@ -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
diff --git a/planning/planning_validator/config/planning_validator_plotjugler_config.xml b/planning/planning_validator/config/planning_validator_plotjugler_config.xml
@@ -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&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#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. If [nil], use the index of the array.&#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.{X}/position/x&#xa;   /trajectory/node.{X}/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;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#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_y == 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;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#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>
+
diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/planning_validator/debug_marker.hpp
@@ -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_
diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp
@@ -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_