From 3895c0f7002efd6feab0e7b9e9adeb782cfba136 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 6 Dec 2021 17:14:44 +0900 Subject: [PATCH] feat: add autoware_debug_tools packages (#99) * Ros2 v0.8.0 autoware debug tools (#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 (#433) * Unify Apache-2.0 license name (#1242) * Fix lint errors (#1378) * Fix lint errors Signed-off-by: Kenji Miyake * Fix variable names Signed-off-by: Kenji Miyake * add sort-package-xml hook in pre-commit (#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 (#2079) * Add shellcheck Signed-off-by: Kenji Miyake * Fix shellcheck Signed-off-by: Kenji Miyake * Feature/add lateral error publisher (#2167) * Add CMakeLists, package.xml and base cpp/hpp for lateral_error_publisher Signed-off-by: Makoto Kurihara * Implementing ... Signed-off-by: Makoto Kurihara * Register lateral_error_publisher node Signed-off-by: Makoto Kurihara * Add control/localization lateral error calculation Signed-off-by: Makoto Kurihara * Add config file Signed-off-by: Makoto Kurihara * Add publisher Signed-off-by: Makoto Kurihara * Change RCLCPP_INFO to RCLCPP_DEBUG Signed-off-by: Makoto Kurihara * Add readme and fix topic name Signed-off-by: Makoto Kurihara * Fix a lateral error publisher overview figure Signed-off-by: Makoto Kurihara * Fix code style Signed-off-by: Makoto Kurihara * Fix to pre-commit test Signed-off-by: Makoto Kurihara * Fix type to reference Signed-off-by: Makoto Kurihara * Use MPL2 licence only at eigen library Signed-off-by: Makoto Kurihara * Fix comment Signed-off-by: Makoto Kurihara * Fix condition Signed-off-by: Makoto Kurihara * Change formatter to clang-format and black (#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 * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * port autoware_debug_tools (#509) Co-authored-by: Takayuki Murooka * fix readme Co-authored-by: Shigeki Kobayashi <32808802+sgk-000@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: Takayuki Murooka Co-authored-by: Takayuki Murooka --- common/autoware_debug_tools/CMakeLists.txt | 44 +++++ common/autoware_debug_tools/README.md | 132 ++++++++++++++ .../config/lateral_error_publisher.param.yaml | 3 + .../lateral_error_publisher.hpp | 74 ++++++++ .../launch/lateral_error_publisher.launch.xml | 13 ++ .../media/lateral_error_publisher.svg | 3 + common/autoware_debug_tools/package.xml | 28 +++ .../scripts/case_converter.py | 25 +++ .../autoware_debug_tools/scripts/pose2tf.py | 78 ++++++++ .../scripts/self_pose_listener.py | 56 ++++++ .../scripts/stop_reason2pose.py | 167 ++++++++++++++++++ .../scripts/stop_reason2tf | 15 ++ .../autoware_debug_tools/scripts/tf2pose.py | 83 +++++++++ .../src/lateral_error_publisher.cpp | 151 ++++++++++++++++ 14 files changed, 872 insertions(+) create mode 100644 common/autoware_debug_tools/CMakeLists.txt create mode 100644 common/autoware_debug_tools/README.md create mode 100644 common/autoware_debug_tools/config/lateral_error_publisher.param.yaml create mode 100644 common/autoware_debug_tools/include/autoware_debug_tools/lateral_error_publisher.hpp create mode 100644 common/autoware_debug_tools/launch/lateral_error_publisher.launch.xml create mode 100644 common/autoware_debug_tools/media/lateral_error_publisher.svg create mode 100644 common/autoware_debug_tools/package.xml create mode 100755 common/autoware_debug_tools/scripts/case_converter.py create mode 100755 common/autoware_debug_tools/scripts/pose2tf.py create mode 100755 common/autoware_debug_tools/scripts/self_pose_listener.py create mode 100755 common/autoware_debug_tools/scripts/stop_reason2pose.py create mode 100755 common/autoware_debug_tools/scripts/stop_reason2tf create mode 100755 common/autoware_debug_tools/scripts/tf2pose.py create mode 100644 common/autoware_debug_tools/src/lateral_error_publisher.cpp diff --git a/common/autoware_debug_tools/CMakeLists.txt b/common/autoware_debug_tools/CMakeLists.txt new file mode 100644 index 0000000000000..bef66b94b411d --- /dev/null +++ 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 new file mode 100644 index 0000000000000..c4227ab8a8025 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..ecde03de7c658 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..1691d9c90c24d --- /dev/null +++ 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 +#include +#include +#include + +#include +#include +#include + +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::SharedPtr + sub_trajectory_; //!< @brief subscription for reference trajectory + rclcpp::Subscription::SharedPtr + sub_vehicle_pose_; //!< @brief subscription for vehicle pose + rclcpp::Subscription::SharedPtr + sub_ground_truth_pose_; //!< @brief subscription for gnss pose + rclcpp::Publisher::SharedPtr + pub_control_lateral_error_; //!< @brief publisher for control lateral error + rclcpp::Publisher::SharedPtr + pub_localization_lateral_error_; //!< @brief publisher for localization lateral error + rclcpp::Publisher::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 new file mode 100644 index 0000000000000..c4732e239c9ab --- /dev/null +++ b/common/autoware_debug_tools/launch/lateral_error_publisher.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/common/autoware_debug_tools/media/lateral_error_publisher.svg b/common/autoware_debug_tools/media/lateral_error_publisher.svg new file mode 100644 index 0000000000000..b7e511c83281b --- /dev/null +++ b/common/autoware_debug_tools/media/lateral_error_publisher.svg @@ -0,0 +1,3 @@ + + +
①
①
Closest trajectory point
Closest trajectory poi...
Next trajectory point
Next trajectory point
Trajectory
Trajectory
Ground truth pose
Ground truth pose
Vehicle pose
Vehicle pose
②
②
① Control lateral error
② Localization lateral error
①+② Lateral error
① Control lateral error...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/common/autoware_debug_tools/package.xml b/common/autoware_debug_tools/package.xml new file mode 100644 index 0000000000000..686744c498d14 --- /dev/null +++ b/common/autoware_debug_tools/package.xml @@ -0,0 +1,28 @@ + + + autoware_debug_tools + 0.1.0 + The autoware_debug_tools package + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_planning_msgs + autoware_debug_msgs + autoware_utils + geometry_msgs + rclcpp + rclcpp_components + tf2_ros + + launch_ros + rclpy + python3-rtree + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_debug_tools/scripts/case_converter.py b/common/autoware_debug_tools/scripts/case_converter.py new file mode 100755 index 0000000000000..2fcc3eebac62e --- /dev/null +++ b/common/autoware_debug_tools/scripts/case_converter.py @@ -0,0 +1,25 @@ +#! /usr/bin/env python3 + +# Copyright 2020 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. + +import re + + +def pascal2snake(s): + return re.sub(r"([a-z])([A-Z])", r"\1_\2", s).lower() + + +def snake2pascal(s): + return "".join([w.capitalize() for w in s.split("_")]) diff --git a/common/autoware_debug_tools/scripts/pose2tf.py b/common/autoware_debug_tools/scripts/pose2tf.py new file mode 100755 index 0000000000000..c954e370d4069 --- /dev/null +++ b/common/autoware_debug_tools/scripts/pose2tf.py @@ -0,0 +1,78 @@ +#! /usr/bin/env python3 + +# Copyright 2020 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. + + +import argparse +import sys + +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import TransformStamped +import rclpy +from rclpy.node import Node +import tf2_ros + + +class Pose2TfNode(Node): + def __init__(self, options): + super().__init__("pose2tf") + self._options = options + self._tf_broadcaster = tf2_ros.TransformBroadcaster(self) + self._sub_pose = self.create_subscription( + PoseStamped, self._options.topic_name, self._on_pose, 1 + ) + + def _on_pose(self, msg): + try: + tfs = Pose2TfNode.create_transform(self, msg) + transforms = [] + transforms.append(tfs) + self._tf_broadcaster.sendTransform(transforms) + except tf2_ros.TransformException as e: + print(e) + + @staticmethod + def create_transform(self, msg): + tfs = TransformStamped() + tfs.header.frame_id = msg.header.frame_id + tfs.header.stamp = msg.header.stamp + tfs.child_frame_id = self._options.tf_name + tfs.transform.translation.x = msg.pose.position.x + tfs.transform.translation.y = msg.pose.position.y + tfs.transform.translation.z = msg.pose.position.z + tfs.transform.rotation.x = msg.pose.orientation.x + tfs.transform.rotation.y = msg.pose.orientation.y + tfs.transform.rotation.z = msg.pose.orientation.z + tfs.transform.rotation.w = msg.pose.orientation.w + return tfs + + +def main(args): + print("{}".format(args)) + rclpy.init() + + parser = argparse.ArgumentParser() + parser.add_argument("topic_name", type=str) + parser.add_argument("tf_name", type=str) + ns = parser.parse_args(args) + + pose2tf_node = Pose2TfNode(ns) + rclpy.spin(pose2tf_node) + pose2tf_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/common/autoware_debug_tools/scripts/self_pose_listener.py b/common/autoware_debug_tools/scripts/self_pose_listener.py new file mode 100755 index 0000000000000..0087bfbbbdf1e --- /dev/null +++ b/common/autoware_debug_tools/scripts/self_pose_listener.py @@ -0,0 +1,56 @@ +#! /usr/bin/env python3 + +# Copyright 2020 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. + +from geometry_msgs.msg import PoseStamped +import rclpy +from rclpy.node import Node +from tf2_ros import LookupException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class SelfPoseListener(Node): + def __init__(self): + super().__init__("self_pose_listener") + self.tf_buffer = Buffer() + self._tf_listener = TransformListener(self.tf_buffer, self) + self.self_pose = PoseStamped() + + def get_current_pose(self): + try: + tf = self.tf_buffer.lookup_transform("map", "base_link", rclpy.time.Time()) + tf_time = self.tf_buffer.get_latest_common_time("map", "base_link") + self.self_pose = SelfPoseListener.create_pose(tf_time, "map", tf) + except LookupException as e: + self.get_logger().warn("Required transformation not found: `{}`".format(str(e))) + return None + + @staticmethod + def create_pose(time, frame_id, tf): + pose = PoseStamped() + + pose.header.stamp = time.to_msg() + pose.header.frame_id = frame_id + + pose.pose.position.x = tf.transform.translation.x + pose.pose.position.y = tf.transform.translation.y + pose.pose.position.z = tf.transform.translation.z + pose.pose.orientation.x = tf.transform.rotation.x + pose.pose.orientation.y = tf.transform.rotation.y + pose.pose.orientation.z = tf.transform.rotation.z + pose.pose.orientation.w = tf.transform.rotation.w + + return pose diff --git a/common/autoware_debug_tools/scripts/stop_reason2pose.py b/common/autoware_debug_tools/scripts/stop_reason2pose.py new file mode 100755 index 0000000000000..d4e1891a34dd9 --- /dev/null +++ b/common/autoware_debug_tools/scripts/stop_reason2pose.py @@ -0,0 +1,167 @@ +#! /usr/bin/env python3 + +# Copyright 2020 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. + +import argparse +import math +import sys + +from autoware_planning_msgs.msg import StopReasonArray +from case_converter import pascal2snake +from geometry_msgs.msg import PoseStamped +import numpy as np +import rclpy +from rclpy.node import Node +from rtree import index +from self_pose_listener import SelfPoseListener + + +class StopReason2PoseNode(Node): + def __init__(self, options): + super().__init__("stop_reason2pose_node") + self._options = options + self._sub_pose = self.create_subscription( + StopReasonArray, self._options.topic_name, self._on_stop_reasons, 1 + ) + self._pub_pose_map = {} + self._idx_map = {} + self._pose_map = {} + self._self_pose_listener = SelfPoseListener() + self.timer = self.create_timer((1.0 / 100), self._self_pose_listener.get_current_pose) + + def _on_stop_reasons(self, msg): + for stop_reason in msg.stop_reasons: + snake_case_stop_reason = pascal2snake(stop_reason.reason) + + if len(stop_reason.stop_factors) == 0: + self.get_logger().warn("stop_factor is null") + return + + for stop_factor in stop_reason.stop_factors: + pose = PoseStamped() + pose.header = msg.header + pose.pose = stop_factor.stop_pose + + # Get nearest pose + th_dist = 1.0 + nearest_pose_id = self._get_nearest_pose_id( + snake_case_stop_reason, pose.pose, th_dist + ) + if nearest_pose_id: + self._update_pose(snake_case_stop_reason, pose.pose, nearest_pose_id) + pose_id = nearest_pose_id + else: + pose_id = self._register_pose(snake_case_stop_reason, pose.pose) + + pose_topic_name = "{snake_case_stop_reason}_{pose_id}".format(**locals()) + topic_ns = "/autoware_debug_tools/stop_reason2pose/" + if pose_topic_name not in self._pub_pose_map: + self._pub_pose_map[pose_topic_name] = self.create_publisher( + PoseStamped, topic_ns + pose_topic_name, 1 + ) + self._pub_pose_map[pose_topic_name].publish(pose) + + # Publish nearest stop_reason without number + nearest_pose = PoseStamped() + nearest_pose.header = msg.header + nearest_pose.pose = self._get_nearest_pose_in_array( + stop_reason, self._self_pose_listener.self_pose + ) + + if nearest_pose.pose: + if snake_case_stop_reason not in self._pub_pose_map: + topic_ns = "/autoware_debug_tools/stop_reason2pose/" + self._pub_pose_map[snake_case_stop_reason] = self.create_publisher( + PoseStamped, topic_ns + snake_case_stop_reason, 1 + ) + self._pub_pose_map[snake_case_stop_reason].publish(nearest_pose) + + def _get_nearest_pose_in_array(self, stop_reason, self_pose): + poses = [stop_factor.stop_pose for stop_factor in stop_reason.stop_factors] + if not poses: + return None + + distances = map(lambda p: StopReason2PoseNode.calc_distance2d(p, self_pose), poses) + nearest_idx = np.argmin(distances) + + return poses[nearest_idx] + + def _find_nearest_pose_id(self, name, pose): + if name not in self._idx_map: + self._idx_map[name] = index.Index() + + return self._idx_map[name].nearest(StopReason2PoseNode.pose2boundingbox(pose), 1) + + def _get_nearest_pose_id(self, name, pose, th_dist): + nearest_pose_ids = list(self._find_nearest_pose_id(name, pose)) + if not nearest_pose_ids: + return None + + nearest_pose_id = nearest_pose_ids[0] + nearest_pose = self._get_pose(name, nearest_pose_id) + if not nearest_pose: + return None + + dist = StopReason2PoseNode.calc_distance2d(pose, nearest_pose) + if dist > th_dist: + return None + + return nearest_pose_id + + def _get_pose(self, name, pose_id): + if name not in self._pose_map: + return None + + return self._pose_map[name][pose_id] + + def _update_pose(self, name, pose, pose_id): + self._pose_map[name][id] = pose + self._idx_map[name].insert(pose_id, StopReason2PoseNode.pose2boundingbox(pose)) + + def _register_pose(self, name, pose): + if name not in self._pose_map: + self._pose_map[name] = {} + + pose_id = len(self._pose_map[name]) + 1 + self._pose_map[name][pose_id] = pose + self._idx_map[name].insert(pose_id, StopReason2PoseNode.pose2boundingbox(pose)) + return pose_id + + @staticmethod + def calc_distance2d(pose1, pose2): + p1 = pose1.position + p2 = pose2.position + return math.hypot(p1.x - p2.x, p1.y - p2.y) + + @staticmethod + def pose2boundingbox(pose): + return [pose.position.x, pose.position.y, pose.position.x, pose.position.y] + + +def main(args): + rclpy.init() + + parser = argparse.ArgumentParser() + parser.add_argument("topic_name", type=str) + ns = parser.parse_args(args) + + stop_reason2pose_node = StopReason2PoseNode(ns) + rclpy.spin(stop_reason2pose_node) + stop_reason2pose_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/common/autoware_debug_tools/scripts/stop_reason2tf b/common/autoware_debug_tools/scripts/stop_reason2tf new file mode 100755 index 0000000000000..65c6b0be7db95 --- /dev/null +++ b/common/autoware_debug_tools/scripts/stop_reason2tf @@ -0,0 +1,15 @@ +#!/usr/bin/env bash + +stop_reason_name="$1" + +if [ -z "${stop_reason_name}" ]; then + echo "Please input stop_reason_name as the 1st argument." + exit 1 +fi + +ros2 run autoware_debug_tools stop_reason2pose.py /planning/scenario_planning/status/stop_reasons >/dev/null 2>&1 & +ros2 run autoware_debug_tools pose2tf.py /autoware_debug_tools/stop_reason2pose/"${stop_reason_name}" "${stop_reason_name}" >/dev/null 2>&1 & +ros2 run autoware_debug_tools tf2pose.py "${stop_reason_name}" base_link 100 >/dev/null 2>&1 & +ros2 run tf2_ros tf2_echo "${stop_reason_name}" base_link 100 2>/dev/null + +wait diff --git a/common/autoware_debug_tools/scripts/tf2pose.py b/common/autoware_debug_tools/scripts/tf2pose.py new file mode 100755 index 0000000000000..6bee0348ca811 --- /dev/null +++ b/common/autoware_debug_tools/scripts/tf2pose.py @@ -0,0 +1,83 @@ +#! /usr/bin/env python3 + +# Copyright 2020 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. + +import argparse +import sys + +from geometry_msgs.msg import PoseStamped +import rclpy +from rclpy.node import Node +from tf2_ros import LookupException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class Tf2PoseNode(Node): + def __init__(self, options): + super().__init__("tf2pose") + + self._options = options + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self._pub_pose = self.create_publisher(PoseStamped, "/autoware_debug_tools/tf2pose/pose", 1) + self.timer = self.create_timer((1.0 / self._options.hz), self._on_timer) + + def _on_timer(self): + try: + tf = self.tf_buffer.lookup_transform( + self._options.tf_from, self._options.tf_to, rclpy.time.Time() + ) + time = self.tf_buffer.get_latest_common_time(self._options.tf_from, self._options.tf_to) + pose = Tf2PoseNode.create_pose(time, self._options.tf_from, tf) + self._pub_pose.publish(pose) + except LookupException as e: + print(e) + + @staticmethod + def create_pose(time, frame_id, tf): + pose = PoseStamped() + + pose.header.stamp = time.to_msg() + pose.header.frame_id = frame_id + + pose.pose.position.x = tf.transform.translation.x + pose.pose.position.y = tf.transform.translation.y + pose.pose.position.z = tf.transform.translation.z + pose.pose.orientation.x = tf.transform.rotation.x + pose.pose.orientation.y = tf.transform.rotation.y + pose.pose.orientation.z = tf.transform.rotation.z + pose.pose.orientation.w = tf.transform.rotation.w + + return pose + + +def main(args): + rclpy.init() + + parser = argparse.ArgumentParser() + parser.add_argument("tf_from", type=str) + parser.add_argument("tf_to", type=str) + parser.add_argument("hz", type=int, default=10) + ns = parser.parse_args(args) + + tf2pose_node = Tf2PoseNode(ns) + rclpy.spin(tf2pose_node) + tf2pose_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/common/autoware_debug_tools/src/lateral_error_publisher.cpp b/common/autoware_debug_tools/src/lateral_error_publisher.cpp new file mode 100644 index 0000000000000..3cbd8e91cf54f --- /dev/null +++ b/common/autoware_debug_tools/src/lateral_error_publisher.cpp @@ -0,0 +1,151 @@ +// 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 "autoware_debug_tools/lateral_error_publisher.hpp" + +#include + +LateralErrorPublisher::LateralErrorPublisher(const rclcpp::NodeOptions & node_options) +: Node("lateral_error_publisher", node_options) +{ + using std::placeholders::_1; + + /* Parameters */ + yaw_threshold_to_search_closest_ = + declare_parameter("yaw_threshold_to_search_closest", M_PI / 4.0); + + /* Publishers and Subscribers */ + sub_trajectory_ = create_subscription( + "~/input/reference_trajectory", rclcpp::QoS{1}, + std::bind(&LateralErrorPublisher::onTrajectory, this, _1)); + sub_vehicle_pose_ = create_subscription( + "~/input/vehicle_pose_with_covariance", rclcpp::QoS{1}, + std::bind(&LateralErrorPublisher::onVehiclePose, this, _1)); + sub_ground_truth_pose_ = create_subscription( + "~/input/ground_truth_pose_with_covariance", rclcpp::QoS{1}, + std::bind(&LateralErrorPublisher::onGroundTruthPose, this, _1)); + pub_control_lateral_error_ = + create_publisher("~/control_lateral_error", 1); + pub_localization_lateral_error_ = + create_publisher("~/localization_lateral_error", 1); + pub_lateral_error_ = + create_publisher("~/lateral_error", 1); +} + +void LateralErrorPublisher::onTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + current_trajectory_ptr_ = msg; +} + +void LateralErrorPublisher::onVehiclePose( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + current_vehicle_pose_ptr_ = msg; +} + +void LateralErrorPublisher::onGroundTruthPose( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + current_ground_truth_pose_ptr_ = msg; + + // Guard + if (current_trajectory_ptr_ == nullptr || current_vehicle_pose_ptr_ == nullptr) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, + "Reference trajectory or EKF pose is not received"); + return; + } + if (current_trajectory_ptr_->points.size() < 2) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, "Reference trajectory is too short"); + return; + } + + // Search closest trajectory point with vehicle pose + const auto closest_index = autoware_utils::findNearestIndex( + current_trajectory_ptr_->points, current_vehicle_pose_ptr_->pose.pose, + std::numeric_limits::max(), yaw_threshold_to_search_closest_); + if (!closest_index) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, "Failed to search closest index"); + return; + } + + // Calculate the normal vector in the reference trajectory direction + size_t base_index = 0; + size_t next_index = 0; + if (*closest_index == current_trajectory_ptr_->points.size() - 1) { + base_index = *closest_index - 1; + next_index = *closest_index; + } else { + base_index = *closest_index; + next_index = *closest_index + 1; + } + + const auto & base_pose = current_trajectory_ptr_->points.at(base_index).pose; + const auto & next_pose = current_trajectory_ptr_->points.at(next_index).pose; + const double dx = next_pose.position.x - base_pose.position.x; + const double dy = next_pose.position.y - base_pose.position.y; + const Eigen::Vector2d trajectory_direction(dx, dy); + RCLCPP_DEBUG(this->get_logger(), "trajectory direction: (%f, %f)", dx, dy); + + const auto rotation = Eigen::Rotation2Dd(M_PI_2); + const Eigen::Vector2d normal_direction = rotation * trajectory_direction; + RCLCPP_DEBUG( + this->get_logger(), "normal direction: (%f, %f)", normal_direction(0), normal_direction(1)); + const Eigen::Vector2d unit_normal_direction = normal_direction.normalized(); + RCLCPP_DEBUG( + this->get_logger(), "unit normal direction: (%f, %f)", unit_normal_direction(0), + unit_normal_direction(1)); + + // Calculate control lateral error + const auto & closest_pose = current_trajectory_ptr_->points.at(*closest_index).pose; + const auto & vehicle_pose = current_vehicle_pose_ptr_->pose.pose; + const Eigen::Vector2d closest_to_vehicle( + vehicle_pose.position.x - closest_pose.position.x, + vehicle_pose.position.y - closest_pose.position.y); + const auto control_lateral_error = closest_to_vehicle.dot(unit_normal_direction); + RCLCPP_DEBUG(this->get_logger(), "control_lateral_error: %f", control_lateral_error); + + // Calculate localization lateral error + const auto ground_truth_pose = current_ground_truth_pose_ptr_->pose.pose; + const Eigen::Vector2d vehicle_to_ground_truth( + ground_truth_pose.position.x - vehicle_pose.position.x, + ground_truth_pose.position.y - vehicle_pose.position.y); + const auto localization_lateral_error = vehicle_to_ground_truth.dot(unit_normal_direction); + RCLCPP_DEBUG(this->get_logger(), "localization_lateral_error: %f", localization_lateral_error); + + const auto lateral_error = control_lateral_error + localization_lateral_error; + RCLCPP_DEBUG(this->get_logger(), "localization_error: %f", lateral_error); + + // Publish lateral errors + autoware_debug_msgs::msg::Float32Stamped control_msg; + control_msg.stamp = this->now(); + control_msg.data = static_cast(control_lateral_error); + pub_control_lateral_error_->publish(control_msg); + + autoware_debug_msgs::msg::Float32Stamped localization_msg; + localization_msg.stamp = this->now(); + localization_msg.data = static_cast(localization_lateral_error); + pub_localization_lateral_error_->publish(localization_msg); + + autoware_debug_msgs::msg::Float32Stamped sum_msg; + sum_msg.stamp = this->now(); + sum_msg.data = static_cast(lateral_error); + pub_lateral_error_->publish(sum_msg); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(LateralErrorPublisher)