feat: move tools from autoware.universe

Signed-off-by: Takayuki Murooka <[email protected]>

common/tier4_debug_tools/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_debug_tools)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Eigen3 REQUIRED)
+
+include_directories(
+  SYSTEM
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+ament_auto_add_library(lateral_error_publisher SHARED
+  src/lateral_error_publisher.cpp
+)
+
+rclcpp_components_register_node(lateral_error_publisher
+  PLUGIN "LateralErrorPublisher"
+  EXECUTABLE lateral_error_publisher_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)
+
+install(PROGRAMS scripts/stop_reason2pose.py scripts/pose2tf.py scripts/tf2pose.py
+                scripts/case_converter.py scripts/self_pose_listener.py
+                scripts/stop_reason2tf DESTINATION lib/${PROJECT_NAME})
+
+install(FILES DESTINATION share/${PROJECT_NAME})

common/tier4_debug_tools/README.md

@@ -0,0 +1,132 @@
+# tier4_debug_tools
+
+This package provides useful features for debugging Autoware.
+
+## Usage
+
+### tf2pose
+
+This tool converts any `tf` to `pose` topic.  
+With this tool, for example, you can plot `x` values of `tf` in `rqt_multiplot`.
+
+```sh
+ros2 run tier4_debug_tools tf2pose {tf_from} {tf_to} {hz}
+```
+
+Example:
+
+```sh
+$ ros2 run tier4_debug_tools tf2pose base_link ndt_base_link 100
+
+$ ros2 topic echo /tf2pose/pose -n1
+header:
+  seq: 13
+  stamp:
+    secs: 1605168366
+    nsecs: 549174070
+  frame_id: "base_link"
+pose:
+  position:
+    x: 0.0387684271191
+    y: -0.00320360406477
+    z: 0.000276674520819
+  orientation:
+    x: 0.000335221893885
+    y: 0.000122020672186
+    z: -0.00539673212896
+    w: 0.999985368502
+---
+```
+
+### pose2tf
+
+This tool converts any `pose` topic to `tf`.
+
+```sh
+ros2 run tier4_debug_tools pose2tf {pose_topic_name} {tf_name}
+```
+
+Example:
+
+```sh
+$ ros2 run tier4_debug_tools pose2tf /localization/pose_estimator/pose ndt_pose
+
+$ ros2 run tf tf_echo ndt_pose ndt_base_link 100
+At time 1605168365.449
+- Translation: [0.000, 0.000, 0.000]
+- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
+            in RPY (radian) [0.000, -0.000, 0.000]
+            in RPY (degree) [0.000, -0.000, 0.000]
+```
+
+### stop_reason2pose
+
+This tool extracts `pose` from `stop_reasons`.  
+Topics without numbers such as `/stop_reason2pose/pose/detection_area` are the nearest stop_reasons, and topics with numbers are individual stop_reasons that are roughly matched with previous ones.
+
+```sh
+ros2 run tier4_debug_tools stop_reason2pose {stop_reason_topic_name}
+```
+
+Example:
+
+```sh
+$ ros2 run tier4_debug_tools stop_reason2pose /planning/scenario_planning/status/stop_reasons
+
+$ ros2 topic list | ag stop_reason2pose
+/stop_reason2pose/pose/detection_area
+/stop_reason2pose/pose/detection_area_1
+/stop_reason2pose/pose/obstacle_stop
+/stop_reason2pose/pose/obstacle_stop_1
+
+$ ros2 topic echo /stop_reason2pose/pose/detection_area -n1
+header:
+  seq: 1
+  stamp:
+    secs: 1605168355
+    nsecs:    821713
+  frame_id: "map"
+pose:
+  position:
+    x: 60608.8433457
+    y: 43886.2410876
+    z: 44.9078212441
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: -0.190261378408
+    w: 0.981733470901
+---
+```
+
+### stop_reason2tf
+
+This is an all-in-one script that uses `tf2pose`, `pose2tf`, and `stop_reason2pose`.  
+With this tool, you can view the relative position from base_link to the nearest stop_reason.
+
+```sh
+ros2 run tier4_debug_tools stop_reason2tf {stop_reason_name}
+```
+
+Example:
+
+```sh
+$ ros2 run tier4_debug_tools stop_reason2tf obstacle_stop
+At time 1605168359.501
+- Translation: [0.291, -0.095, 0.266]
+- Rotation: in Quaternion [0.007, 0.011, -0.005, 1.000]
+            in RPY (radian) [0.014, 0.023, -0.010]
+            in RPY (degree) [0.825, 1.305, -0.573]
+```
+
+### lateral_error_publisher
+
+This node calculate the control error and localization error in the trajectory normal direction as shown in the figure below.
+
+![lateral_error_publisher_overview](./media/lateral_error_publisher.svg)
+
+Set the reference trajectory, vehicle pose and ground truth pose in the launch file.
+
+```sh
+ros2 launch tier4_debug_tools lateral_error_publisher.launch.xml
+```

common/tier4_debug_tools/config/lateral_error_publisher.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    yaw_threshold_to_search_closest: 0.785398  # yaw threshold to search closest index [rad]

common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp

@@ -0,0 +1,74 @@
+// 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 TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_
+#define TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_
+
+#define EIGEN_MPL2_ONLY
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <tier4_debug_msgs/msg/float32_stamped.hpp>
+
+class LateralErrorPublisher : public rclcpp::Node
+{
+public:
+  explicit LateralErrorPublisher(const rclcpp::NodeOptions & node_options);
+
+private:
+  /* Parameters */
+  double yaw_threshold_to_search_closest_;
+
+  /* States */
+  autoware_auto_planning_msgs::msg::Trajectory::SharedPtr
+    current_trajectory_ptr_;  //!< @brief reference trajectory
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr
+    current_vehicle_pose_ptr_;  //!< @brief current EKF pose
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr
+    current_ground_truth_pose_ptr_;  //!< @brief current GNSS pose
+
+  /* Publishers and Subscribers */
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    sub_trajectory_;  //!< @brief subscription for reference trajectory
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    sub_vehicle_pose_;  //!< @brief subscription for vehicle pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    sub_ground_truth_pose_;  //!< @brief subscription for gnss pose
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
+    pub_control_lateral_error_;  //!< @brief publisher for control lateral error
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
+    pub_localization_lateral_error_;  //!< @brief publisher for localization lateral error
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
+    pub_lateral_error_;  //!< @brief publisher for lateral error (control + localization)
+
+  /**
+   * @brief set current_trajectory_ with received message
+   */
+  void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
+  /**
+   * @brief set current_vehicle_pose_ with received message
+   */
+  void onVehiclePose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+  /**
+   * @brief set current_ground_truth_pose_ and calculate lateral error
+   */
+  void onGroundTruthPose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+};
+
+#endif  // TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_

common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="lateral_error_publisher_param_path" default="$(find-pkg-share tier4_debug_tools)/config/lateral_error_publisher.param.yaml"/>
+
+  <!-- mpc for trajectory following -->
+  <node pkg="tier4_debug_tools" exec="lateral_error_publisher_node" name="lateral_error_publisher" output="screen">
+    <param from="$(var lateral_error_publisher_param_path)"/>
+    <remap from="~/input/reference_trajectory" to="/planning/scenario_planning/trajectory"/>
+    <remap from="~/input/vehicle_pose_with_covariance" to="/localization/pose_with_covariance"/>
+    <remap from="~/input/ground_truth_pose_with_covariance" to="/localization/pose_with_covariance"/>
+  </node>
+</launch>