feat(planning_debug_tools): add planning debug tools

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp

@@ -19,6 +19,8 @@
 
 #include <visualization_msgs/msg/marker_array.hpp>
 
+#include <boost/optional.hpp>
+
 #include <string>
 
 namespace tier4_autoware_utils
@@ -88,10 +90,15 @@ inline visualization_msgs::msg::Marker createDefaultMarker(
 
 inline void appendMarkerArray(
   const visualization_msgs::msg::MarkerArray & additional_marker_array,
-  visualization_msgs::msg::MarkerArray * marker_array)
+  visualization_msgs::msg::MarkerArray * marker_array,
+  const boost::optional<rclcpp::Time> & current_time = {})
 {
   for (const auto & marker : additional_marker_array.markers) {
     marker_array->markers.push_back(marker);
+
+    if (current_time) {
+      marker_array->markers.back().header.stamp = current_time.get();
+    }
   }
 }
 }  // namespace tier4_autoware_utils

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,54 @@
+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
+)
+
+ament_auto_add_library(stop_reason_visualizer_node SHARED
+  src/stop_reason_visualizer.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
+)
+
+rclcpp_components_register_node(stop_reason_visualizer_node
+  PLUGIN "planning_debug_tools::StopReasonVisualizerNode"
+  EXECUTABLE stop_reason_visualizer_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/Readme.md

@@ -0,0 +1,154 @@
+# Planning Debug Tools
+
+This package contains several planning-related debug tools.
+
+- **Trajectory analyzer**: visualizes the information (speed, curvature, yaw, etc) along the trajectory
+- **Closest velocity checker**: prints the velocity information indicated by each modules
+
+## Trajectory analyzer
+
+The `trajectory_analyzer` visualizes the information (speed, curvature, yaw, etc) along the trajectory. This feature would be helpful for purposes such as "_investigating the reason why the vehicle decelerates here_". This feature employs the OSS [PlotJuggler](https://www.plotjuggler.io/).
+
+![how this works](https://user-images.githubusercontent.com/21360593/179361367-a9fa136c-cd65-4f3c-ad7c-f542346a8d37.mp4)
+
+## Stop reason visualizer
+
+This is to visualize stop factor and reason.
+[see the details](./doc-stop-reason-visualizer.md)
+
+### How to use
+
+please launch the analyzer node
+
+```bash
+ros2 launch planning_debug_tools trajectory_analyzer.launch.xml
+```
+
+and visualize the analyzed data on the plot juggler following below.
+
+#### setup PlotJuggler
+
+For the first time, please add the following code to reactive script and save it as the picture below!
+(Looking for the way to automatically load the configuration file...)
+
+You can customize what you plot by editing this code.
+
+![image](./image/lua.png)
+
+in Global code
+
+```lua
+behavior_path = '/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id/debug_info'
+behavior_velocity = '/planning/scenario_planning/lane_driving/behavior_planning/path/debug_info'
+motion_avoid = '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory/debug_info'
+motion_smoother_latacc = '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/debug_info'
+motion_smoother = '/planning/scenario_planning/trajectory/debug_info'
+```
+
+in function(tracker_time)
+
+```lua
+PlotCurvatureOverArclength('k_behavior_path', behavior_path, tracker_time)
+PlotCurvatureOverArclength('k_behavior_velocity', behavior_velocity, tracker_time)
+PlotCurvatureOverArclength('k_motion_avoid', motion_avoid, tracker_time)
+PlotCurvatureOverArclength('k_motion_smoother', motion_smoother, tracker_time)
+
+PlotVelocityOverArclength('v_behavior_path', behavior_path, tracker_time)
+PlotVelocityOverArclength('v_behavior_velocity', behavior_velocity, tracker_time)
+PlotVelocityOverArclength('v_motion_avoid', motion_avoid, tracker_time)
+PlotVelocityOverArclength('v_motion_smoother_latacc', motion_smoother_latacc, tracker_time)
+PlotVelocityOverArclength('v_motion_smoother', motion_smoother, tracker_time)
+
+PlotAccelerationOverArclength('a_behavior_path', behavior_path, tracker_time)
+PlotAccelerationOverArclength('a_behavior_velocity', behavior_velocity, tracker_time)
+PlotAccelerationOverArclength('a_motion_avoid', motion_avoid, tracker_time)
+PlotAccelerationOverArclength('a_motion_smoother_latacc', motion_smoother_latacc, tracker_time)
+PlotAccelerationOverArclength('a_motion_smoother', motion_smoother, tracker_time)
+
+PlotYawOverArclength('yaw_behavior_path', behavior_path, tracker_time)
+PlotYawOverArclength('yaw_behavior_velocity', behavior_velocity, tracker_time)
+PlotYawOverArclength('yaw_motion_avoid', motion_avoid, tracker_time)
+PlotYawOverArclength('yaw_motion_smoother_latacc', motion_smoother_latacc, tracker_time)
+PlotYawOverArclength('yaw_motion_smoother', motion_smoother, tracker_time)
+
+PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_state', tracker_time)
+```
+
+in Function Library
+![image](./image/script.png)
+
+```lua
+
+function PlotValue(name, path, timestamp, value)
+  new_series = ScatterXY.new(name)
+  index = 0
+  while(true) do
+    series_k = TimeseriesView.find( string.format( "%s/"..value..".%d", path, index) )
+    series_s = TimeseriesView.find( string.format( "%s/arclength.%d", path, index) )
+    series_size = TimeseriesView.find( string.format( "%s/size", path) )
+
+    if series_k == nil or series_s == nil then break end
+
+    k = series_k:atTime(timestamp)
+    s = series_s:atTime(timestamp)
+    size = series_size:atTime(timestamp)
+
+    if index >= size then break end
+
+    new_series:push_back(s,k)
+    index = index+1
+  end
+
+function PlotCurvatureOverArclength(name, path, timestamp)
+  PlotValue(name, path, timestamp,"curvature")
+end
+
+function PlotVelocityOverArclength(name, path, timestamp)
+  PlotValue(name, path, timestamp,"velocity")
+end
+
+function PlotAccelerationOverArclength(name, path, timestamp)
+  PlotValue(name, path, timestamp,"acceleration")
+end
+
+function PlotYawOverArclength(name, path, timestamp)
+  PlotValue(name, path, timestamp,"yaw")
+end
+
+function PlotCurrentVelocity(name, kinematics_name, timestamp)
+  new_series = ScatterXY.new(name)
+  series_v = TimeseriesView.find( string.format( "%s/twist/twist/linear/x", kinematics_name))
+  if series_v == nil then
+    print("error")
+    return
+  end
+  v = series_v:atTime(timestamp)
+  new_series:push_back(0.0, v)
+end
+```
+
+Then, run the plot juggler.
+
+### How to customize the plot
+
+Add Path/PathWithLaneIds/Trajectory topics you want to plot in the `trajectory_analyzer.launch.xml`, then the analyzed topics for these messages will be published with `TrajectoryDebugINfo.msg` type. You can then visualize these data by editing the reactive script on the PlotJuggler.
+
+### Requirements
+
+The version of the plotJuggler must be > `3.5.0`
+
+## Closest velocity checker
+
+This node prints the velocity information indicated by planning/control modules on a terminal. For trajectories calculated by planning modules, the target velocity on the trajectory point which is closest to the ego vehicle is printed. For control commands calculated by control modules, the target velocity and acceleration is directly printed. This feature would be helpful for purposes such as "_investigating the reason why the vehicle does not move_".
+
+You can launch by
+
+```bash
+ros2 run planning_debug_tools closest_velocity_checker.py
+```
+
+![closest-velocity-checker](image/closest-velocity-checker.png)
+
+## Trajectory visualizer
+
+The old version of the trajectory analyzer. It is written in Python and more flexible, but very slow.

planning/planning_debug_tools/doc-stop-reason-visualizer.md

@@ -0,0 +1,21 @@
+## stop_reason_visualizer
+
+This module is to visualize stop factor quickly without selecting correct debug markers.
+This is supposed to use with virtual wall marker like below.
+![image](image/stop_reason_image.png)
+
+### How to use
+
+Run this node.
+
+```sh
+ros2 run planning_debug_tools stop_reason_visualizer_exe
+```
+
+Add stop reason debug marker from rviz.
+
+![image](image/add_marker.png)
+
+Note: ros2 process can be sometimes deleted only from `killall stop_reason_visualizer_exe`
+
+[Reference](https://answers.ros.org/question/323329/how-to-kill-nodes-in-ros2/?answer=403184#post-id-403184)

planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp

@@ -0,0 +1,125 @@
+// 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);
+    data.acceleration = getAccelerationArray(points);
+    data.yaw = getYawArray(points);
+
+    if (
+      data.size != data.arclength.size() || data.size != data.velocity.size() ||
+      data.size != data.yaw.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_