tmp

Signed-off-by: Takamasa Horibe <[email protected]>

planning/planning_validator/CMakeLists.txt

@@ -37,6 +37,19 @@ else()
 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(

planning/planning_validator/config/planning_validator.param.yaml

@@ -1,5 +1,7 @@
 /**:
   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.
     interval_threshold: 100.0
     relative_angle_threshold: 10.0
     curvature_threshold: 100.0

planning/planning_validator/include/planning_validator/planning_validator.hpp

@@ -59,15 +59,6 @@ class PlanningValidator : public rclcpp::Node
 
   void onTrajectory(const Trajectory::ConstSharedPtr msg);
 
-private:
-  void setupDiag();
-  void setDiagStatus(
-    const std::string & name, const bool ok_condition, const std::string & error_msg);
-  void setupParameters();
-
-  bool isDataReady();
-
-  void validate(const Trajectory & trajectory);
   bool checkValidFiniteValue(const Trajectory & trajectory);
   bool checkValidInterval(const Trajectory & trajectory);
   bool checkValidRelativeAngle(const Trajectory & trajectory);
@@ -80,6 +71,16 @@ class PlanningValidator : public rclcpp::Node
   bool checkValidVelocityDeviation(const Trajectory & trajectory);
   bool checkValidDistanceDeviation(const Trajectory & trajectory);
 
+private:
+  void setupDiag();
+  void checkDiagFlag(
+   DiagnosticStatusWrapper & stat,  const std::string & name, const bool ok_condition, const std::string & error_msg);
+  void setupParameters();
+
+  bool isDataReady();
+
+  void validate(const Trajectory & trajectory);
+
   void publishTrajectory();
   void publishDebugInfo();
 
@@ -88,6 +89,10 @@ class PlanningValidator : public rclcpp::Node
   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;
+
   Updater diag_updater_{this};
 
   PlanningValidatorStatus validation_status_;

planning/planning_validator/launch/planning_validator.launch.xml

@@ -6,8 +6,8 @@
     <param from="$(var planning_validator_param_path)"/>
 
     <!-- remap topic name -->
-    <remap from="~/input/trajectory" to="/planning/scenario_planning/trajectory"/>
+    <!-- <remap from="~/input/trajectory" to="/planning/scenario_planning/trajectory"/>
     <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
-    <remap from="~/output/trajectory" to="/planning/trajectory"/>
+    <remap from="~/output/trajectory" to="/planning/trajectory"/> -->
   </node>
 </launch>

planning/planning_validator/package.xml

@@ -5,9 +5,11 @@
   <version>0.1.0</version>
   <description>ros node for planning_validator</description>
   <maintainer email="[email protected]">Takamasa Horibe</maintainer>
+  <maintainer email="[email protected]">Yutaka Shimizu</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Takamasa Horibe</author>
+  <author email="[email protected]">Yutaka Shimizu</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <build_depend>autoware_cmake</build_depend>

planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp

@@ -0,0 +1,78 @@
+// Copyright 2021 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.
+
+#include "planning_error_monitor/invalid_trajectory_publisher.hpp"
+
+#include <memory>
+#include <string>
+#include <utility>
+
+namespace planning_diagnostics
+{
+InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode(
+  const rclcpp::NodeOptions & node_options)
+: Node("invalid_trajectory_publisher", node_options)
+{
+  using std::placeholders::_1;
+  using std::chrono_literals::operator""ms;
+
+  traj_sub_ = create_subscription<Trajectory>(
+    "~/input/trajectory", 1,
+    std::bind(&InvalidTrajectoryPublisherNode::onCurrentTrajectory, this, _1));
+
+  traj_pub_ = create_publisher<Trajectory>("~/output/trajectory", 1);
+
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), 100ms, std::bind(&InvalidTrajectoryPublisherNode::onTimer, this));
+}
+
+void InvalidTrajectoryPublisherNode::onTimer()
+{
+  if (!current_trajectory_) {
+    RCLCPP_INFO(this->get_logger(), "waiting trajectory");
+    return;
+  }
+  if (current_trajectory_->points.empty()) {
+    RCLCPP_INFO(this->get_logger(), "waiting trajectory");
+    return;
+  }
+
+  constexpr auto ADDED_VALUE = 1.0e3;
+
+  auto output = *current_trajectory_;
+  auto & p = output.points.back().pose.position;
+  p.x += ADDED_VALUE;
+  p.y += ADDED_VALUE;
+  p.z += ADDED_VALUE;
+
+  traj_pub_->publish(output);
+
+  RCLCPP_INFO(this->get_logger(), "invalid trajectory is published.");
+
+  bool EXIT_AFTER_PUBLISH = false;
+  if (EXIT_AFTER_PUBLISH) {
+    exit(0);
+  }
+}
+
+void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg)
+{
+  current_trajectory_ = msg;
+  traj_sub_.reset();
+}
+
+}  // namespace planning_diagnostics
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(planning_diagnostics::InvalidTrajectoryPublisherNode)

planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp

@@ -0,0 +1,46 @@
+// Copyright 2021 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_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_
+#define PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+
+#include <string>
+
+namespace planning_diagnostics
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+
+class InvalidTrajectoryPublisherNode : public rclcpp::Node
+{
+public:
+  explicit InvalidTrajectoryPublisherNode(const rclcpp::NodeOptions & node_options);
+  void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg);
+  void onTimer();
+
+private:
+  // ROS
+  rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
+  rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  Trajectory::ConstSharedPtr current_trajectory_ = nullptr;
+};
+}  // namespace planning_diagnostics
+
+#endif  // PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_

planning/planning_validator/src/planning_validator.cpp

@@ -44,6 +44,9 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options)
   debugger_ = std::make_shared<PlanningValidatorDebugPosePublisher>(this);
 
   setupParameters();
+std::cerr << __LINE__ << std::endl;
+  setupDiag();
+std::cerr << __LINE__ << std::endl;
 }
 
 void PlanningValidator::setupParameters()
@@ -56,17 +59,24 @@ void PlanningValidator::setupParameters()
       std::exit(1);
     }
   };
-  auto & p = validation_params_;
-  p.interval_threshold = dp("interval_threshold");
-  p.relative_angle_threshold = dp("relative_angle_threshold");
-  p.curvature_threshold = dp("curvature_threshold");
-  p.lateral_acc_threshold = dp("lateral_acc_threshold");
-  p.longitudinal_max_acc_threshold = dp("longitudinal_max_acc_threshold");
-  p.longitudinal_min_acc_threshold = dp("longitudinal_min_acc_threshold");
-  p.steering_threshold = dp("steering_threshold");
-  p.steering_rate_threshold = dp("steering_rate_threshold");
-  p.velocity_deviation_threshold = dp("velocity_deviation_threshold");
-  p.distance_deviation_threshold = dp("distance_deviation_threshold");
+
+  use_previous_trajectory_on_invalid_ =
+    declare_parameter<bool>("use_previous_trajectory_on_invalid");
+  publish_diag_ = declare_parameter<bool>("publish_diag");
+
+  {
+    auto & p = validation_params_;
+    p.interval_threshold = dp("interval_threshold");
+    p.relative_angle_threshold = dp("relative_angle_threshold");
+    p.curvature_threshold = dp("curvature_threshold");
+    p.lateral_acc_threshold = dp("lateral_acc_threshold");
+    p.longitudinal_max_acc_threshold = dp("longitudinal_max_acc_threshold");
+    p.longitudinal_min_acc_threshold = dp("longitudinal_min_acc_threshold");
+    p.steering_threshold = dp("steering_threshold");
+    p.steering_rate_threshold = dp("steering_rate_threshold");
+    p.velocity_deviation_threshold = dp("velocity_deviation_threshold");
+    p.distance_deviation_threshold = dp("distance_deviation_threshold");
+  }
 
   try {
     vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -78,33 +88,56 @@ void PlanningValidator::setupParameters()
 
 void PlanningValidator::setupDiag()
 {
-  diag_updater_.setHardwareID("planning_validator");
+  const auto checkFlag =
+    [](const bool & is_ok, DiagnosticStatusWrapper & stat, const std::string & msg) {
+      is_ok ? stat.summary(DiagnosticStatus::OK, "ok") : stat.summary(DiagnosticStatus::ERROR, msg);
+    };
 
   const auto & s = validation_status_;
-  setDiagStatus("finite", s.is_valid_finite_value, "infinite value is found");
-  setDiagStatus("interval", s.is_valid_interval, "points interval is too long");
-  setDiagStatus("relative_angle", s.is_valid_relative_angle, "relative angle is too large");
-  setDiagStatus("curvature", s.is_valid_curvature, "curvature is too large");
-  setDiagStatus(
-    "lateral_acceleration", s.is_valid_lateral_acc, "lateral acceleration is too large");
-  setDiagStatus("acceleration", s.is_valid_longitudinal_max_acc, "acceleration is too large");
-  setDiagStatus("deceleration", s.is_valid_longitudinal_min_acc, "deceleration is too large");
-  setDiagStatus("steering", s.is_valid_steering, "expected steering is too large");
-  setDiagStatus("steering_rate", s.is_valid_steering_rate, "expected steering rate is too large");
-  setDiagStatus(
-    "velocity_deviation", s.is_valid_velocity_deviation,
-    "velocity deviation between planning and actual is too large");
+  auto & d = diag_updater_;
+  d.setHardwareID("planning_validator");
+
+  std::string ns = "trajectory_validation_";
+  d.add(ns + "finite", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_finite_value, stat, "infinite value is found");
+  });
+  d.add(ns + "interval", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_interval, stat, "points interval is too long");
+  });
+  d.add(ns + "relative_angle", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_relative_angle, stat, "relative angle is too large");
+  });
+  d.add(ns + "curvature", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_curvature, stat, "curvature is too large");
+  });
+  d.add(ns + "lateral_acceleration", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_lateral_acc, stat, "lateral acceleration is too large");
+  });
+  d.add(ns + "acceleration", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_longitudinal_max_acc, stat, "acceleration is too large");
+  });
+  d.add(ns + "deceleration", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_longitudinal_min_acc, stat, "deceleration is too large");
+  });
+  d.add(ns + "steering", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_steering, stat, "expected steering is too large");
+  });
+  d.add(ns + "steering_rate", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_steering_rate, stat, "expected steering rate is too large");
+  });
+  d.add(ns + "velocity_deviation", [this, checkFlag, &s](auto & stat) {
+    checkFlag(s.is_valid_velocity_deviation, stat, "velocity deviation is too large");
+  });
 }
 
-void PlanningValidator::setDiagStatus(
-  const std::string & name, const bool is_ok, const std::string & error_msg)
-{
-  diag_updater_.add(
-    "trajectory_validation_" + name, [this, is_ok, &error_msg](DiagnosticStatusWrapper & stat) {
-      is_ok ? stat.summary(DiagnosticStatus::OK, "ok")
-            : stat.summary(DiagnosticStatus::ERROR, error_msg);
-    });
-}
+void PlanningValidator::checkDiagFlag(
+  DiagnosticStatusWrapper & stat, const std::string & name, const bool ok_condition, const std::string & error_msg)
+  {
+    [](const bool & is_ok, DiagnosticStatusWrapper & stat, const std::string & msg) {
+      is_ok ? stat.summary(DiagnosticStatus::OK, "ok") : stat.summary(DiagnosticStatus::ERROR, msg);
+    };
+
+  }
 
 bool PlanningValidator::isDataReady()
 {
@@ -204,6 +237,7 @@ bool PlanningValidator::checkValidInterval(const Trajectory & trajectory)
   const auto interval_distances = calcIntervalDistance(trajectory);
   const auto [max_interval_distance, i] = getAbsMaxValAndIdx(interval_distances);
   validation_status_.max_interval_distance = max_interval_distance;
+ std::cerr << "max_interval_distance = " << max_interval_distance << ", index = " << i << ", threshold = " << validation_params_.interval_threshold << std::endl;
 
   if (max_interval_distance > validation_params_.interval_threshold) {
     // const auto &p = trajectory.points;

planning/planning_validator/test/src/test_main.cpp

@@ -0,0 +1,26 @@
+// Copyright 2021 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.
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  bool result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}