feat: add planning debug tools (autowarefoundation#1362)

* add planning_debug_tools and trajectory_analyzer implementation

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* change output topic name

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add missing depend

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* ci(pre-commit): autofix

* update

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move smoother tools to debug_tools

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/motion_velocity_smoother/CMakeLists.txt

@@ -61,9 +61,3 @@ ament_auto_package(
     launch
     config
 )
-
-install(PROGRAMS
-  scripts/trajectory_visualizer.py
-  scripts/closest_velocity_checker.py
-  DESTINATION lib/${PROJECT_NAME}
-)

planning/planning_debug_tools/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.5)
+project(planning_debug_tools)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(
+  planning_debug_tools
+  "msg/TrajectoryDebugInfo.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+ament_auto_add_library(trajectory_analyzer_node SHARED
+  src/trajectory_analyzer.cpp
+)
+
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(trajectory_analyzer_node
+    planning_debug_tools "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target planning_debug_tools "rosidl_typesupport_cpp")
+    target_link_libraries(trajectory_analyzer_node "${cpp_typesupport_target}")
+endif()
+
+
+rclcpp_components_register_node(trajectory_analyzer_node
+  PLUGIN "planning_debug_tools::TrajectoryAnalyzerNode"
+  EXECUTABLE trajectory_analyzer_exe
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+)
+
+install(PROGRAMS
+  scripts/trajectory_visualizer.py
+  scripts/closest_velocity_checker.py
+  DESTINATION lib/${PROJECT_NAME}
+)

planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp

@@ -0,0 +1,121 @@
+// 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_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_
+#define PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_
+
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "planning_debug_tools/msg/trajectory_debug_info.hpp"
+#include "planning_debug_tools/util.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp"
+
+#include "autoware_auto_planning_msgs/msg/path.hpp"
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "tier4_debug_msgs/msg/float64_multi_array_stamped.hpp"
+
+#include <iostream>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace planning_debug_tools
+{
+using autoware_auto_planning_msgs::msg::Path;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using nav_msgs::msg::Odometry;
+using planning_debug_tools::msg::TrajectoryDebugInfo;
+
+template <typename T>
+class TrajectoryAnalyzer
+{
+  using SubscriberType = typename rclcpp::Subscription<T>::SharedPtr;
+  using PublisherType = rclcpp::Publisher<TrajectoryDebugInfo>::SharedPtr;
+  using T_ConstSharedPtr = typename T::ConstSharedPtr;
+
+public:
+  TrajectoryAnalyzer(rclcpp::Node * node, const std::string & sub_name)
+  : node_(node), name_(sub_name)
+  {
+    const auto pub_name = sub_name + "/debug_info";
+    pub_ = node->create_publisher<TrajectoryDebugInfo>(pub_name, 1);
+    sub_ = node->create_subscription<T>(
+      sub_name, 1, [this](const T_ConstSharedPtr msg) { run(msg->points); });
+  }
+  ~TrajectoryAnalyzer() = default;
+
+  void setKinematics(const Odometry::ConstSharedPtr input) { ego_kinematics_ = input; }
+
+  // Note: the lambda used in the subscriber captures "this", so any operations that change the
+  // address of "this" are prohibited.
+  TrajectoryAnalyzer(const TrajectoryAnalyzer &) = delete;                      // copy
+  TrajectoryAnalyzer(TrajectoryAnalyzer &&) = delete;                           // move
+  auto operator=(const TrajectoryAnalyzer &) -> TrajectoryAnalyzer & = delete;  // copy assignment
+  auto operator=(TrajectoryAnalyzer &&) -> TrajectoryAnalyzer & = delete;       // move assignment
+
+public:
+  std::shared_ptr<rclcpp::Node> node_;
+  std::string name_;
+  PublisherType pub_;
+  SubscriberType sub_;
+  Odometry::ConstSharedPtr ego_kinematics_;
+
+  template <typename P>
+  void run(const P & points)
+  {
+    if (!ego_kinematics_) return;
+    if (points.size() < 3) return;
+
+    const auto & ego_p = ego_kinematics_->pose.pose.position;
+
+    TrajectoryDebugInfo data;
+    data.stamp = node_->now();
+    data.size = points.size();
+    data.curvature = calcCurvature(points);
+    const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p);
+    data.arclength = calcPathArcLengthArray(points, -arclength_offset);
+    data.velocity = getVelocityArray(points);
+
+    if (data.size != data.arclength.size() || data.size != data.velocity.size()) {
+      RCLCPP_ERROR(node_->get_logger(), "computation failed.");
+      return;
+    }
+
+    pub_->publish(data);
+  }
+};
+
+class TrajectoryAnalyzerNode : public rclcpp::Node
+{
+public:
+  explicit TrajectoryAnalyzerNode(const rclcpp::NodeOptions & options);
+  ~TrajectoryAnalyzerNode() = default;
+
+private:
+  rclcpp::Subscription<Odometry>::SharedPtr sub_ego_kinematics_;
+  void onEgoKinematics(const Odometry::ConstSharedPtr msg);
+
+  std::vector<std::shared_ptr<TrajectoryAnalyzer<Path>>> path_analyzers_;
+  std::vector<std::shared_ptr<TrajectoryAnalyzer<PathWithLaneId>>> path_wlid_analyzers_;
+  std::vector<std::shared_ptr<TrajectoryAnalyzer<Trajectory>>> trajectory_analyzers_;
+};
+
+}  // namespace planning_debug_tools
+
+#endif  // PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_

planning/planning_debug_tools/include/planning_debug_tools/util.hpp

@@ -0,0 +1,87 @@
+// 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_DEBUG_TOOLS__UTIL_HPP_
+#define PLANNING_DEBUG_TOOLS__UTIL_HPP_
+
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp"
+
+#include "autoware_auto_planning_msgs/msg/path.hpp"
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+
+#include <vector>
+
+namespace planning_debug_tools
+{
+
+using tier4_autoware_utils::calcDistance2d;
+using tier4_autoware_utils::getPoint;
+
+double getVelocity(const autoware_auto_planning_msgs::msg::PathPoint & p)
+{
+  return p.longitudinal_velocity_mps;
+}
+double getVelocity(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+{
+  return p.point.longitudinal_velocity_mps;
+}
+double getVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p)
+{
+  return p.longitudinal_velocity_mps;
+}
+template <class T>
+inline std::vector<double> getVelocityArray(const T & points)
+{
+  std::vector<double> v_arr;
+  for (const auto & p : points) {
+    v_arr.push_back(getVelocity(p));
+  }
+  return v_arr;
+}
+
+template <typename T>
+std::vector<double> calcPathArcLengthArray(const T & points, const double offset)
+{
+  std::vector<double> out;
+  out.push_back(offset);
+  double sum = offset;
+  for (size_t i = 1; i < points.size(); ++i) {
+    sum += calcDistance2d(getPoint(points.at(i)), getPoint(points.at(i - 1)));
+    out.push_back(sum);
+  }
+  return out;
+}
+
+template <class T>
+inline std::vector<double> calcCurvature(const T & points)
+{
+  std::vector<double> curvature_arr;
+  curvature_arr.push_back(0.0);
+  for (size_t i = 1; i < points.size() - 1; ++i) {
+    const auto p1 = getPoint(points.at(i - 1));
+    const auto p2 = getPoint(points.at(i));
+    const auto p3 = getPoint(points.at(i + 1));
+    curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3));
+  }
+  curvature_arr.push_back(0.0);
+  return curvature_arr;
+}
+
+}  // namespace planning_debug_tools
+
+#endif  // PLANNING_DEBUG_TOOLS__UTIL_HPP_

planning/planning_debug_tools/launch/trajectory_analyzer.launch.xml

@@ -0,0 +1,19 @@
+<launch>
+  <node pkg="planning_debug_tools" exec="trajectory_analyzer_exe" name="trajectory_analyzer" output="screen">
+    <param name="path_topics" value="[/planning/scenario_planning/lane_driving/behavior_planning/path]"/>
+    <param name="path_wlid_topics" value="[/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id]"/>
+    <param
+      name="trajectory_topics"
+      value="[/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory,
+              /planning/scenario_planning/motion_velocity_smoother/debug/backward_filtered_trajectory,
+              /planning/scenario_planning/motion_velocity_smoother/debug/forward_filtered_trajectory,
+              /planning/scenario_planning/motion_velocity_smoother/debug/merged_filtered_trajectory,
+              /planning/scenario_planning/motion_velocity_smoother/debug/trajectory_external_velocity_limited,
+              /planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered,
+              /planning/scenario_planning/motion_velocity_smoother/debug/trajectory_raw,
+              /planning/scenario_planning/motion_velocity_smoother/debug/trajectory_time_resampled,
+              /planning/scenario_planning/trajectory]"
+    />
+    <remap from="ego_kinematics" to="/localization/kinematic_state"/>
+  </node>
+</launch>

planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg

@@ -0,0 +1,5 @@
+builtin_interfaces/Time stamp
+uint32 size
+float64[] arclength
+float64[] curvature
+float64[] velocity

planning/planning_debug_tools/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>planning_debug_tools</name>
+  <version>0.1.0</version>
+  <description>The planning_debug_tools package</description>
+  <maintainer email="horibe.takamasa@gmail.com">Takamasa Horibe</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>motion_utils</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

planning/planning_debug_tools/src/trajectory_analyzer.cpp

@@ -0,0 +1,62 @@
+// 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_debug_tools/trajectory_analyzer.hpp"
+
+namespace planning_debug_tools
+{
+TrajectoryAnalyzerNode::TrajectoryAnalyzerNode(const rclcpp::NodeOptions & options)
+: Node("trajectory_analyzer", options)
+{
+  using TopicNames = std::vector<std::string>;
+  const auto path_topics = declare_parameter<TopicNames>("path_topics", TopicNames{});
+  const auto path_wlid_topics = declare_parameter<TopicNames>("path_wlid_topics", TopicNames{});
+  const auto trajectory_topics = declare_parameter<TopicNames>("trajectory_topics", TopicNames{});
+
+  for (const auto & s : path_topics) {
+    path_analyzers_.push_back(std::make_shared<TrajectoryAnalyzer<Path>>(this, s));
+    RCLCPP_INFO(get_logger(), "path_topics: %s", s.c_str());
+  }
+  for (const auto & s : path_wlid_topics) {
+    path_wlid_analyzers_.push_back(std::make_shared<TrajectoryAnalyzer<PathWithLaneId>>(this, s));
+    RCLCPP_INFO(get_logger(), "path_wlid_topics: %s", s.c_str());
+  }
+
+  for (const auto & s : trajectory_topics) {
+    trajectory_analyzers_.push_back(std::make_shared<TrajectoryAnalyzer<Trajectory>>(this, s));
+    RCLCPP_INFO(get_logger(), "trajectory_topics: %s", s.c_str());
+  }
+
+  using std::placeholders::_1;
+  sub_ego_kinematics_ = create_subscription<Odometry>(
+    "ego_kinematics", 1, std::bind(&TrajectoryAnalyzerNode::onEgoKinematics, this, _1));
+}
+
+void TrajectoryAnalyzerNode::onEgoKinematics(const Odometry::ConstSharedPtr msg)
+{
+  for (auto & a : path_analyzers_) {
+    a->setKinematics(msg);
+  }
+  for (auto & a : path_wlid_analyzers_) {
+    a->setKinematics(msg);
+  }
+  for (auto & a : trajectory_analyzers_) {
+    a->setKinematics(msg);
+  }
+}
+
+}  // namespace planning_debug_tools
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(planning_debug_tools::TrajectoryAnalyzerNode)