feat: add autoware_debug_tools packages (autowarefoundation#99)

* Ros2 v0.8.0 autoware debug tools (autowarefoundation#330) * initial commit * fix bug * fix lint * delete setup.py * fix too long line * add copyright * change directory structure * fix tf2pose * fix pose2tf.py * add test self pose listener * format * delete queue size * use timer * fix stop_reason2pose * fix stop_reason2tf * fix topic name on tf2pose * format * Fix typo in common module (autowarefoundation#433) * Unify Apache-2.0 license name (autowarefoundation#1242) * Fix lint errors (autowarefoundation#1378) * Fix lint errors Signed-off-by: Kenji Miyake <[email protected]> * Fix variable names Signed-off-by: Kenji Miyake <[email protected]> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Add shellcheck (autowarefoundation#2079) * Add shellcheck Signed-off-by: Kenji Miyake <[email protected]> * Fix shellcheck Signed-off-by: Kenji Miyake <[email protected]> * Feature/add lateral error publisher (autowarefoundation#2167) * Add CMakeLists, package.xml and base cpp/hpp for lateral_error_publisher Signed-off-by: Makoto Kurihara <[email protected]> * Implementing ... Signed-off-by: Makoto Kurihara <[email protected]> * Register lateral_error_publisher node Signed-off-by: Makoto Kurihara <[email protected]> * Add control/localization lateral error calculation Signed-off-by: Makoto Kurihara <[email protected]> * Add config file Signed-off-by: Makoto Kurihara <[email protected]> * Add publisher Signed-off-by: Makoto Kurihara <[email protected]> * Change RCLCPP_INFO to RCLCPP_DEBUG Signed-off-by: Makoto Kurihara <[email protected]> * Add readme and fix topic name Signed-off-by: Makoto Kurihara <[email protected]> * Fix a lateral error publisher overview figure Signed-off-by: Makoto Kurihara <[email protected]> * Fix code style Signed-off-by: Makoto Kurihara <[email protected]> * Fix to pre-commit test Signed-off-by: Makoto Kurihara <[email protected]> * Fix type to reference Signed-off-by: Makoto Kurihara <[email protected]> * Use MPL2 licence only at eigen library Signed-off-by: Makoto Kurihara <[email protected]> * Fix comment Signed-off-by: Makoto Kurihara <[email protected]> * Fix condition Signed-off-by: Makoto Kurihara <[email protected]> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <[email protected]> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <[email protected]> * Apply Black Signed-off-by: Kenji Miyake <[email protected]> * Apply clang-format Signed-off-by: Kenji Miyake <[email protected]> * Fix build errors Signed-off-by: Kenji Miyake <[email protected]> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <[email protected]> * Apply clang-format Signed-off-by: Kenji Miyake <[email protected]> * Fix build errors Signed-off-by: Kenji Miyake <[email protected]> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <[email protected]> * port autoware_debug_tools (autowarefoundation#509) Co-authored-by: Takayuki Murooka <[email protected]> * fix readme Co-authored-by: Shigeki Kobayashi <[email protected]> Co-authored-by: Kazuki Miyahara <[email protected]> Co-authored-by: Kenji Miyake <[email protected]> Co-authored-by: Keisuke Shima <[email protected]> Co-authored-by: Makoto Kurihara <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]>
mitsudome-r · Dec 6, 2021 · 3895c0f · 3895c0f
1 parent 5d6f417
commit 3895c0f
Show file tree

Hide file tree

Showing 14 changed files with 872 additions and 0 deletions.
diff --git a/common/autoware_debug_tools/CMakeLists.txt b/common/autoware_debug_tools/CMakeLists.txt
@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_debug_tools)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Eigen3 REQUIRED)
+
+#ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR "scripts/")
+
+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
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+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})
diff --git a/common/autoware_debug_tools/README.md b/common/autoware_debug_tools/README.md
@@ -0,0 +1,132 @@
+# autoware_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 autoware_debug_tools tf2pose {tf_from} {tf_to} {hz}
+```
+
+Example:
+
+```sh
+$ ros2 run autoware_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 autoware_debug_tools pose2tf {pose_topic_name} {tf_name}
+```
+
+Example:
+
+```sh
+$ ros2 run autoware_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 autoware_debug_tools stop_reason2pose {stop_reason_topic_name}
+```
+
+Example:
+
+```sh
+$ ros2 run autoware_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 autoware_debug_tools stop_reason2tf {stop_reason_name}
+```
+
+Example:
+
+```sh
+$ ros2 run autoware_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 autoware_debug_tools lateral_error_publisher.launch.xml
+```
diff --git a/common/autoware_debug_tools/config/lateral_error_publisher.param.yaml b/common/autoware_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]
diff --git a/common/autoware_debug_tools/include/autoware_debug_tools/lateral_error_publisher.hpp b/common/autoware_debug_tools/include/autoware_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 AUTOWARE_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_
+#define AUTOWARE_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_
+
+#define EIGEN_MPL2_ONLY
+
+#include <autoware_utils/trajectory/trajectory.hpp>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_debug_msgs/msg/float32_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_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<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr
+    pub_control_lateral_error_;  //!< @brief publisher for control lateral error
+  rclcpp::Publisher<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr
+    pub_localization_lateral_error_;  //!< @brief publisher for localization lateral error
+  rclcpp::Publisher<autoware_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  // AUTOWARE_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_
diff --git a/common/autoware_debug_tools/launch/lateral_error_publisher.launch.xml b/common/autoware_debug_tools/launch/lateral_error_publisher.launch.xml
@@ -0,0 +1,13 @@
+<launch>
+
+   <arg name="lateral_error_publisher_param_path" default="$(find-pkg-share autoware_debug_tools)/config/lateral_error_publisher.param.yaml"/>
+
+  <!-- mpc for trajectory following -->
+  <node pkg="autoware_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>