feat(planning_debug_tools): add readme and add yaw to analyzer (#1613)

* feat(planning_debug_tools): add readme and add yaw to analyzer Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore: remove verbose Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * update docs Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix precommit Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · Aug 19, 2022 · 020c6d8 · 020c6d8
1 parent d47adcc
commit 020c6d8
Show file tree

Hide file tree

Showing 7 changed files with 163 additions and 11 deletions.
diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/Readme.md
@@ -0,0 +1,139 @@
+# 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)
+
+### 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)
+
+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 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.
diff --git a/planning/planning_debug_tools/image/closest-velocity-checker.png b/planning/planning_debug_tools/image/closest-velocity-checker.png
diff --git a/planning/planning_debug_tools/image/lua.png b/planning/planning_debug_tools/image/lua.png
diff --git a/planning/planning_debug_tools/image/script.png b/planning/planning_debug_tools/image/script.png
diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp
@@ -91,8 +91,11 @@ class TrajectoryAnalyzer
     const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p);
     data.arclength = calcPathArcLengthArray(points, -arclength_offset);
     data.velocity = getVelocityArray(points);
+    data.yaw = getYawArray(points);
 
-    if (data.size != data.arclength.size() || data.size != data.velocity.size()) {
+    if (
+      data.size != data.arclength.size() || data.size != data.velocity.size() ||
+      data.size != data.yaw.size()) {
       RCLCPP_ERROR(node_->get_logger(), "computation failed.");
       return;
     }

diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp
@@ -29,20 +29,29 @@
 namespace planning_debug_tools
 {
 
+using autoware_auto_planning_msgs::msg::PathPoint;
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using tier4_autoware_utils::calcDistance2d;
 using tier4_autoware_utils::getPoint;
+using tier4_autoware_utils::getRPY;
 
-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)
+double getVelocity(const PathPoint & p) { return p.longitudinal_velocity_mps; }
+double getVelocity(const PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; }
+double getVelocity(const TrajectoryPoint & p) { return p.longitudinal_velocity_mps; }
+
+double getYaw(const PathPoint & p) { return getRPY(p.pose.orientation).z; }
+double getYaw(const PathPointWithLaneId & p) { return getRPY(p.point.pose.orientation).z; }
+double getYaw(const TrajectoryPoint & p) { return getRPY(p.pose.orientation).z; }
+
+template <class T>
+inline std::vector<double> getYawArray(const T & points)
 {
-  return p.longitudinal_velocity_mps;
+  std::vector<double> yaw_arr;
+  for (const auto & p : points) {
+    yaw_arr.push_back(getYaw(p));
+  }
+  return yaw_arr;
 }
 template <class T>
 inline std::vector<double> getVelocityArray(const T & points)

diff --git a/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg b/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg
@@ -3,3 +3,4 @@ uint32 size
 float64[] arclength
 float64[] curvature
 float64[] velocity
+float64[] yaw