diff --git a/build_depends.repos b/build_depends.repos index 620cba58..69312a4f 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -20,3 +20,16 @@ repositories: type: git url: https://github.com/tier4/autoware_auto_msgs.git version: tier4/main + # universe + universe/autoware.universe: + type: git + url: https://github.com/autowarefoundation/autoware.universe.git + version: main + universe/external/tier4_autoware_msgs: + type: git + url: https://github.com/tier4/tier4_autoware_msgs.git + version: tier4/universe + universe/external/morai_msgs: + type: git + url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git + version: main diff --git a/common/tier4_debug_tools/CMakeLists.txt b/common/tier4_debug_tools/CMakeLists.txt new file mode 100644 index 00000000..609b8737 --- /dev/null +++ b/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}) diff --git a/common/tier4_debug_tools/README.md b/common/tier4_debug_tools/README.md new file mode 100644 index 00000000..606172d1 --- /dev/null +++ b/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 +``` diff --git a/common/tier4_debug_tools/config/lateral_error_publisher.param.yaml b/common/tier4_debug_tools/config/lateral_error_publisher.param.yaml new file mode 100644 index 00000000..ecde03de --- /dev/null +++ b/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] diff --git a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp new file mode 100644 index 00000000..962fee8a --- /dev/null +++ b/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 +#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 // TIER4_DEBUG_TOOLS__LATERAL_ERROR_PUBLISHER_HPP_ diff --git a/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml b/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml new file mode 100644 index 00000000..cd118832 --- /dev/null +++ b/common/tier4_debug_tools/launch/lateral_error_publisher.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/common/tier4_debug_tools/media/lateral_error_publisher.svg b/common/tier4_debug_tools/media/lateral_error_publisher.svg new file mode 100644 index 00000000..9d44bc26 --- /dev/null +++ b/common/tier4_debug_tools/media/lateral_error_publisher.svg @@ -0,0 +1,183 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ ① +
+
+
+ + ① + + + + + + + + + + + +
+
+
+ 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 + + + diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml new file mode 100644 index 00000000..8f03a3b5 --- /dev/null +++ b/common/tier4_debug_tools/package.xml @@ -0,0 +1,31 @@ + + + + tier4_debug_tools + 0.1.0 + The tier4_debug_tools package + Ryohsuke Mitsudome + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_planning_msgs + geometry_msgs + motion_utils + rclcpp + rclcpp_components + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + + launch_ros + python3-rtree + rclpy + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/tier4_debug_tools/scripts/case_converter.py b/common/tier4_debug_tools/scripts/case_converter.py new file mode 100755 index 00000000..2fcc3eeb --- /dev/null +++ b/common/tier4_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/tier4_debug_tools/scripts/pose2tf.py b/common/tier4_debug_tools/scripts/pose2tf.py new file mode 100755 index 00000000..c954e370 --- /dev/null +++ b/common/tier4_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/tier4_debug_tools/scripts/self_pose_listener.py b/common/tier4_debug_tools/scripts/self_pose_listener.py new file mode 100755 index 00000000..0087bfbb --- /dev/null +++ b/common/tier4_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/tier4_debug_tools/scripts/stop_reason2pose.py b/common/tier4_debug_tools/scripts/stop_reason2pose.py new file mode 100755 index 00000000..433f4e75 --- /dev/null +++ b/common/tier4_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 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 +from tier4_planning_msgs.msg import StopReasonArray + + +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 = "/tier4_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 = "/tier4_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 = [StopReason2PoseNode.calc_distance2d(p, self_pose.pose) for p in 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/tier4_debug_tools/scripts/stop_reason2tf b/common/tier4_debug_tools/scripts/stop_reason2tf new file mode 100755 index 00000000..60d59561 --- /dev/null +++ b/common/tier4_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 tier4_debug_tools stop_reason2pose.py /planning/scenario_planning/status/stop_reasons >/dev/null 2>&1 & +ros2 run tier4_debug_tools pose2tf.py /tier4_debug_tools/stop_reason2pose/"${stop_reason_name}" "${stop_reason_name}" >/dev/null 2>&1 & +ros2 run tier4_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/tier4_debug_tools/scripts/tf2pose.py b/common/tier4_debug_tools/scripts/tf2pose.py new file mode 100755 index 00000000..a4422fdd --- /dev/null +++ b/common/tier4_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, "/tier4_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/tier4_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp new file mode 100644 index 00000000..18c97a47 --- /dev/null +++ b/common/tier4_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 "tier4_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 = motion_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 + tier4_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); + + tier4_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); + + tier4_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) diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt new file mode 100644 index 00000000..389bfdaa --- /dev/null +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -0,0 +1,58 @@ +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/processing_time_checker.py + scripts/trajectory_visualizer.py + scripts/closest_velocity_checker.py + scripts/perception_replayer/perception_reproducer.py + scripts/perception_replayer/perception_replayer.py + scripts/update_logger_level.sh + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/planning_debug_tools/README.md b/planning/planning_debug_tools/README.md new file mode 100644 index 00000000..a89cec28 --- /dev/null +++ b/planning/planning_debug_tools/README.md @@ -0,0 +1,283 @@ +# 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 +- **Perception reproducer**: generates detected objects from rosbag data in planning simulator environment +- **processing time checker**: displays processing_time of modules on the terminal +- **logging level updater**: updates the logging level of the planning 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 +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. + +## For other use case (experimental) + +To see behavior velocity planner's internal plath with lane id +add below example value to behavior velocity analyzer and set `is_publish_debug_path: true` + +```lua +crosswalk ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/crosswalk/debug_info' +intersection ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/intersection/debug_info' +traffic_light ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/traffic_light/debug_info' +merge_from_private ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/merge_from_private/debug_info' +occlusion_spot ='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path_with_lane_id/occlusion_spot/debug_info' +``` + +```lua +PlotVelocityOverArclength('v_crosswalk', crosswalk, tracker_time) +PlotVelocityOverArclength('v_intersection', intersection, tracker_time) +PlotVelocityOverArclength('v_merge_from_private', merge_from_private, tracker_time) +PlotVelocityOverArclength('v_traffic_light', traffic_light, tracker_time) +PlotVelocityOverArclength('v_occlusion', occlusion_spot, tracker_time) + +PlotYawOverArclength('yaw_crosswalk', crosswalk, tracker_time) +PlotYawOverArclength('yaw_intersection', intersection, tracker_time) +PlotYawOverArclength('yaw_merge_from_private', merge_from_private, tracker_time) +PlotYawOverArclength('yaw_traffic_light', traffic_light, tracker_time) +PlotYawOverArclength('yaw_occlusion', occlusion_spot, tracker_time) + +PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_state', tracker_time) +``` + +## Perception reproducer + +This script can overlay the perception results from the rosbag on the planning simulator synchronized with the simulator's ego pose. + +In detail, the ego pose in the rosbag which is closest to the current ego pose in the simulator is calculated. +The perception results at the timestamp of the closest ego pose is extracted, and published. + +### How to use + +First, launch the planning simulator, and put the ego pose. +Then, run the script according to the following command. + +By designating a rosbag, perception reproducer can be launched. + +```bash +ros2 run planning_debug_tools perception_reproducer.py -b +``` + +You can designate multiple rosbags in the directory. + +```bash +ros2 run planning_debug_tools perception_reproducer.py -b +``` + +Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. + +## Perception replayer + +A part of the feature is under development. + +This script can overlay the perception results from the rosbag on the planning simulator. + +In detail, this script publishes the data at a certain timestamp from the rosbag. +The timestamp will increase according to the real time without any operation. +By using the GUI, you can modify the timestamp by pausing, changing the rate or going back into the past. + +### How to use + +First, launch the planning simulator, and put the ego pose. +Then, run the script according to the following command. + +By designating a rosbag, perception replayer can be launched. +The GUI is launched as well with which a timestamp of rosbag can be managed. + +```bash +ros2 run planning_debug_tools perception_replayer.py -b +``` + +You can designate multiple rosbags in the directory. + +```bash +ros2 run planning_debug_tools perception_replayer.py -b +``` + +Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. + +## Processing time checker + +The purpose of the Processing Time Subscriber is to monitor and visualize the processing times of various ROS 2 topics in a system. By providing a real-time terminal-based visualization, users can easily confirm the processing time performance as in the picture below. + +![processing_time_checker](image/processing_time_checker.png) + +You can run the program by the following command. + +```bash +ros2 run planning_debug_tools processing_time_checker.py -f -m +``` + +This program subscribes to ROS 2 topics that have a suffix of `processing_time_ms`. + +The program allows users to customize two parameters via command-line arguments: + +- --max_display_time (or -m): This sets the maximum display time in milliseconds. The default value is 150ms. +- --display_frequency (or -f): This sets the frequency at which the terminal UI updates. The default value is 5Hz. + +By adjusting these parameters, users can tailor the display to their specific monitoring needs. + +## Logging Level Updater + +The purpose of the Logging Level Updater is to update the logging level of the planning modules via ROS 2 service. Users can easily update the logging level for debugging. + +```bash +ros2 run planning_debug_tools update_logger_level.sh +``` + +`` will be `DEBUG`, `INFO`, `WARN`, or `ERROR`. + +![logging_level_updater](image/logging_level_updater.png) + +When you have a typo of the planning module, the script will show the available modules. + +![logging_level_updater_typo](image/logging_level_updater_typo.png) diff --git a/planning/planning_debug_tools/doc-stop-reason-visualizer.md b/planning/planning_debug_tools/doc-stop-reason-visualizer.md new file mode 100644 index 00000000..57fb5b30 --- /dev/null +++ b/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) diff --git a/planning/planning_debug_tools/image/add_marker.png b/planning/planning_debug_tools/image/add_marker.png new file mode 100644 index 00000000..f2b9992d Binary files /dev/null and b/planning/planning_debug_tools/image/add_marker.png differ diff --git a/planning/planning_debug_tools/image/closest-velocity-checker.png b/planning/planning_debug_tools/image/closest-velocity-checker.png new file mode 100644 index 00000000..2849648d Binary files /dev/null and b/planning/planning_debug_tools/image/closest-velocity-checker.png differ diff --git a/planning/planning_debug_tools/image/logging_level_updater.png b/planning/planning_debug_tools/image/logging_level_updater.png new file mode 100644 index 00000000..ae911400 Binary files /dev/null and b/planning/planning_debug_tools/image/logging_level_updater.png differ diff --git a/planning/planning_debug_tools/image/logging_level_updater_typo.png b/planning/planning_debug_tools/image/logging_level_updater_typo.png new file mode 100644 index 00000000..8226c0d4 Binary files /dev/null and b/planning/planning_debug_tools/image/logging_level_updater_typo.png differ diff --git a/planning/planning_debug_tools/image/lua.png b/planning/planning_debug_tools/image/lua.png new file mode 100644 index 00000000..7ae0e5e6 Binary files /dev/null and b/planning/planning_debug_tools/image/lua.png differ diff --git a/planning/planning_debug_tools/image/processing_time_checker.png b/planning/planning_debug_tools/image/processing_time_checker.png new file mode 100644 index 00000000..fb064372 Binary files /dev/null and b/planning/planning_debug_tools/image/processing_time_checker.png differ diff --git a/planning/planning_debug_tools/image/script.png b/planning/planning_debug_tools/image/script.png new file mode 100644 index 00000000..5cc8ca98 Binary files /dev/null and b/planning/planning_debug_tools/image/script.png differ diff --git a/planning/planning_debug_tools/image/stop_reason_image.png b/planning/planning_debug_tools/image/stop_reason_image.png new file mode 100644 index 00000000..92c994ce Binary files /dev/null and b/planning/planning_debug_tools/image/stop_reason_image.png differ diff --git a/planning/planning_debug_tools/image/stop_reason_rviz.png b/planning/planning_debug_tools/image/stop_reason_rviz.png new file mode 100644 index 00000000..1e99b959 Binary files /dev/null and b/planning/planning_debug_tools/image/stop_reason_rviz.png differ 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 new file mode 100644 index 00000000..d460cbf0 --- /dev/null +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -0,0 +1,124 @@ +// 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 "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 +#include +#include +#include +#include + +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 +class TrajectoryAnalyzer +{ + using SubscriberType = typename rclcpp::Subscription::SharedPtr; + using PublisherType = rclcpp::Publisher::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(pub_name, 1); + sub_ = node->create_subscription( + 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 node_; + std::string name_; + PublisherType pub_; + SubscriberType sub_; + Odometry::ConstSharedPtr ego_kinematics_; + + template + 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 = motion_utils::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::SharedPtr sub_ego_kinematics_; + void onEgoKinematics(const Odometry::ConstSharedPtr msg); + + std::vector>> path_analyzers_; + std::vector>> path_with_lane_id_analyzers_; + std::vector>> trajectory_analyzers_; +}; + +} // namespace planning_debug_tools + +#endif // PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp new file mode 100644 index 00000000..1f0b9a93 --- /dev/null +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -0,0 +1,136 @@ +// Copyright 2022 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__UTIL_HPP_ +#define PLANNING_DEBUG_TOOLS__UTIL_HPP_ + +#include "motion_utils/trajectory/trajectory.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/geometry/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 + +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 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 +inline std::vector getYawArray(const T & points) +{ + std::vector yaw_arr; + for (const auto & p : points) { + yaw_arr.push_back(getYaw(p)); + } + return yaw_arr; +} +template +inline std::vector getVelocityArray(const T & points) +{ + std::vector v_arr; + for (const auto & p : points) { + v_arr.push_back(getVelocity(p)); + } + return v_arr; +} + +template +inline std::vector getAccelerationArray(const T & points) +{ + std::vector segment_wise_a_arr; + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto & prev_point = points.at(i); + const auto & next_point = points.at(i + 1); + + const double delta_s = tier4_autoware_utils::calcDistance2d(prev_point, next_point); + if (delta_s == 0.0) { + segment_wise_a_arr.push_back(0.0); + } else { + const double prev_vel = getVelocity(prev_point); + const double next_vel = getVelocity(next_point); + + const double acc = (std::pow(next_vel, 2) - std::pow(prev_vel, 2)) / 2.0 / delta_s; + + segment_wise_a_arr.push_back(acc); + } + } + + std::vector point_wise_a_arr; + for (size_t i = 0; i < points.size(); ++i) { + if (i == 0) { + point_wise_a_arr.push_back(segment_wise_a_arr.at(i)); + } else if (i == points.size() - 1 || i == points.size() - 2) { + // Ignore the last two acceleration values which are negative infinity since the path end + // velocity is always 0 by motion_velocity_smoother. NOTE: Path end velocity affects the last + // two acceleration values. + point_wise_a_arr.push_back(0.0); + } else { + point_wise_a_arr.push_back((segment_wise_a_arr.at(i - 1) + segment_wise_a_arr.at(i)) / 2.0); + } + } + + return point_wise_a_arr; +} + +template +std::vector calcPathArcLengthArray(const T & points, const double offset) +{ + std::vector out; + out.push_back(offset); + double sum = offset; + for (size_t i = 1; i < points.size(); ++i) { + sum += calcDistance2d(getPoint(points.at(i)), getPoint(points.at(i - 1))); + out.push_back(sum); + } + return out; +} + +} // namespace planning_debug_tools + +#endif // PLANNING_DEBUG_TOOLS__UTIL_HPP_ diff --git a/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml new file mode 100644 index 00000000..3d5f3f88 --- /dev/null +++ b/planning/planning_debug_tools/launch/behavior_velocity_analyzer.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + diff --git a/planning/planning_debug_tools/launch/stop_reason_visualizer.launch.xml b/planning/planning_debug_tools/launch/stop_reason_visualizer.launch.xml new file mode 100644 index 00000000..d4a92dec --- /dev/null +++ b/planning/planning_debug_tools/launch/stop_reason_visualizer.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/planning_debug_tools/launch/trajectory_analyzer.launch.xml b/planning/planning_debug_tools/launch/trajectory_analyzer.launch.xml new file mode 100644 index 00000000..c525ed0f --- /dev/null +++ b/planning/planning_debug_tools/launch/trajectory_analyzer.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + diff --git a/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg b/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg new file mode 100644 index 00000000..c8d349b2 --- /dev/null +++ b/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg @@ -0,0 +1,7 @@ +builtin_interfaces/Time stamp +uint32 size +float64[] arclength +float64[] curvature +float64[] velocity +float64[] acceleration +float64[] yaw diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml new file mode 100644 index 00000000..14db1277 --- /dev/null +++ b/planning/planning_debug_tools/package.xml @@ -0,0 +1,39 @@ + + + + planning_debug_tools + 0.1.0 + The planning_debug_tools package + Takamasa Horibe + Taiki Tanaka + Takayuki Murooka + Apache License 2.0 + + Takamasa Horibe + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + rosidl_default_generators + + autoware_auto_planning_msgs + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_debug_msgs + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/planning_debug_tools/scripts/closest_velocity_checker.py b/planning/planning_debug_tools/scripts/closest_velocity_checker.py new file mode 100755 index 00000000..4123766a --- /dev/null +++ b/planning/planning_debug_tools/scripts/closest_velocity_checker.py @@ -0,0 +1,351 @@ +#!/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 time + +from autoware_auto_control_msgs.msg import AckermannControlCommand +from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathWithLaneId +from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_vehicle_msgs.msg import Engage +from autoware_auto_vehicle_msgs.msg import VelocityReport +from geometry_msgs.msg import Pose +from nav_msgs.msg import Odometry +import numpy as np +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 +from tier4_debug_msgs.msg import Float32MultiArrayStamped +from tier4_debug_msgs.msg import Float32Stamped +from tier4_planning_msgs.msg import VelocityLimit + +REF_LINK = "map" +SELF_LINK = "base_link" + +LANE_CHANGE = 0 +BEHAVIOR_VELOCITY = 1 +OBSTACLE_AVOID = 2 +OBSTACLE_STOP = 3 +LAT_ACC = 4 +VELOCITY_OPTIMIZE = 5 +ACCELERATION_OPTIMIZE = 6 +CONTROL_CMD = 7 +VEHICLE_CMD = 8 +CONTROL_CMD_ACC = 9 +VEHICLE_CMD_ACC = 10 +DATA_NUM = 11 + + +class VelocityChecker(Node): + def __init__(self): + super().__init__("velocity_checker") + + self.autoware_engage = None + self.vehicle_engage = None + self.external_v_lim = np.nan + self.localization_twist_vx = np.nan + self.distance_to_stopline = np.nan + self.vehicle_twist_vx = np.nan + self.self_pose = Pose() + self.data_arr = [np.nan] * DATA_NUM + self.count = 0 + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # planning path and trajectories + profile = rclpy.qos.QoSProfile(depth=1) + transient_local = rclpy.qos.QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL + transient_local_profile = rclpy.qos.QoSProfile(depth=1, durability=transient_local) + lane_drv = "/planning/scenario_planning/lane_driving" + scenario = "/planning/scenario_planning" + self.sub0 = self.create_subscription( + PathWithLaneId, + lane_drv + "/behavior_planning/path_with_lane_id", + self.CallBackBehaviorPathWLid, + 1, + ) + self.sub1 = self.create_subscription( + Path, lane_drv + "/behavior_planning/path", self.CallBackBehaviorPath, 1 + ) + self.sub2 = self.create_subscription( + Trajectory, + lane_drv + "/motion_planning/obstacle_avoidance_planner/trajectory", + self.CallBackAvoidTrajectory, + 1, + ) + self.sub3 = self.create_subscription( + Trajectory, lane_drv + "/trajectory", self.CallBackLaneDriveTrajectory, 1 + ) + self.sub4 = self.create_subscription( + Trajectory, + scenario + "/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered", + self.CallBackLataccTrajectory, + 1, + ) + self.sub5 = self.create_subscription( + Trajectory, scenario + "/trajectory", self.CallBackScenarioTrajectory, 1 + ) + + # control commands + self.sub6 = self.create_subscription( + AckermannControlCommand, + "/control/trajectory_follower/control_cmd", + self.CallBackControlCmd, + 1, + ) + self.sub7 = self.create_subscription( + AckermannControlCommand, "/control/command/control_cmd", self.CallBackVehicleCmd, 1 + ) + + # others related to velocity + self.sub8 = self.create_subscription( + Engage, "/autoware/engage", self.CallBackAwEngage, profile + ) + self.sub12 = self.create_subscription( + Engage, "/vehicle/engage", self.CallBackVehicleEngage, profile + ) + self.sub9 = self.create_subscription( + VelocityLimit, + "/planning/scenario_planning/current_max_velocity", + self.CallBackExternalVelLim, + transient_local_profile, + ) + + # self twist + self.sub10 = self.create_subscription( + Odometry, "/localization/kinematic_state", self.CallBackLocalizationTwist, 1 + ) + self.sub11 = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.CallBackVehicleTwist, 1 + ) + + # distance_to_stopline + self.pub12 = self.create_subscription( + Float32Stamped, + scenario + "/motion_velocity_smoother/distance_to_stopline", + self.CallBackDistanceToStopline, + 1, + ) + + # publish data + self.pub_v_arr = self.create_publisher(Float32MultiArrayStamped, "closest_speeds", 1) + + time.sleep(1.0) # wait for ready to publish/subscribe + + # for publish traffic signal image + self.create_timer(0.1, self.timerCallback) + + def printInfo(self): + self.count = self.count % 30 + if self.count == 0: + self.get_logger().info("") + self.get_logger().info( + "| Behavior Path | Behavior Vel | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered " + "| Optimized Vel | Optimized Acc | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd " + "| AW Engage | VC Engage | Localization Vel | Vehicle Vel | Distance [m] |" + ) # noqa: E501 + mps2kmph = 3.6 + distance_to_stopline = self.distance_to_stopline + vel_map_lim = self.data_arr[LANE_CHANGE] * mps2kmph + vel_behavior = self.data_arr[BEHAVIOR_VELOCITY] * mps2kmph + vel_obs_avoid = self.data_arr[OBSTACLE_AVOID] * mps2kmph + vel_obs_stop = self.data_arr[OBSTACLE_STOP] * mps2kmph + vel_external_lim = self.external_v_lim * mps2kmph + vel_latacc_filtered = self.data_arr[LAT_ACC] * mps2kmph + vel_optimized = self.data_arr[VELOCITY_OPTIMIZE] * mps2kmph + acc_optimized = self.data_arr[ACCELERATION_OPTIMIZE] + vel_ctrl_cmd = self.data_arr[CONTROL_CMD] * mps2kmph + acc_ctrl_cmd = self.data_arr[CONTROL_CMD_ACC] + vel_vehicle_cmd = self.data_arr[VEHICLE_CMD] * mps2kmph + acc_vehicle_cmd = self.data_arr[VEHICLE_CMD_ACC] + vel_localization = self.localization_twist_vx * mps2kmph + vel_vehicle = self.vehicle_twist_vx * mps2kmph + engage = ( + "None" + if self.autoware_engage is None + else ("True" if self.autoware_engage is True else "False") + ) + vehicle_engage = ( + "None" + if self.vehicle_engage is None + else ("True" if self.vehicle_engage is True else "False") + ) + self.get_logger().info( + "| {0: 13.2f} | {1: 12.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} " + "| {5: 15.2f} | {6: 13.2f} | {7: 13.2f} | {8: 14.2f} | {9: 14.2f} | {10: 14.2f} | {11: 14.2f} " + "| {12:>9s} | {13:>9s} | {14: 16.2f} | {15: 11.2f} | {16: 12.2f} |".format( # noqa: E501 + vel_map_lim, + vel_behavior, + vel_obs_avoid, + vel_obs_stop, + vel_external_lim, + vel_latacc_filtered, + vel_optimized, + acc_optimized, + vel_ctrl_cmd, + acc_ctrl_cmd, + vel_vehicle_cmd, + acc_vehicle_cmd, + engage, + vehicle_engage, + vel_localization, + vel_vehicle, + distance_to_stopline, + ) + ) + self.count += 1 + + def timerCallback(self): + # self.get_logger().info('timer called') + self.updatePose(REF_LINK, SELF_LINK) + self.pub_v_arr.publish(Float32MultiArrayStamped(data=self.data_arr)) + self.printInfo() + + def CallBackAwEngage(self, msg): + self.autoware_engage = msg.engage + + def CallBackVehicleEngage(self, msg): + self.vehicle_engage = msg.engage + + def CallBackExternalVelLim(self, msg): + self.external_v_lim = msg.max_velocity + + def CallBackLocalizationTwist(self, msg): + self.localization_twist_vx = msg.twist.twist.linear.x + + def CallBackVehicleTwist(self, msg): + self.vehicle_twist_vx = msg.longitudinal_velocity + + def CallBackDistanceToStopline(self, msg): + self.distance_to_stopline = msg.data + + def CallBackBehaviorPathWLid(self, msg): + # self.get_logger().info('LANE_CHANGE called') + closest = self.calcClosestPathWLid(msg) + self.data_arr[LANE_CHANGE] = msg.points[closest].point.longitudinal_velocity_mps + return + + def CallBackBehaviorPath(self, msg): + # self.get_logger().info('BEHAVIOR_VELOCITY called') + closest = self.calcClosestPath(msg) + self.data_arr[BEHAVIOR_VELOCITY] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackAvoidTrajectory(self, msg): + # self.get_logger().info('OBSTACLE_AVOID called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[OBSTACLE_AVOID] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackLaneDriveTrajectory(self, msg): + # self.get_logger().info('OBSTACLE_STOP called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[OBSTACLE_STOP] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackLataccTrajectory(self, msg): + # self.get_logger().info('LAT_ACC called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[LAT_ACC] = msg.points[closest].longitudinal_velocity_mps + return + + def CallBackScenarioTrajectory(self, msg): + # self.get_logger().info('VELOCITY_OPTIMIZE called') + closest = self.calcClosestTrajectory(msg) + self.data_arr[VELOCITY_OPTIMIZE] = msg.points[closest].longitudinal_velocity_mps + self.data_arr[ACCELERATION_OPTIMIZE] = msg.points[closest].acceleration_mps2 + return + + def CallBackControlCmd(self, msg): + # self.get_logger().info('CONTROL_CMD called') + self.data_arr[CONTROL_CMD] = msg.longitudinal.speed + self.data_arr[CONTROL_CMD_ACC] = msg.longitudinal.acceleration + return + + def CallBackVehicleCmd(self, msg): + # self.get_logger().info('VEHICLE_CMD called') + self.data_arr[VEHICLE_CMD] = msg.longitudinal.speed + self.data_arr[VEHICLE_CMD_ACC] = msg.longitudinal.acceleration + return + + def calcClosestPath(self, path): + closest = -1 + min_dist_squared = 1.0e10 + for i in range(0, len(path.points)): + dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) + if dist_sq < min_dist_squared: + min_dist_squared = dist_sq + closest = i + return closest + + def calcClosestPathWLid(self, path): + closest = -1 + min_dist_squared = 1.0e10 + for i in range(0, len(path.points)): + dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose) + if dist_sq < min_dist_squared: + min_dist_squared = dist_sq + closest = i + return closest + + def calcClosestTrajectory(self, path): + closest = -1 + min_dist_squared = 1.0e10 + for i in range(0, len(path.points)): + dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) + if dist_sq < min_dist_squared: + min_dist_squared = dist_sq + closest = i + return closest + + def calcSquaredDist2d(self, p1, p2): + dx = p1.position.x - p2.position.x + dy = p1.position.y - p2.position.y + return dx * dx + dy * dy + + def updatePose(self, from_link, to_link): + try: + tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time()) + self.self_pose.position.x = tf.transform.translation.x + self.self_pose.position.y = tf.transform.translation.y + self.self_pose.position.z = tf.transform.translation.z + self.self_pose.orientation.x = tf.transform.rotation.x + self.self_pose.orientation.y = tf.transform.rotation.y + self.self_pose.orientation.z = tf.transform.rotation.z + self.self_pose.orientation.w = tf.transform.rotation.w + return + except LookupException as e: + self.get_logger().warn("No required transformation found: `{}`".format(str(e))) + return + + +def main(args=None): + try: + rclpy.init(args=args) + node = VelocityChecker() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/perception_replayer_common.cpython-310.pyc b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/perception_replayer_common.cpython-310.pyc new file mode 100644 index 00000000..a06184ad Binary files /dev/null and b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/perception_replayer_common.cpython-310.pyc differ diff --git a/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/perception_replayer_utils.cpython-310.pyc b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/perception_replayer_utils.cpython-310.pyc new file mode 100644 index 00000000..9bcf80db Binary files /dev/null and b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/perception_replayer_utils.cpython-310.pyc differ diff --git a/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/time_manager_for_perception_replayer.cpython-310.pyc b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/time_manager_for_perception_replayer.cpython-310.pyc new file mode 100644 index 00000000..5bd21fb4 Binary files /dev/null and b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/time_manager_for_perception_replayer.cpython-310.pyc differ diff --git a/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/time_manager_widget.cpython-310.pyc b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/time_manager_widget.cpython-310.pyc new file mode 100644 index 00000000..adf58e06 Binary files /dev/null and b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/time_manager_widget.cpython-310.pyc differ diff --git a/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/utils.cpython-310.pyc b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/utils.cpython-310.pyc new file mode 100644 index 00000000..0922ff2c Binary files /dev/null and b/planning/planning_debug_tools/scripts/perception_replayer/__pycache__/utils.cpython-310.pyc differ diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py new file mode 100755 index 00000000..498f7e45 --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 copy +import functools +import sys + +from PyQt5.QtWidgets import QApplication +from geometry_msgs.msg import PoseWithCovarianceStamped +from perception_replayer_common import PerceptionReplayerCommon +import rclpy +from time_manager_widget import TimeManagerWidget +from utils import create_empty_pointcloud +from utils import translate_objects_coordinate + + +class PerceptionReplayer(PerceptionReplayerCommon): + def __init__(self, args): + super().__init__(args, "perception_replayer") + + self.bag_timestamp = self.rosbag_objects_data[0][0] + self.is_pause = False + self.rate = 1.0 + self.prev_traffic_signals_msg = None + + # initialize widget + self.widget = TimeManagerWidget( + self.rosbag_objects_data[0][0], self.rosbag_objects_data[-1][0] + ) + self.widget.show() + self.widget.button.clicked.connect(self.onPushed) + for button in self.widget.rate_button: + button.clicked.connect(functools.partial(self.onSetRate, button)) + self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose) + + # start timer callback + self.delta_time = 0.1 + self.timer = self.create_timer(self.delta_time, self.on_timer) + print("Start timer callback") + + def on_timer(self): + timestamp = self.get_clock().now().to_msg() + + self.kill_online_perception_node() + + if self.args.detected_object: + pointcloud_msg = create_empty_pointcloud(timestamp) + self.pointcloud_pub.publish(pointcloud_msg) + + # step timestamp + # get timestamp from slider + self.bag_timestamp = self.rosbag_objects_data[0][ + 0 + ] + self.widget.slider.value() / 1000000 * ( + self.rosbag_objects_data[-1][0] - self.rosbag_objects_data[0][0] + ) + if not self.is_pause: + self.bag_timestamp += self.rate * self.delta_time * 1e9 # seconds to timestamp + # update slider value from the updated timestamp + self.widget.slider.setValue(self.widget.timestamp_to_value(self.bag_timestamp)) + + # extract message by the timestamp + msgs = copy.deepcopy(self.find_topics_by_timestamp(self.bag_timestamp)) + objects_msg = msgs[0] + traffic_signals_msg = msgs[1] + + # objects + if objects_msg: + objects_msg.header.stamp = timestamp + if self.args.detected_object: + if not self.ego_pose: + print("No ego pose found.") + return + + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + log_ego_pose = ego_odom.pose.pose + + translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) + self.objects_pub.publish(objects_msg) + + # traffic signals + # temporary support old auto msgs + if traffic_signals_msg: + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) + self.prev_traffic_signals_msg = traffic_signals_msg + elif self.prev_traffic_signals_msg: + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + + def onPushed(self, event): + if self.widget.button.isChecked(): + self.is_pause = True + else: + self.is_pause = False + + def onSetRate(self, button): + self.rate = float(button.text()) + + def publish_recorded_ego_pose(self): + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + + recorded_ego_pose = PoseWithCovarianceStamped() + recorded_ego_pose.header.stamp = self.get_clock().now().to_msg() + recorded_ego_pose.header.frame_id = "map" + recorded_ego_pose.pose.pose = ego_odom.pose.pose + recorded_ego_pose.pose.covariance = [ + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.06853892326654787, + ] + + self.recorded_ego_pub_as_initialpose.publish(recorded_ego_pose) + print("Published recorded ego pose as /initialpose") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-b", "--bag", help="rosbag", default=None) + parser.add_argument( + "-d", "--detected-object", help="publish detected object", action="store_true" + ) + parser.add_argument( + "-t", "--tracked-object", help="publish tracked object", action="store_true" + ) + parser.add_argument( + "-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3" + ) + args = parser.parse_args() + + app = QApplication(sys.argv) + + rclpy.init() + node = PerceptionReplayer(args) + + try: + while True: + app.processEvents() + rclpy.spin_once(node, timeout_sec=0.01) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py new file mode 100644 index 00000000..7bf54c02 --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 os +from subprocess import CalledProcessError +from subprocess import check_output +import time + +from autoware_auto_perception_msgs.msg import DetectedObjects +from autoware_auto_perception_msgs.msg import PredictedObjects +from autoware_auto_perception_msgs.msg import TrackedObjects +from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray +from autoware_perception_msgs.msg import TrafficSignalArray +from geometry_msgs.msg import PoseWithCovarianceStamped +from nav_msgs.msg import Odometry +import psutil +from rclpy.node import Node +from rclpy.serialization import deserialize_message +from rosbag2_py import StorageFilter +from rosidl_runtime_py.utilities import get_message +from sensor_msgs.msg import PointCloud2 +from utils import open_reader + + +class PerceptionReplayerCommon(Node): + def __init__(self, args, name): + super().__init__(name) + self.args = args + + self.ego_pose = None + self.rosbag_objects_data = [] + self.rosbag_ego_odom_data = [] + self.rosbag_traffic_signals_data = [] + self.is_auto_traffic_signals = False + + # subscriber + self.sub_odom = self.create_subscription( + Odometry, "/localization/kinematic_state", self.on_odom, 1 + ) + + # publisher + if self.args.detected_object: + self.objects_pub = self.create_publisher( + DetectedObjects, "/perception/object_recognition/detection/objects", 1 + ) + elif self.args.tracked_object: + self.objects_pub = self.create_publisher( + TrackedObjects, "/perception/object_recognition/tracking/objects", 1 + ) + else: + self.objects_pub = self.create_publisher( + PredictedObjects, "/perception/object_recognition/objects", 1 + ) + + self.pointcloud_pub = self.create_publisher( + PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 + ) + self.recorded_ego_pub_as_initialpose = self.create_publisher( + PoseWithCovarianceStamped, "/initialpose", 1 + ) + + self.recorded_ego_pub = self.create_publisher( + Odometry, "/perception_reproducer/rosbag_ego_odom", 1 + ) + + # load rosbag + print("Stared loading rosbag") + if os.path.isdir(args.bag): + for bag_file in sorted(os.listdir(args.bag)): + if bag_file.endswith(self.args.rosbag_format): + self.load_rosbag(args.bag + "/" + bag_file) + else: + self.load_rosbag(args.bag) + print("Ended loading rosbag") + + # temporary support old auto msgs + if self.is_auto_traffic_signals: + self.auto_traffic_signals_pub = self.create_publisher( + AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + else: + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + + # wait for ready to publish/subscribe + time.sleep(1.0) + + def on_odom(self, odom): + self.ego_pose = odom.pose.pose + + def load_rosbag(self, rosbag2_path: str): + reader = open_reader(str(rosbag2_path)) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + objects_topic = ( + "/perception/object_recognition/detection/objects" + if self.args.detected_object + else "/perception/object_recognition/tracking/objects" + if self.args.tracked_object + else "/perception/object_recognition/objects" + ) + ego_odom_topic = "/localization/kinematic_state" + traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" + topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic]) + reader.set_filter(topic_filter) + + while reader.has_next(): + (topic, data, stamp) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + if topic == objects_topic: + self.rosbag_objects_data.append((stamp, msg)) + if topic == ego_odom_topic: + self.rosbag_ego_odom_data.append((stamp, msg)) + if topic == traffic_signals_topic: + self.rosbag_traffic_signals_data.append((stamp, msg)) + self.is_auto_traffic_signals = ( + "autoware_auto_perception_msgs" in type(msg).__module__ + ) + + def kill_online_perception_node(self): + # kill node if required + kill_process_name = None + if self.args.detected_object: + kill_process_name = "dummy_perception_publisher_node" + elif self.args.tracked_object: + kill_process_name = "multi_object_tracker" + else: + kill_process_name = "map_based_prediction" + if kill_process_name: + try: + pid = check_output(["pidof", kill_process_name]) + process = psutil.Process(int(pid[:-1])) + process.terminate() + except CalledProcessError: + pass + + def binary_search(self, data, timestamp): + if data[-1][0] < timestamp: + return data[-1][1] + elif data[0][0] > timestamp: + return data[0][1] + + low, high = 0, len(data) - 1 + + while low <= high: + mid = (low + high) // 2 + if data[mid][0] < timestamp: + low = mid + 1 + elif data[mid][0] > timestamp: + high = mid - 1 + else: + return data[mid][1] + + # Return the next timestamp's data if available + if low < len(data): + return data[low][1] + return None + + def find_topics_by_timestamp(self, timestamp): + objects_data = self.binary_search(self.rosbag_objects_data, timestamp) + traffic_signals_data = self.binary_search(self.rosbag_traffic_signals_data, timestamp) + return objects_data, traffic_signals_data + + def find_ego_odom_by_timestamp(self, timestamp): + return self.binary_search(self.rosbag_ego_odom_data, timestamp) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py new file mode 100755 index 00000000..b2b6a3c0 --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 pickle + +import numpy as np +from perception_replayer_common import PerceptionReplayerCommon +import rclpy +from utils import StopWatch +from utils import create_empty_pointcloud +from utils import translate_objects_coordinate + + +class PerceptionReproducer(PerceptionReplayerCommon): + def __init__(self, args): + super().__init__(args, "perception_reproducer") + + self.prev_traffic_signals_msg = None + self.stopwatch = StopWatch(self.args.verbose) # for debug + + # to make some data to accelerate computation + self.preprocess_data() + + # start main timer callback + self.timer = self.create_timer(0.1, self.on_timer) + + # kill perception process to avoid a conflict of the perception topics + self.timer_check_perception_process = self.create_timer(3.0, self.on_timer_kill_perception) + + print("Start timer callback") + + def preprocess_data(self): + # closest search with numpy data is much faster than usual + self.rosbag_ego_odom_data_numpy = np.array( + [ + [data[1].pose.pose.position.x, data[1].pose.pose.position.y] + for data in self.rosbag_ego_odom_data + ] + ) + + def on_timer_kill_perception(self): + self.kill_online_perception_node() + + def on_timer(self): + if self.args.verbose: + print("\n-- on_timer start --") + self.stopwatch.tic("total on_timer") + + timestamp = self.get_clock().now().to_msg() + + if self.args.detected_object: + pointcloud_msg = create_empty_pointcloud(timestamp) + self.pointcloud_pub.publish(pointcloud_msg) + + if not self.ego_pose: + print("No ego pose found.") + return + + # find nearest ego odom by simulation observation + self.stopwatch.tic("find_nearest_ego_odom_by_observation") + ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose) + pose_timestamp = ego_odom[0] + log_ego_pose = ego_odom[1].pose.pose + self.stopwatch.toc("find_nearest_ego_odom_by_observation") + + # extract message by the nearest ego odom timestamp + self.stopwatch.tic("find_topics_by_timestamp") + msgs_orig = self.find_topics_by_timestamp(pose_timestamp) + self.stopwatch.toc("find_topics_by_timestamp") + + # copy the messages + self.stopwatch.tic("message deepcopy") + if self.args.detected_object: + msgs = pickle.loads(pickle.dumps(msgs_orig)) # this is x5 faster than deepcopy + objects_msg = msgs[0] + traffic_signals_msg = msgs[1] + else: + # NOTE: No need to deepcopy since only timestamp will be changed and it will be changed every time correctly. + objects_msg = msgs_orig[0] + traffic_signals_msg = msgs_orig[1] + self.stopwatch.toc("message deepcopy") + + self.stopwatch.tic("transform and publish") + # objects + if objects_msg: + objects_msg.header.stamp = timestamp + if self.args.detected_object: + translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) + self.objects_pub.publish(objects_msg) + + # ego odom + self.recorded_ego_pub.publish(ego_odom[1]) + + # traffic signals + # temporary support old auto msgs + if traffic_signals_msg: + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) + self.prev_traffic_signals_msg = traffic_signals_msg + elif self.prev_traffic_signals_msg: + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + self.stopwatch.toc("transform and publish") + + self.stopwatch.toc("total on_timer") + + def find_nearest_ego_odom_by_observation(self, ego_pose): + # nearest search with numpy format is much (~ x100) faster than regular for loop + self_pose = np.array([ego_pose.position.x, ego_pose.position.y]) + dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1) + nearest_idx = np.argmin(dists_squared) + + return self.rosbag_ego_odom_data[nearest_idx] + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-b", "--bag", help="rosbag", default=None) + parser.add_argument( + "-d", "--detected-object", help="publish detected object", action="store_true" + ) + parser.add_argument( + "-t", "--tracked-object", help="publish tracked object", action="store_true" + ) + parser.add_argument( + "-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3" + ) + parser.add_argument( + "-v", "--verbose", help="output debug data", action="store_true", default=False + ) + args = parser.parse_args() + + rclpy.init() + node = PerceptionReproducer(args) + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py new file mode 100644 index 00000000..a1d87a8b --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 PyQt5 import QtCore +from PyQt5.QtWidgets import QGridLayout +from PyQt5.QtWidgets import QMainWindow +from PyQt5.QtWidgets import QPushButton +from PyQt5.QtWidgets import QSizePolicy +from PyQt5.QtWidgets import QSlider +from PyQt5.QtWidgets import QWidget + + +# With QSlider, the slider's handle cannot be captured if the mouse cursor is not the handle position when pressing the mouse. +class QJumpSlider(QSlider): + def __init__(self, slider_direction, max_value): + super(self.__class__, self).__init__(slider_direction) + + self.max_value = max_value + self.is_mouse_pressed = False + + def mouse_to_value(self, event): + x = event.pos().x() + return int(self.max_value * x / self.width()) + + def mousePressEvent(self, event): + super(self.__class__, self).mousePressEvent(event) + + if event.button() == QtCore.Qt.LeftButton: + self.setValue(self.mouse_to_value(event)) + self.is_mouse_pressed = True + + def mouseMoveEvent(self, event): + super(self.__class__, self).mouseMoveEvent(event) + if self.is_mouse_pressed: + self.setValue(self.mouse_to_value(event)) + + def mouseReleaseEvent(self, event): + super(self.__class__, self).mouseReleaseEvent(event) + + if event.button() == QtCore.Qt.LeftButton: + self.is_mouse_pressed = False + + +class TimeManagerWidget(QMainWindow): + def __init__(self, start_timestamp, end_timestamp): + super(self.__class__, self).__init__() + + self.start_timestamp = start_timestamp + self.end_timestamp = end_timestamp + self.max_value = 1000000 + + self.setupUI() + + def setupUI(self): + self.setObjectName("PerceptionReplayer") + self.resize(480, 120) + self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) + + self.central_widget = QWidget(self) + self.central_widget.setObjectName("central_widget") + + self.grid_layout = QGridLayout(self.central_widget) + self.grid_layout.setContentsMargins(10, 10, 10, 10) + self.grid_layout.setObjectName("grid_layout") + + # rate button + self.rate_button = [] + for i, rate in enumerate([0.1, 0.5, 1.0, 2.0, 5.0, 10.0]): + button = QPushButton(str(rate)) + button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.rate_button.append(button) + self.grid_layout.addWidget(self.rate_button[-1], 0, i, 1, 1) + + # pause button + self.button = QPushButton("pause") + self.button.setCheckable(True) + self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.grid_layout.addWidget(self.button, 1, 0, 1, -1) + self.pub_recorded_ego_pose_button = QPushButton("publish recorded ego pose") + self.grid_layout.addWidget(self.pub_recorded_ego_pose_button, 2, 0, 1, -1) + + # slider + self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) + self.slider.setMinimum(0) + self.slider.setMaximum(self.max_value) + self.slider.setValue(0) + self.grid_layout.addWidget(self.slider, 3, 0, 1, -1) + + self.setCentralWidget(self.central_widget) + + def timestamp_to_value(self, timestamp): + return int( + (timestamp - self.start_timestamp) + / (self.end_timestamp - self.start_timestamp) + * self.max_value + ) + + def value_to_timestamp(self, value): + return self.start_timestamp + self.slider.value() / self.max_value * ( + self.end_timestamp - self.start_timestamp + ) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py new file mode 100644 index 00000000..5ded8d2e --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 math +import time + +from geometry_msgs.msg import Quaternion +import numpy as np +import rosbag2_py +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from tf_transformations import euler_from_quaternion +from tf_transformations import quaternion_from_euler + + +def get_rosbag_options(path, serialization_format="cdr"): + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format, + ) + + return storage_options, converter_options + + +def open_reader(path: str): + storage_options, converter_options = get_rosbag_options(path) + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + return reader + + +def calc_squared_distance(p1, p2): + return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2) + + +def create_empty_pointcloud(timestamp): + pointcloud_msg = PointCloud2() + pointcloud_msg.header.stamp = timestamp + pointcloud_msg.header.frame_id = "map" + pointcloud_msg.height = 1 + pointcloud_msg.is_dense = True + pointcloud_msg.point_step = 16 + field_name_vec = ["x", "y", "z"] + offset_vec = [0, 4, 8] + for field_name, offset in zip(field_name_vec, offset_vec): + field = PointField() + field.name = field_name + field.offset = offset + field.datatype = 7 + field.count = 1 + pointcloud_msg.fields.append(field) + return pointcloud_msg + + +def get_yaw_from_quaternion(orientation): + orientation_list = [ + orientation.x, + orientation.y, + orientation.z, + orientation.w, + ] + return euler_from_quaternion(orientation_list)[2] + + +def get_quaternion_from_yaw(yaw): + q = quaternion_from_euler(0, 0, yaw) + orientation = Quaternion() + orientation.x = q[0] + orientation.y = q[1] + orientation.z = q[2] + orientation.w = q[3] + return orientation + + +def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg): + log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) + log_ego_pose_trans_mat = np.array( + [ + [ + math.cos(log_ego_yaw), + -math.sin(log_ego_yaw), + log_ego_pose.position.x, + ], + [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + ego_yaw = get_yaw_from_quaternion(ego_pose.orientation) + ego_pose_trans_mat = np.array( + [ + [math.cos(ego_yaw), -math.sin(ego_yaw), ego_pose.position.x], + [math.sin(ego_yaw), math.cos(ego_yaw), ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + for o in objects_msg.objects: + log_object_pose = o.kinematics.pose_with_covariance.pose + log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) + log_object_pos_vec = np.array([log_object_pose.position.x, log_object_pose.position.y, 1.0]) + + # translate object pose from ego pose in log to ego pose in simulation + object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( + log_ego_pose_trans_mat.dot(log_object_pos_vec.T) + ) + + object_pose = o.kinematics.pose_with_covariance.pose + object_pose.position.x = object_pos_vec[0] + object_pose.position.y = object_pos_vec[1] + object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw) + + +class StopWatch: + def __init__(self, verbose): + # A dictionary to store the starting times + self.start_times = {} + self.verbose = verbose + + def tic(self, name): + """Store the current time with the given name.""" + self.start_times[name] = time.perf_counter() + + def toc(self, name): + """Print the elapsed time since the last call to tic() with the same name.""" + if name not in self.start_times: + print(f"No start time found for {name}!") + return + + elapsed_time = ( + time.perf_counter() - self.start_times[name] + ) * 1000 # Convert to milliseconds + if self.verbose: + print(f"Time for {name}: {elapsed_time:.2f} ms") + + # Reset the starting time for the name + del self.start_times[name] diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py new file mode 100755 index 00000000..39e94ebc --- /dev/null +++ b/planning/planning_debug_tools/scripts/processing_time_checker.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 +from collections import deque +import os +import sys + +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float64Stamped + + +class ProcessingTimeSubscriber(Node): + def __init__( + self, + max_display_time=150, + display_frequency=5.0, + window_size=1, + bar_scale=2, + display_character="|", + ): + super().__init__("processing_time_subscriber") + self.data_map = {} # {topic_name: data_value} + self.data_queue_map = {} # {topic_name: deque} + self.window_size = window_size + self.max_display_time = max_display_time + self.bar_scale = bar_scale + self.display_character = display_character + + # Initialize a set to keep track of subscribed topics + self.subscribed_topics = set() + + # Call get_topic_list immediately and set a timer to call it regularly + self.get_topic_list() + self.create_timer(1.0, self.get_topic_list) # update topic list every 1 second + + # Timer to print data at given frequency + self.create_timer(1.0 / display_frequency, self.print_data) + + def get_topic_list(self): + # Get list of existing topics in the system + topic_list = self.get_topic_names_and_types() + + # Subscribe to topics with 'processing_time_ms' suffix + for topic, types in topic_list: + if topic.endswith("processing_time_ms") and topic not in self.subscribed_topics: + self.create_subscription( + Float64Stamped, topic, lambda msg, topic=topic: self.callback(msg, topic), 1 + ) + self.get_logger().info(f"Subscribed to {topic} | Type: {types}") + self.subscribed_topics.add(topic) + + def callback(self, msg, topic): + # Add the new data to the queue + if topic not in self.data_queue_map: + self.data_queue_map[topic] = deque(maxlen=self.window_size) + self.data_queue_map[topic].append(msg.data) + + # Calculate the average + avg_value = sum(self.data_queue_map[topic]) / len(self.data_queue_map[topic]) + self.data_map[topic] = avg_value + + def print_data(self): + # Clear terminal + os.system("cls" if os.name == "nt" else "clear") + + if not self.data_map: + print("No topics available.") + return + + # Get the maximum topic name length for alignment + max_topic_length = max(map(len, self.data_map.keys())) + + # Generate the header based on the max_display_time + header_str = f"topics (window size = {self.window_size})".ljust(max_topic_length) + ":" + for i in range(0, self.max_display_time + 1, 10): + header_str += f" {i}ms".ljust(self.bar_scale * 10) + + # Print the header + print(header_str) + print("-" * len(header_str)) + + # Print each topic's data + for topic in sorted(self.data_map.keys()): + # Fetch the data for the current topic + data = self.data_map[topic] + # Round the data to the third decimal place for display + data_rounded = round(data, 3) + # Calculate the number of bars to be displayed (clamped to max_display_time) + num_bars = ( + int(min(data * self.bar_scale, self.max_display_time * self.bar_scale - 1)) + 1 + ) # Convert bar_scale's reciprocal to milliseconds + print( + f"{topic.ljust(max_topic_length)}: {self.display_character * num_bars} [{data_rounded}ms]" + ) + + +def main(args=None): + # Get the command line arguments before argparse processes them + cmd_args = sys.argv[1:] + + parser = argparse.ArgumentParser(description="Processing Time Subscriber Parameters") + parser.add_argument( + "-m", "--max_display_time", type=int, default=50, help="Maximum display time in ms" + ) + parser.add_argument( + "-f", "--display_frequency", type=float, default=5.0, help="Display frequency in Hz" + ) + parser.add_argument( + "-w", "--window_size", type=int, default=5, help="Number of samples to average" + ) + parser.add_argument( + "-bs", + "--bar_scale", + type=int, + default=2, + help="Number of bars per second. Default is 2 bars per second.", + ) + parser.add_argument( + "-dc", + "--display_character", + type=str, + default="|", + help="Character to use for the display. Default is '|'.", + ) + args = parser.parse_args() + + rclpy.init(args=cmd_args) # Use the original command line arguments here + subscriber = ProcessingTimeSubscriber( + max_display_time=args.max_display_time, + display_frequency=args.display_frequency, + window_size=args.window_size, + bar_scale=args.bar_scale, + display_character=args.display_character, + ) + rclpy.spin(subscriber) + subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py new file mode 100755 index 00000000..10bd41c5 --- /dev/null +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -0,0 +1,583 @@ +#!/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 + +from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathPoint +from autoware_auto_planning_msgs.msg import PathPointWithLaneId +from autoware_auto_planning_msgs.msg import PathWithLaneId +from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_planning_msgs.msg import TrajectoryPoint +from autoware_auto_vehicle_msgs.msg import VelocityReport +from geometry_msgs.msg import Pose +from matplotlib import animation +import matplotlib.pyplot as plt +import message_filters +from nav_msgs.msg import Odometry +import numpy as np +import rclpy +from rclpy.node import Node +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tier4_planning_msgs.msg import VelocityLimit + +parser = argparse.ArgumentParser() +parser.add_argument("-l", "--length", help="max arclength in plot") +parser.add_argument( + "-t", + "--type", + help="Options VEL(default): show velocity only, VEL_ACC_JERK: show vel & acc & jerk", +) + +parser.add_argument( + "-v", + "--max-velocity", + type=int, + help="maximum plotting velocity in Matplotlib", +) + +args = parser.parse_args() + +PLOT_MIN_ARCLENGTH = -5 + +if args.length is None: + PLOT_MAX_ARCLENGTH = 200 +else: + PLOT_MAX_ARCLENGTH = int(args.length) +print("max arclength = " + str(PLOT_MAX_ARCLENGTH)) + +if args.type is None: + PLOT_TYPE = "VEL" +elif args.type == "VEL": + PLOT_TYPE = "VEL" +elif args.type == "VEL_ACC_JERK": + PLOT_TYPE = "VEL_ACC_JERK" +else: + print("invalid type. set default VEL.") + PLOT_TYPE = "VEL" +print("plot type = " + PLOT_TYPE) + +if args.max_velocity is None: + MAX_VELOCITY = 20 +else: + MAX_VELOCITY = args.max_velocity + + +class TrajectoryVisualizer(Node): + def __init__(self): + super().__init__("trajectory_visualizer") + + self.fig = plt.figure() + + self.max_vel = 0.0 + self.min_vel = 0.0 + self.min_accel = 0.0 + self.max_accel = 0.0 + self.min_jerk = 0.0 + self.max_jerk = 0.0 + + # update flag + self.update_ex_vel_lim = False + self.update_lat_acc_fil = False + self.update_steer_rate_fil = False + self.update_traj_raw = False + self.update_traj_resample = False + self.update_traj_final = False + self.update_behavior_path_planner_path = False + self.update_behavior_velocity_planner_path = False + self.update_traj_ob_avoid = False + self.update_traj_ob_stop = False + + self.tf_buffer = Buffer(node=self) + self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True) + + self.self_pose = Pose() + self.self_pose_received = False + self.localization_vx = 0.0 + self.vehicle_vx = 0.0 + self.velocity_limit = None + + self.trajectory_external_velocity_limited = Trajectory() + self.trajectory_lateral_acc_filtered = Trajectory() + self.trajectory_steer_rate_filtered = Trajectory() + self.trajectory_raw = Trajectory() + self.trajectory_time_resampled = Trajectory() + self.trajectory_final = Trajectory() + + self.behavior_path_planner_path = PathWithLaneId() + self.behavior_velocity_planner_path = Path() + self.obstacle_avoid_traj = Trajectory() + self.obstacle_stop_traj = Trajectory() + + self.plotted = [False] * 9 + self.sub_localization_kinematics = self.create_subscription( + Odometry, "/localization/kinematic_state", self.CallbackLocalizationTwist, 1 + ) + self.sub_vehicle_twist = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.CallbackVehicleTwist, 1 + ) + + self.sub_external_velocity_limit = self.create_subscription( + VelocityLimit, "/planning/scenario_planning/max_velocity", self.CallbackVelocityLimit, 1 + ) + + # BUFFER_SIZE = 65536*100 + optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/" + self.sub1 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_external_velocity_limited" + ) + self.sub2 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_lateral_acc_filtered" + ) + self.sub3 = message_filters.Subscriber(self, Trajectory, optimizer_debug + "trajectory_raw") + self.sub4 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_time_resampled" + ) + self.sub41 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_steering_rate_limited" + ) + self.sub5 = message_filters.Subscriber( + self, Trajectory, "/planning/scenario_planning/trajectory" + ) + + lane_driving = "/planning/scenario_planning/lane_driving" + self.sub6 = message_filters.Subscriber( + self, PathWithLaneId, lane_driving + "/behavior_planning/path_with_lane_id" + ) + self.sub7 = message_filters.Subscriber(self, Path, lane_driving + "/behavior_planning/path") + self.sub8 = message_filters.Subscriber( + self, + Trajectory, + lane_driving + "/motion_planning/obstacle_avoidance_planner/trajectory", + ) + self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory") + + self.ts1 = message_filters.ApproximateTimeSynchronizer( + [self.sub1, self.sub2, self.sub3, self.sub4, self.sub41], 30, 0.5 + ) + self.ts1.registerCallback(self.CallbackMotionVelOptTraj) + + self.ts2 = message_filters.ApproximateTimeSynchronizer( + [self.sub5, self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0 + ) + self.ts2.registerCallback(self.CallBackLaneDrivingTraj) + + # main process + if PLOT_TYPE == "VEL_ACC_JERK": + self.setPlotTrajectory() + self.ani = animation.FuncAnimation( + self.fig, self.plotTrajectory, interval=100, blit=True + ) + else: + self.setPlotTrajectoryVelocity() + self.ani = animation.FuncAnimation( + self.fig, self.plotTrajectoryVelocity, interval=100, blit=True + ) + + plt.show() + + return + + def CallbackLocalizationTwist(self, cmd): + self.localization_vx = cmd.twist.twist.linear.x + self.self_pose = cmd.pose.pose + self.self_pose_received = True + + def CallbackVehicleTwist(self, cmd): + self.vehicle_vx = cmd.longitudinal_velocity + + def CallbackVelocityLimit(self, cmd): + self.velocity_limit = cmd.max_velocity + + def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): + self.get_logger().info("CallbackMotionVelOptTraj called") + self.CallBackTrajExVelLim(cmd1) + self.CallBackTrajLatAccFiltered(cmd2) + self.CallBackTrajRaw(cmd3) + self.CallBackTrajTimeResampled(cmd4) + self.CallBackTrajSteerRateFiltered(cmd5) + + def CallBackTrajExVelLim(self, cmd): + self.trajectory_external_velocity_limited = cmd + self.update_ex_vel_lim = True + + def CallBackTrajLatAccFiltered(self, cmd): + self.trajectory_lateral_acc_filtered = cmd + self.update_lat_acc_fil = True + + def CallBackTrajSteerRateFiltered(self, cmd): + self.trajectory_steer_rate_filtered = cmd + self.update_steer_rate_fil = True + + def CallBackTrajRaw(self, cmd): + self.trajectory_raw = cmd + self.update_traj_raw = True + + def CallBackTrajTimeResampled(self, cmd): + self.trajectory_time_resampled = cmd + self.update_traj_resample = True + + def CallBackTrajFinal(self, cmd): + self.trajectory_final = cmd + self.update_traj_final = True + + def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9): + self.get_logger().info("CallBackLaneDrivingTraj called") + self.CallBackTrajFinal(cmd5) + self.CallBackLBehaviorPathPlannerPath(cmd6) + self.CallBackBehaviorVelocityPlannerPath(cmd7) + self.CallbackObstacleAvoidTraj(cmd8) + self.CallbackObstacleStopTraj(cmd9) + + def CallBackLBehaviorPathPlannerPath(self, cmd): + self.behavior_path_planner_path = cmd + self.update_behavior_path_planner_path = True + + def CallBackBehaviorVelocityPlannerPath(self, cmd): + self.behavior_velocity_planner_path = cmd + self.update_behavior_velocity_planner_path = True + + def CallbackObstacleAvoidTraj(self, cmd): + self.obstacle_avoid_traj = cmd + self.update_traj_ob_avoid = True + + def CallbackObstacleStopTraj(self, cmd): + self.obstacle_stop_traj = cmd + self.update_traj_ob_stop = True + + def setPlotTrajectoryVelocity(self): + self.ax1 = plt.subplot(1, 1, 1) # row, col, index( 0: + a_arr.append(0) + a_arr.append(0) + return a_arr + + def CalcJerk(self, traj): + j_arr = [] + for i in range(1, len(traj.points) - 2): + p0 = traj.points[i - 1] + p1 = traj.points[i] + p2 = traj.points[i + 1] + v0 = p0.longitudinal_velocity_mps + v1 = p1.longitudinal_velocity_mps + v2 = p2.longitudinal_velocity_mps + + dx0 = p1.pose.position.x - p0.pose.position.x + dy0 = p1.pose.position.y - p0.pose.position.y + ds0 = np.sqrt(dx0**2 + dy0**2) + + dx1 = p2.pose.position.x - p1.pose.position.x + dy1 = p2.pose.position.y - p1.pose.position.y + ds1 = np.sqrt(dx1**2 + dy1**2) + + dt0 = ds0 / max(abs(0.5 * (v1 + v0)), 0.001) + dt1 = ds1 / max(abs(0.5 * (v2 + v1)), 0.001) + + a0 = (v1 - v0) / max(dt0, 0.001) + a1 = (v2 - v1) / max(dt1, 0.001) + j = (a1 - a0) / max(dt1, 0.001) + j_arr.append(j) + if len(traj.points) > 0: + j_arr.append(0) + j_arr.append(0) + j_arr.append(0) + return j_arr + + def setPlotTrajectory(self): + self.ax1 = plt.subplot(3, 1, 1) # row, col, index( +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace planning_debug_tools +{ +using std::placeholders::_1; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +class StopReasonVisualizerNode : public rclcpp::Node +{ +public: + explicit StopReasonVisualizerNode(const rclcpp::NodeOptions & options) + : Node("stop_reason_visualizer", options) + { + pub_stop_reasons_marker_ = create_publisher("~/debug/markers", 1); + sub_stop_reasons_ = create_subscription( + "/planning/scenario_planning/status/stop_reasons", rclcpp::QoS{1}, + std::bind(&StopReasonVisualizerNode::onStopReasonArray, this, _1)); + } + +private: + void onStopReasonArray(const StopReasonArray::ConstSharedPtr msg) + { + using tier4_autoware_utils::appendMarkerArray; + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + + MarkerArray all_marker_array; + const auto header = msg->header; + const double offset_z = 1.0; + for (auto stop_reason : msg->stop_reasons) { + std::string reason = stop_reason.reason; + if (reason.empty()) continue; + const auto current_time = this->now(); + int id = 0; + MarkerArray marker_array; + for (auto stop_factor : stop_reason.stop_factors) { + if (stop_factor.stop_factor_points.empty()) continue; + std::string prefix = reason + "[" + std::to_string(id) + "]"; + const auto stop_factor_point = stop_factor.stop_factor_points.front(); + // base stop pose marker + { + auto stop_point_marker = createDefaultMarker( + "map", current_time, prefix + ":stop_factor_point", id, Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + stop_point_marker.pose.position = stop_factor_point; + marker_array.markers.emplace_back(stop_point_marker); + } + // attention ! marker + { + auto attention_text_marker = createDefaultMarker( + "map", current_time, prefix + ":attention text", id, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + attention_text_marker.pose.position = stop_factor_point; + attention_text_marker.pose.position.z += offset_z; + attention_text_marker.text = "!"; + marker_array.markers.emplace_back(attention_text_marker); + } + // point to pose + { + auto stop_to_pose_marker = createDefaultMarker( + "map", current_time, prefix + ":stop_to_pose", id, Marker::LINE_STRIP, + createMarkerScale(0.02, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + stop_to_pose_marker.points.emplace_back(stop_factor.stop_factor_points.front()); + stop_to_pose_marker.points.emplace_back(stop_factor.stop_pose.position); + marker_array.markers.emplace_back(stop_to_pose_marker); + } + // point to pose + { + auto stop_pose_marker = createDefaultMarker( + "map", current_time, prefix + ":stop_pose", id, Marker::ARROW, + createMarkerScale(0.4, 0.2, 0.2), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + stop_pose_marker.pose = stop_factor.stop_pose; + marker_array.markers.emplace_back(stop_pose_marker); + } + // add view distance text + { + auto reason_text_marker = createDefaultMarker( + "map", current_time, prefix + ":reason", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.2), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + reason_text_marker.pose = stop_factor.stop_pose; + reason_text_marker.text = prefix; + marker_array.markers.emplace_back(reason_text_marker); + } + id++; + } + if (!marker_array.markers.empty()) + appendMarkerArray(marker_array, &all_marker_array, current_time); + } + pub_stop_reasons_marker_->publish(all_marker_array); + } + rclcpp::Publisher::SharedPtr pub_stop_reasons_marker_; + rclcpp::Subscription::SharedPtr sub_stop_reasons_; +}; +} // namespace planning_debug_tools + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(planning_debug_tools::StopReasonVisualizerNode) diff --git a/planning/planning_debug_tools/src/trajectory_analyzer.cpp b/planning/planning_debug_tools/src/trajectory_analyzer.cpp new file mode 100644 index 00000000..2dfcf940 --- /dev/null +++ b/planning/planning_debug_tools/src/trajectory_analyzer.cpp @@ -0,0 +1,63 @@ +// 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 "planning_debug_tools/trajectory_analyzer.hpp" + +namespace planning_debug_tools +{ +TrajectoryAnalyzerNode::TrajectoryAnalyzerNode(const rclcpp::NodeOptions & options) +: Node("trajectory_analyzer", options) +{ + using TopicNames = std::vector; + const auto path_topics = declare_parameter("path_topics"); + const auto path_with_lane_id_topics = declare_parameter("path_with_lane_id_topics"); + const auto trajectory_topics = declare_parameter("trajectory_topics"); + + for (const auto & s : path_topics) { + path_analyzers_.push_back(std::make_shared>(this, s)); + RCLCPP_INFO(get_logger(), "path_topics: %s", s.c_str()); + } + for (const auto & s : path_with_lane_id_topics) { + path_with_lane_id_analyzers_.push_back( + std::make_shared>(this, s)); + RCLCPP_INFO(get_logger(), "path_with_lane_id_topics: %s", s.c_str()); + } + + for (const auto & s : trajectory_topics) { + trajectory_analyzers_.push_back(std::make_shared>(this, s)); + RCLCPP_INFO(get_logger(), "trajectory_topics: %s", s.c_str()); + } + + using std::placeholders::_1; + sub_ego_kinematics_ = create_subscription( + "ego_kinematics", 1, std::bind(&TrajectoryAnalyzerNode::onEgoKinematics, this, _1)); +} + +void TrajectoryAnalyzerNode::onEgoKinematics(const Odometry::ConstSharedPtr msg) +{ + for (auto & a : path_analyzers_) { + a->setKinematics(msg); + } + for (auto & a : path_with_lane_id_analyzers_) { + a->setKinematics(msg); + } + for (auto & a : trajectory_analyzers_) { + a->setKinematics(msg); + } +} + +} // namespace planning_debug_tools + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(planning_debug_tools::TrajectoryAnalyzerNode) diff --git a/simulator/simulator_compatibility_test/.gitignore b/simulator/simulator_compatibility_test/.gitignore new file mode 100644 index 00000000..7b91e388 --- /dev/null +++ b/simulator/simulator_compatibility_test/.gitignore @@ -0,0 +1,8 @@ +simulator_compatibility_test/*pycache*/ +simulator_compatibility_test/clients/*pycache*/ +simulator_compatibility_test/clients/moraisim/*pycache*/ +simulator_compatibility_test/publishers/*pycache*/ +simulator_compatibility_test/subscribers/*pycache*/ +test_base/*pycache*/ +test_sim/*pycache*/ +test_moraisim/*pycache*/ diff --git a/simulator/simulator_compatibility_test/README.md b/simulator/simulator_compatibility_test/README.md new file mode 100644 index 00000000..5dc9d233 --- /dev/null +++ b/simulator/simulator_compatibility_test/README.md @@ -0,0 +1,181 @@ +# simulator_compatibility_test + +## Purpose + +Test procedures (e.g. test codes) to check whether a certain simulator is compatible with Autoware + +## Overview of the test codes + +File structure + +- test_base +- test_sim_common_manual_testing +- test_morai_sim + +1. test_base provides shared methods for testing. Other test codes are created based on functions defined here. +2. test_sim_common_manual_testing provides the most basic functions. Any simulator can be tested using codes here. However, to make these codes usable with any simulators, the codes do not include any features for test automation. +3. test_morai_sim is an automated version of test_sim_common_manual_testing for MORAI SIM: Drive. Thus it includes 'MORAI SIM: Drive'-specific codes. Users of the other simulators may create similar version for their simulator of interest. + +## Test Procedures for test_sim_common_manual_testing + +### Build process before test + +```bash +source install/setup.bash +colcon build --packages-select simulator_compatibility_test +cd src/universe/autoware.universe/tools/simulator_test/simulator_compatibility_test/test_sim_common_manual_testing +``` + +To run each test case manually + +### Test Case #1 + +1. Run your simulator +2. Load a map and an ego vehicle for the test +3. Run the test using the following command + + ```bash + python -m pytest test_01_control_mode_and_report.py + ``` + +4. Check if expected behavior is created within the simulator + - Ego vehicle control mode is changed into Manual (If the simulator has a GUI for this one, it should display the ego is in Manual) + - Ego vehicle control mode is changed into Auto (If the simulator has a GUI for this one, it should display the ego is in Auto) +5. Check if pytest output is passed or failure + +### Test Case #2 + +1. Run your simulator (If the simulator is already running, skip this part) +2. Load a map and an ego vehicle for the test (If a map and an ego are loaded already, skip this part) +3. Run the test using the following command + + ```bash + python -m pytest test_02_change_gear_and_report.py + ``` + +4. Check if expected behavior is created within the simulator + - Ego vehicle gear mode is changed into "P" (If the simulator has a GUI for this one, it should display the gear mode is in "P") + - Ego vehicle gear mode is changed into "N" (If the simulator has a GUI for this one, it should display the gear mode is in "N") + - Ego vehicle gear mode is changed into "R" (If the simulator has a GUI for this one, it should display the gear mode is in "R") + - Ego vehicle gear mode is changed into "D" (If the simulator has a GUI for this one, it should display the gear mode is in "D") +5. Check if pytest output is passed or failure + +### Test Case #3 + +1. Run your simulator (If the simulator is already running, skip this part) +2. Load a map and an ego vehicle for the test (If a map and an ego are loaded already, skip this part) +3. Run the test using the following command + + ```bash + python -m pytest test_03_longitudinal_command_and_report.py + ``` + +4. Check if expected behavior is created within the simulator + - Ego vehicle longitudinal velocity is greater than 10 kph (If the simulator has a GUI for this one, it should display the longitudinal velocity is greater than 10 kph) + - Ego vehicle longitudinal velocity is going below 10 kph. This is an ego vehicle initialize process to ensure the following acceleration is made by longitudinal.acceleration value (If the simulator has a GUI for this one, it should display the longitudinal velocity is less than 10 kph) + - Ego vehicle longitudinal velocity is greater than 10 kph (If the simulator has a GUI for this one, it should display the longitudinal velocity is greater than 10 kph) + - Ego vehicle longitudinal velocity is going below 10 kph. This is an ego vehicle reset process to tear down this test case. +5. Check if pytest output is passed or failure + +### Test Case #4 + +1. Run your simulator (If the simulator is already running, skip this part) +2. Load a map and an ego vehicle for the test (If a map and an ego are loaded already, skip this part) +3. Run the test using the following command + + ```bash + python -m pytest test_04_lateral_command_and_report.py + ``` + +4. Check if expected behavior is created within the simulator + - Ego vehicle steering and/or tire value is greater than 0 degree (If the simulator has a GUI for this one, it should display the steering and/or tire is greater than 0 degree) + - Ego vehicle steering and/or tire value is 0 degree. This is a reset process. (If the simulator has a GUI for this one, it should display the steering and/or tire is 0 degree) + - Ego vehicle steering and/or tire value is less than 0 degree (If the simulator has a GUI for this one, it should display the steering and/or tire is less than 0 degree) + - Ego vehicle steering and/or tire value is 0 degree. This is a reset process. (If the simulator has a GUI for this one, it should display the steering and/or tire is 0 degree) +5. Check if pytest output is passed or failure + +### Test Case #5 + +1. Run your simulator (If the simulator is already running, skip this part) +2. Load a map and an ego vehicle for the test (If a map and an ego are loaded already, skip this part) +3. Run the test using the following command + + ```bash + python -m pytest test_05_turn_indicators_cmd_and_report.py + ``` + +4. Check if expected behavior is created within the simulator + - Ego vehicle left turn indicator is turned on (If the simulator has a GUI for this one, it should display the left turn indicator is turned on) + - Ego vehicle right turn indicator is turned on (If the simulator has a GUI for this one, it should display the right turn indicator is turned on) + - Ego vehicle both turn indicators are turned off. This is a reset process. (If the simulator has a GUI for this one, it should display both left and right turn indicators are turned off) +5. Check if pytest output is passed or failure + +### Test Case #6 + +1. Run your simulator (If the simulator is already running, skip this part) +2. Load a map and an ego vehicle for the test (If a map and an ego are loaded already, skip this part) +3. Run the test using the following command + + ```bash + python -m pytest test_06_hazard_lights_cmd_and_report.py + ``` + +4. Check if expected behavior is created within the simulator + - Ego vehicle hazard lights are turned on (If the simulator has a GUI for this one, it should display the hazard lights are turned on or blinking) + - Ego vehicle hazard lights are turned off. This is a reset process. (If the simulator has a GUI for this one, it should display the hazard lights are turned off) +5. Check if pytest output is passed or failure + +## Test Procedures for test_morai_sim + +### Build process before test + +```bash +source install/setup.bash +colcon build --packages-select simulator_compatibility_test +cd src/universe/autoware.universe/tools/simulator_test/simulator_compatibility_test/test_morai_sim +``` + +Detailed process + +(WIP) + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------------------------- | ------------------ | +| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | for [Test Case #1] | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | for [Test Case #2] | +| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | for [Test Case #3] | +| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | for [Test Case #4] | +| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | for [Test Case #5] | +| `/vehicle/status/hazard_lights_status` | `autoware_auto_vehicle_msgs::msg::HazardLightsReport` | for [Test Case #6] | + +### Output + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------------- | ---------------------- | +| `/control/command/control_mode_cmd` | `autoware_auto_vehicle_msgs/ControlModeCommand` | for [Test Case #1] | +| `/control/command/gear_cmd` | `autoware_auto_vehicle_msgs/GearCommand` | for [Test Case #2] | +| `/control/command/control_cmd` | `autoware_auto_vehicle_msgs/AckermannControlCommand` | for [Test Case #3, #4] | +| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs/TurnIndicatorsCommand` | for [Test Case #5] | +| `/control/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs/HazardLightsCommand` | for [Test Case #6] | + +## Parameters + +None. + +### Node Parameters + +None. + +### Core Parameters + +None. + +## Assumptions / Known limits + +None. diff --git a/simulator/simulator_compatibility_test/package.xml b/simulator/simulator_compatibility_test/package.xml new file mode 100644 index 00000000..d0ed3e69 --- /dev/null +++ b/simulator/simulator_compatibility_test/package.xml @@ -0,0 +1,31 @@ + + + + simulator_compatibility_test + 0.0.0 + TODO: Package description + shpark + Tomoya Kimura + Shumpei Wakabayashi + TODO: License declaration + + autoware_auto_control_msgs + autoware_auto_geometry_msgs + autoware_auto_mapping_msgs + autoware_auto_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + morai_msgs + python3-pytest + + + ament_python + + diff --git a/simulator/simulator_compatibility_test/resource/simulator_compatibility_test b/simulator/simulator_compatibility_test/resource/simulator_compatibility_test new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/setup.cfg b/simulator/simulator_compatibility_test/setup.cfg new file mode 100644 index 00000000..f27928bb --- /dev/null +++ b/simulator/simulator_compatibility_test/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/simulator_compatibility_test +[install] +install_scripts=$base/lib/simulator_compatibility_test diff --git a/simulator/simulator_compatibility_test/setup.py b/simulator/simulator_compatibility_test/setup.py new file mode 100644 index 00000000..f5b1e18c --- /dev/null +++ b/simulator/simulator_compatibility_test/setup.py @@ -0,0 +1,109 @@ +from warnings import simplefilter + +from pkg_resources import PkgResourcesDeprecationWarning +from setuptools import SetuptoolsDeprecationWarning +from setuptools import setup + +# cspell: ignore moraisim + +simplefilter("ignore", category=SetuptoolsDeprecationWarning) +simplefilter("ignore", category=PkgResourcesDeprecationWarning) + +package_name = "simulator_compatibility_test" +clients = "simulator_compatibility_test/clients/" +publishers = "simulator_compatibility_test/publishers/" +subscribers = "simulator_compatibility_test/subscribers/" + +clients_moraisim = "simulator_compatibility_test/clients/moraisim/" +publishers_moraisim = "simulator_compatibility_test/publishers/moraisim/" + +setup( + name=package_name, + version="0.0.0", + packages=[ + package_name, + clients, + publishers, + subscribers, + clients_moraisim, + publishers_moraisim, + ], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="shpark", + maintainer_email="shpark@morai.ai", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=[], + entry_points={ + "console_scripts": [ + # Client + ( + "morai_ctrl_mode" + + "=" + + "simulator_compatibility_test" + + ".clients.moraisim.morai_client_event_cmd:main" + ), + # Publisher + ( + "gear_command" + + "=" + + "simulator_compatibility_test" + + ".publishers.gear_command:main" + ), + ( + "control_mode_command" + + "=" + + "simulator_compatibility_test" + + ".publishers.control_mode_command:main" + ), + ( + "morai_ctrl_cmd" + + "=" + + "simulator_compatibility_test" + + ".publishers.moraisim.morai_ctrl_cmd:main" + ), + # Subscriber + ( + "gear_report" + + "=" + + "simulator_compatibility_test" + + ".subscribers.gear_report:main" + ), + ( + "control_mode_report" + + "=" + + "simulator_compatibility_test" + + ".subscribers.control_mode_report:main" + ), + ( + "velocity_report" + + "=" + + "simulator_compatibility_test" + + ".subscribers.velocity_report:main" + ), + ( + "steering_report" + + "=" + + "simulator_compatibility_test" + + ".subscribers.steering_report:main" + ), + ( + "turn_indicators_report" + + "=" + + "simulator_compatibility_test" + + ".subscribers.turn_indicators_report:main" + ), + ( + "hazard_lights_report" + + "=" + + "simulator_compatibility_test" + + ".subscribers.hazard_lights_report:main" + ), + ], + }, +) diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/__init__.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/clients/__init__.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/clients/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/clients/moraisim/__init__.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/clients/moraisim/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/clients/moraisim/morai_client_event_cmd.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/clients/moraisim/morai_client_event_cmd.py new file mode 100644 index 00000000..3fdd0b45 --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/clients/moraisim/morai_client_event_cmd.py @@ -0,0 +1,45 @@ +from morai_msgs.srv import MoraiEventCmdSrv +import rclpy +from rclpy.node import Node + + +class ClientEventCmdAsync(Node): + def __init__(self): + super().__init__("MoraiEventCmdSrv") + self.client_ = self.create_client(MoraiEventCmdSrv, "morai_msgs/MoraiEventCmdSrv") + while not self.client_.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available, waiting ...") + self.msg_ = MoraiEventCmdSrv.Request() + + def send_request(self): + self.msg_.request.option = 1 + self.msg_.request.ctrl_mode = 3 + self.msg_.request.gear = 0 + self.msg_.request.lamps.turn_signal = 0 + self.msg_.request.lamps.emergency_signal = 0 + self.msg_.request.set_pause = False + self.future = self.client_.call_async(self.msg_) + + +def main(args=None): + rclpy.init(args=args) + + event_cmd_client = ClientEventCmdAsync() + event_cmd_client.send_request() + + while rclpy.ok(): + rclpy.spin_once(event_cmd_client) + if event_cmd_client.future.done(): + result_msg = event_cmd_client.future.result() + event_cmd_client.get_logger().info( + f"Change Control Mode : \ + {result_msg.response.gear}" + ) + break + + event_cmd_client.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/__init__.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py new file mode 100644 index 00000000..9e5b0dfb --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py @@ -0,0 +1,67 @@ +from autoware_auto_control_msgs.msg import AckermannControlCommand +from autoware_auto_control_msgs.msg import AckermannLateralCommand +from autoware_auto_control_msgs.msg import LongitudinalCommand +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class PublisherAckermannControlCommand(Node): + def __init__(self): + super().__init__("ackermann_control_command_publisher") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + self.topic = "/control/command/control_cmd" + self.publisher_ = self.create_publisher(AckermannControlCommand, self.topic, QOS_RKL10TL) + + def publish_msg(self, control_cmd): + stamp = self.get_clock().now().to_msg() + msg = AckermannControlCommand() + lateral_cmd = AckermannLateralCommand() + longitudinal_cmd = LongitudinalCommand() + lateral_cmd.stamp.sec = stamp.sec + lateral_cmd.stamp.nanosec = stamp.nanosec + lateral_cmd.steering_tire_angle = control_cmd["lateral"]["steering_tire_angle"] + lateral_cmd.steering_tire_rotation_rate = control_cmd["lateral"][ + "steering_tire_rotation_rate" + ] + longitudinal_cmd.stamp.sec = stamp.sec + longitudinal_cmd.stamp.nanosec = stamp.nanosec + longitudinal_cmd.speed = control_cmd["longitudinal"]["speed"] + longitudinal_cmd.acceleration = control_cmd["longitudinal"]["acceleration"] + longitudinal_cmd.jerk = control_cmd["longitudinal"]["jerk"] + + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.lateral = lateral_cmd + msg.longitudinal = longitudinal_cmd + + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = PublisherAckermannControlCommand() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/control_mode_command.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/control_mode_command.py new file mode 100644 index 00000000..c39c7848 --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/control_mode_command.py @@ -0,0 +1,57 @@ +from enum import Enum + +from autoware_auto_vehicle_msgs.msg import ControlModeCommand +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class ControlModeCommand_Constants(Enum): + NO_COMMAND = 0 + AUTONOMOUS = 1 + MANUAL = 2 + + +class PublisherControlModeCommand(Node): + def __init__(self): + super().__init__("control_mode_command_publisher") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + self.topic = "/control/command/control_mode_cmd" + self.publisher_ = self.create_publisher(ControlModeCommand, self.topic, QOS_RKL10TL) + + def publish_msg(self, control_mode): + stamp = self.get_clock().now().to_msg() + msg = ControlModeCommand() + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.mode = control_mode.value + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = PublisherControlModeCommand() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/gear_command.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/gear_command.py new file mode 100644 index 00000000..d17329df --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/gear_command.py @@ -0,0 +1,60 @@ +from enum import Enum + +from autoware_auto_vehicle_msgs.msg import GearCommand +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class GearMode(Enum): + NONE = 0 + NEUTRAL = 1 + DRIVE = 2 + REVERSE = 20 + PARK = 22 + LOW = 23 + + +class PublisherGearCommand(Node): + def __init__(self): + super().__init__("gear_command_publisher") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + self.topic = "/control/command/gear_cmd" + self.publisher_ = self.create_publisher(GearCommand, self.topic, QOS_RKL10TL) + + def publish_msg(self, gear_mode): + stamp = self.get_clock().now().to_msg() + msg = GearCommand() + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.command = gear_mode.value + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = PublisherGearCommand() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/__init__.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/morai_ctrl_cmd.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/morai_ctrl_cmd.py new file mode 100644 index 00000000..a1af495c --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/moraisim/morai_ctrl_cmd.py @@ -0,0 +1,45 @@ +from enum import Enum + +from morai_msgs.msg import CtrlCmd +import rclpy +from rclpy.node import Node + + +class LongCmdType(Enum): + NONE = 0 + THROTTLE = 1 + VELOCITY = 2 + ACCELERATION = 3 + + +class PublisherMoraiCtrlCmd(Node): + def __init__(self): + super().__init__("CtrlCmd") + self.topic = "/ctrl_cmd" + self.publisher_ = self.create_publisher(CtrlCmd, self.topic, 10) + + # cspell: ignore longl + def publish_msg(self, cmd): + msg = CtrlCmd() + msg.longl_cmd_type = cmd["longCmdType"] + msg.accel = cmd["accel"] + msg.brake = cmd["brake"] + msg.steering = cmd["steering"] + msg.velocity = cmd["velocity"] + msg.acceleration = cmd["acceleration"] + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + publisher = PublisherMoraiCtrlCmd() + try: + rclpy.spin(publisher) + except KeyboardInterrupt: + publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/__init__.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/control_mode_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/control_mode_report.py new file mode 100644 index 00000000..965d2ac7 --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/control_mode_report.py @@ -0,0 +1,57 @@ +from enum import Enum + +from autoware_auto_vehicle_msgs.msg import ControlModeReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class ControlModeReport_Constants(Enum): + NO_COMMAND = 0 + AUTONOMOUS = 1 + MANUAL = 2 + DISENGAGED = 3 + NOT_READY = 4 + + +class SubscriberControlModeReport(Node): + def __init__(self): + super().__init__("control_mode_report_subscriber") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10V = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.topic = "/vehicle/status/control_mode" + self.subscription_ = self.create_subscription( + ControlModeReport, self.topic, self.get_control_mode, QOS_RKL10V + ) + self.received = [] + + def get_control_mode(self, msg): + self.received.append(msg.mode) + + +def main(args=None): + rclpy.init(args=args) + + node = SubscriberControlModeReport() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/gear_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/gear_report.py new file mode 100644 index 00000000..f8c9ec30 --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/gear_report.py @@ -0,0 +1,58 @@ +from enum import Enum + +from autoware_auto_vehicle_msgs.msg import GearReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class GearMode(Enum): + NONE = 0 + NEUTRAL = 1 + DRIVE = 2 + REVERSE = 20 + PARK = 22 + LOW = 23 + + +class SubscriberGearReport(Node): + def __init__(self): + super().__init__("gear_report_subscriber") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10V = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.topic = "/vehicle/status/gear_status" + self.subscription_ = self.create_subscription( + GearReport, self.topic, self.get_gear, QOS_RKL10V + ) + self.received = [] + + def get_gear(self, msg): + self.received.append(msg.report) + + +def main(args=None): + rclpy.init(args=args) + + node = SubscriberGearReport() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/hazard_lights_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/hazard_lights_report.py new file mode 100644 index 00000000..b9399137 --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/hazard_lights_report.py @@ -0,0 +1,55 @@ +from enum import Enum + +from autoware_auto_vehicle_msgs.msg import HazardLightsReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class HazardLightsReport_Constants(Enum): + DISABLE = 1 + ENABLE = 2 + + +class SubscriberHazardLightsReport(Node): + def __init__(self): + super().__init__("hazard_lights_report_subscriber") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10V = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.topic = "/vehicle/status/hazard_lights_status" + self.subscription_ = self.create_subscription( + HazardLightsReport, self.topic, self.get_status, QOS_RKL10V + ) + + self.received = [] + + def get_status(self, msg): + self.received.append(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = SubscriberHazardLightsReport() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/steering_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/steering_report.py new file mode 100644 index 00000000..64967239 --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/steering_report.py @@ -0,0 +1,48 @@ +from autoware_auto_vehicle_msgs.msg import SteeringReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class SubscriberSteeringReport(Node): + def __init__(self): + super().__init__("steering_report_subscriber") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10V = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.topic = "/vehicle/status/steering_status" + self.subscription_ = self.create_subscription( + SteeringReport, self.topic, self.get_steering, QOS_RKL10V + ) + + self.received = [] + + def get_steering(self, msg): + self.received.append(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = SubscriberSteeringReport() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/turn_indicators_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/turn_indicators_report.py new file mode 100644 index 00000000..6d3727ea --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/turn_indicators_report.py @@ -0,0 +1,56 @@ +from enum import Enum + +from autoware_auto_vehicle_msgs.msg import TurnIndicatorsReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class TurnIndicatorsReport_Constants(Enum): + DISABLE = 1 + ENABLE_LEFT = 2 + ENABLE_RIGHT = 3 + + +class SubscriberTurnIndicatorsReport(Node): + def __init__(self): + super().__init__("turn_indicators_report_subscriber") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10V = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.topic = "/vehicle/status/turn_indicators_status" + self.subscription_ = self.create_subscription( + TurnIndicatorsReport, self.topic, self.get_status, QOS_RKL10V + ) + + self.received = [] + + def get_status(self, msg): + self.received.append(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = SubscriberTurnIndicatorsReport() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/velocity_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/velocity_report.py new file mode 100644 index 00000000..c83f1b09 --- /dev/null +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/velocity_report.py @@ -0,0 +1,48 @@ +from autoware_auto_vehicle_msgs.msg import VelocityReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy + + +class SubscriberVelocityReport(Node): + def __init__(self): + super().__init__("velocity_report_subscriber") + + self.declare_parameter("qos_depth", 10) + qos_depth = self.get_parameter("qos_depth").value + + QOS_RKL10V = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=qos_depth, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.topic = "/vehicle/status/velocity_status" + self.subscription_ = self.create_subscription( + VelocityReport, self.topic, self.get_velocity, QOS_RKL10V + ) + + self.received = [] + + def get_velocity(self, msg): + self.received.append(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = SubscriberVelocityReport() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/simulator_compatibility_test/test_base/__init__.py b/simulator/simulator_compatibility_test/test_base/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/test_base/test_01_control_mode_and_report.py b/simulator/simulator_compatibility_test/test_base/test_01_control_mode_and_report.py new file mode 100644 index 00000000..d1aad9c9 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_01_control_mode_and_report.py @@ -0,0 +1,94 @@ +from enum import Enum +import time + +from autoware_auto_vehicle_msgs.msg import ControlModeCommand +import pytest +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from simulator_compatibility_test.subscribers.control_mode_report import ControlModeReport_Constants +from simulator_compatibility_test.subscribers.control_mode_report import SubscriberControlModeReport + + +class ControlModeCommand_Constants(Enum): + NO_COMMAND = 0 + AUTONOMOUS = 1 + MANUAL = 2 + + +class Test01ControlModeAndReportBase: + msgs_rx = [] + node = None + sub = None + pub = None + sub_control_mode_report = None + executor = None + + @classmethod + def setup_class(cls) -> None: + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + rclpy.init() + cls.msgs_rx = [] + cls.node = rclpy.create_node("test_01_control_mode_and_report_base") + cls.sub = cls.node.create_subscription( + ControlModeCommand, + "/control/command/control_mode_cmd", + lambda msg: cls.msgs_rx.append(msg), + 10, + ) + cls.pub = cls.node.create_publisher( + ControlModeCommand, "/control/command/control_mode_cmd", QOS_RKL10TL + ) + cls.sub_control_mode_report = SubscriberControlModeReport() + cls.executor = MultiThreadedExecutor() + cls.executor.add_node(cls.sub_control_mode_report) + cls.executor.add_node(cls.node) + + @classmethod + def teardown_class(cls) -> None: + cls.node.destroy_node() + cls.executor.shutdown() + rclpy.shutdown() + + @pytest.fixture + def setup_method(self): + self.msgs_rx.clear() + + def set_control_mode(self, control_mode): + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + self.pub.publish(self.generate_control_mode_cmd_msg(control_mode)) + if len(self.msgs_rx) > 2: + break + received = self.msgs_rx[-1] + assert received.mode == control_mode.value + self.msgs_rx.clear() + + def generate_control_mode_cmd_msg(self, control_mode): + stamp = self.node.get_clock().now().to_msg() + msg = ControlModeCommand() + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.mode = control_mode.value + return msg + + def get_control_mode_report(self): + time.sleep(1) + self.sub_control_mode_report.received.clear() + received = ControlModeReport_Constants.NO_COMMAND + try: + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + if len(self.sub_control_mode_report.received) > 2: + break + received = self.sub_control_mode_report.received.pop() + finally: + return received diff --git a/simulator/simulator_compatibility_test/test_base/test_02_change_gear_and_report.py b/simulator/simulator_compatibility_test/test_base/test_02_change_gear_and_report.py new file mode 100644 index 00000000..d86a569c --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_02_change_gear_and_report.py @@ -0,0 +1,78 @@ +import time + +from autoware_auto_vehicle_msgs.msg import GearCommand +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from simulator_compatibility_test.subscribers.gear_report import GearMode +from simulator_compatibility_test.subscribers.gear_report import SubscriberGearReport + + +class Test02ChangeGearAndReportBase: + msgs_rx = [] + node = None + sub = None + pub = None + sub_gear_report = None + executor = None + + @classmethod + def setup_class(cls) -> None: + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + rclpy.init() + cls.msgs_rx = [] + cls.node = rclpy.create_node("test_02_change_gear_and_report_base") + cls.sub = cls.node.create_subscription( + GearCommand, "/control/command/gear_cmd", lambda msg: cls.msgs_rx.append(msg), 10 + ) + cls.pub = cls.node.create_publisher(GearCommand, "/control/command/gear_cmd", QOS_RKL10TL) + cls.sub_gear_report = SubscriberGearReport() + cls.executor = MultiThreadedExecutor() + cls.executor.add_node(cls.sub_gear_report) + cls.executor.add_node(cls.node) + + @classmethod + def teardown_class(cls) -> None: + cls.node.destroy_node() + cls.executor.shutdown() + rclpy.shutdown() + + def generate_gear_msg(self, gear_mode): + stamp = self.node.get_clock().now().to_msg() + msg = GearCommand() + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.command = gear_mode.value + return msg + + def set_gear_mode(self, gear_mode): + self.msgs_rx.clear() + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + self.pub.publish(self.generate_gear_msg(gear_mode)) + if len(self.msgs_rx) > 2: + break + received = self.msgs_rx[-1] + assert received.command == gear_mode.value + self.msgs_rx.clear() + + def get_gear_mode(self): + time.sleep(1) + self.sub_gear_report.received.clear() + received = GearMode.NONE + try: + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + if len(self.sub_gear_report.received) > 2: + break + received = self.sub_gear_report.received.pop() + finally: + return received diff --git a/simulator/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py b/simulator/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py new file mode 100644 index 00000000..d55fa7eb --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py @@ -0,0 +1,129 @@ +import time + +from autoware_auto_control_msgs.msg import AckermannControlCommand +from autoware_auto_control_msgs.msg import AckermannLateralCommand +from autoware_auto_control_msgs.msg import LongitudinalCommand +import pytest +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from simulator_compatibility_test.subscribers.velocity_report import SubscriberVelocityReport + + +class Test03LongitudinalCommandAndReportBase: + msgs_rx = [] + control_cmd = {} + node = None + sub = None + pub = None + sub_velocity_report = None + executor = None + + @classmethod + def setup_class(cls) -> None: + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + rclpy.init() + cls.msgs_rx = [] + cls.control_cmd = { + "lateral": {"steering_tire_angle": 0.0, "steering_tire_rotation_rate": 0.0}, + "longitudinal": {"speed": 0.0, "acceleration": 0.0, "jerk": 0.0}, + } + + cls.node = rclpy.create_node("test_03_longitudinal_command_and_report_base") + cls.sub = cls.node.create_subscription( + AckermannControlCommand, + "/control/command/control_cmd", + lambda msg: cls.msgs_rx.append(msg), + 10, + ) + cls.pub = cls.node.create_publisher( + AckermannControlCommand, "/control/command/control_cmd", QOS_RKL10TL + ) + cls.sub_velocity_report = SubscriberVelocityReport() + cls.executor = MultiThreadedExecutor() + cls.executor.add_node(cls.sub_velocity_report) + cls.executor.add_node(cls.node) + + @classmethod + def teardown_class(cls) -> None: + cls.node.destroy_node() + cls.executor.shutdown() + rclpy.shutdown() + + @pytest.fixture + def setup_and_teardown(self): + yield time.sleep(3) + self.control_cmd["longitudinal"]["speed"] = 0.0 + self.control_cmd["longitudinal"]["acceleration"] = 0.0 + self.init_vehicle() + time.sleep(3) + + def init_vehicle(self): + self.set_speed(0.0) + + def generate_control_msg(self, control_cmd): + stamp = self.node.get_clock().now().to_msg() + msg = AckermannControlCommand() + lateral_cmd = AckermannLateralCommand() + longitudinal_cmd = LongitudinalCommand() + lateral_cmd.stamp.sec = stamp.sec + lateral_cmd.stamp.nanosec = stamp.nanosec + lateral_cmd.steering_tire_angle = control_cmd["lateral"]["steering_tire_angle"] + lateral_cmd.steering_tire_rotation_rate = control_cmd["lateral"][ + "steering_tire_rotation_rate" + ] + longitudinal_cmd.stamp.sec = stamp.sec + longitudinal_cmd.stamp.nanosec = stamp.nanosec + longitudinal_cmd.speed = control_cmd["longitudinal"]["speed"] + longitudinal_cmd.acceleration = control_cmd["longitudinal"]["acceleration"] + longitudinal_cmd.jerk = control_cmd["longitudinal"]["jerk"] + + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.lateral = lateral_cmd + msg.longitudinal = longitudinal_cmd + return msg + + def set_speed(self, speed): + self.control_cmd["longitudinal"]["speed"] = speed + self.msgs_rx.clear() + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + self.pub.publish(self.generate_control_msg(self.control_cmd)) + if len(self.msgs_rx) > 2: + break + received = self.msgs_rx[-1] + assert received.longitudinal.speed == speed + self.msgs_rx.clear() + + def set_acceleration(self, acceleration): + self.control_cmd["longitudinal"]["acceleration"] = acceleration + self.msgs_rx.clear() + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + self.pub.publish(self.generate_control_msg(self.control_cmd)) + if len(self.msgs_rx) > 2: + break + received = self.msgs_rx[-1] + assert received.longitudinal.acceleration == acceleration + self.msgs_rx.clear() + + def get_velocity_report(self): + self.sub_velocity_report.received.clear() + received = 0.0 + try: + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + if len(self.sub_velocity_report.received) > 2: + break + received = self.sub_velocity_report.received.pop() + finally: + return received diff --git a/simulator/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py b/simulator/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py new file mode 100644 index 00000000..0648ffd2 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py @@ -0,0 +1,116 @@ +import time + +from autoware_auto_control_msgs.msg import AckermannControlCommand +from autoware_auto_control_msgs.msg import AckermannLateralCommand +from autoware_auto_control_msgs.msg import LongitudinalCommand +import pytest +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from simulator_compatibility_test.subscribers.steering_report import SubscriberSteeringReport + + +class Test04LateralCommandAndReportBase: + msgs_rx = [] + control_cmd = {} + node = None + sub = None + pub = None + sub_steering_report = None + executor = None + + @classmethod + def setup_class(cls) -> None: + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + rclpy.init() + cls.msgs_rx = [] + cls.control_cmd = { + "lateral": {"steering_tire_angle": 0.0, "steering_tire_rotation_rate": 0.0}, + "longitudinal": {"speed": 0.0, "acceleration": 0.0, "jerk": 0.0}, + } + cls.node = rclpy.create_node("test_04_lateral_command_and_report_base") + cls.sub = cls.node.create_subscription( + AckermannControlCommand, + "/control/command/control_cmd", + lambda msg: cls.msgs_rx.append(msg), + 10, + ) + cls.pub = cls.node.create_publisher( + AckermannControlCommand, "/control/command/control_cmd", QOS_RKL10TL + ) + cls.sub_steering_report = SubscriberSteeringReport() + cls.executor = MultiThreadedExecutor() + cls.executor.add_node(cls.sub_steering_report) + cls.executor.add_node(cls.node) + + @classmethod + def teardown_class(cls) -> None: + cls.node.destroy_node() + cls.executor.shutdown() + rclpy.shutdown() + + @pytest.fixture + def setup_and_teardown(self): + self.control_cmd["lateral"]["steering_tire_angle"] = 0.0 + self.control_cmd["longitudinal"]["speed"] = 0.0 + yield time.sleep(3) + self.init_vehicle() + time.sleep(3) + + def init_vehicle(self): + self.set_steering_tire_angle(0.0) + + def generate_control_msg(self, control_cmd): + stamp = self.node.get_clock().now().to_msg() + msg = AckermannControlCommand() + lateral_cmd = AckermannLateralCommand() + longitudinal_cmd = LongitudinalCommand() + lateral_cmd.stamp.sec = stamp.sec + lateral_cmd.stamp.nanosec = stamp.nanosec + lateral_cmd.steering_tire_angle = control_cmd["lateral"]["steering_tire_angle"] + lateral_cmd.steering_tire_rotation_rate = control_cmd["lateral"][ + "steering_tire_rotation_rate" + ] + longitudinal_cmd.stamp.sec = stamp.sec + longitudinal_cmd.stamp.nanosec = stamp.nanosec + longitudinal_cmd.speed = control_cmd["longitudinal"]["speed"] + longitudinal_cmd.acceleration = control_cmd["longitudinal"]["acceleration"] + longitudinal_cmd.jerk = control_cmd["longitudinal"]["jerk"] + + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.lateral = lateral_cmd + msg.longitudinal = longitudinal_cmd + return msg + + def set_steering_tire_angle(self, angle_rad): + self.control_cmd["lateral"]["steering_tire_angle"] = angle_rad + self.msgs_rx.clear() + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + self.pub.publish(self.generate_control_msg(self.control_cmd)) + if len(self.msgs_rx) > 2: + break + received = self.msgs_rx[-1] + assert round(received.lateral.steering_tire_angle, 2) == round(angle_rad, 2) + self.msgs_rx.clear() + + def get_steering_report(self): + self.sub_steering_report.received.clear() + received = 0.0 + try: + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + if len(self.sub_steering_report.received) > 2: + break + received = self.sub_steering_report.received.pop() + finally: + return received diff --git a/simulator/simulator_compatibility_test/test_base/test_05_turn_indicators_cmd_and_report.py b/simulator/simulator_compatibility_test/test_base/test_05_turn_indicators_cmd_and_report.py new file mode 100644 index 00000000..974f61e1 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_05_turn_indicators_cmd_and_report.py @@ -0,0 +1,98 @@ +from enum import Enum +import time + +from autoware_auto_vehicle_msgs.msg import TurnIndicatorsCommand +import pytest +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from simulator_compatibility_test.subscribers.turn_indicators_report import ( + SubscriberTurnIndicatorsReport, +) +from simulator_compatibility_test.subscribers.turn_indicators_report import ( + TurnIndicatorsReport_Constants, +) + + +class TurnIndicatorsCommand_Constants(Enum): + NO_COMMAND = 0 + DISABLE = 1 + ENABLE_LEFT = 2 + ENABLE_RIGHT = 3 + + +class Test05TurnIndicatorsCmdAndReportBase: + msgs_rx = [] + node = None + sub = None + pub = None + sub_turn_indicators_status = None + executor = None + + @classmethod + def setup_class(cls) -> None: + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + rclpy.init() + cls.msgs_rx = [] + cls.node = rclpy.create_node("test_turn_indicator_cmd_and_report_base") + cls.sub = cls.node.create_subscription( + TurnIndicatorsCommand, + "/control/command/turn_indicators_cmd", + lambda msg: cls.msgs_rx.append(msg), + 10, + ) + cls.pub = cls.node.create_publisher( + TurnIndicatorsCommand, "/control/command/turn_indicators_cmd", QOS_RKL10TL + ) + cls.sub_turn_indicators_status = SubscriberTurnIndicatorsReport() + cls.executor = MultiThreadedExecutor() + cls.executor.add_node(cls.sub_turn_indicators_status) + cls.executor.add_node(cls.node) + + @classmethod + def teardown_class(cls) -> None: + cls.node.destroy_node() + cls.executor.shutdown() + rclpy.shutdown() + + @pytest.fixture + def setup_method(self): + self.msgs_rx.clear() + + def set_turn_indicators(self, command): + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + self.pub.publish(self.generate_turn_indicators_cmd_msg(command)) + if len(self.msgs_rx) > 2: + break + received = self.msgs_rx[-1] + assert received.command == command.value + + def generate_turn_indicators_cmd_msg(self, command): + stamp = self.node.get_clock().now().to_msg() + msg = TurnIndicatorsCommand() + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.command = command.value + return msg + + def get_turn_indicators_status(self): + time.sleep(1) + self.sub_turn_indicators_status.received.clear() + received = TurnIndicatorsReport_Constants.DISABLE + try: + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + if len(self.sub_turn_indicators_status.received) > 2: + break + received = self.sub_turn_indicators_status.received.pop() + finally: + return received diff --git a/simulator/simulator_compatibility_test/test_base/test_06_hazard_lights_cmd_and_report.py b/simulator/simulator_compatibility_test/test_base/test_06_hazard_lights_cmd_and_report.py new file mode 100644 index 00000000..7e361d54 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_06_hazard_lights_cmd_and_report.py @@ -0,0 +1,97 @@ +from enum import Enum +import time + +from autoware_auto_vehicle_msgs.msg import HazardLightsCommand +import pytest +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from simulator_compatibility_test.subscribers.hazard_lights_report import ( + HazardLightsReport_Constants, +) +from simulator_compatibility_test.subscribers.hazard_lights_report import ( + SubscriberHazardLightsReport, +) + + +class HazardLightsCommand_Constants(Enum): + NO_COMMAND = 0 + DISABLE = 1 + ENABLE = 2 + + +class Test06HazardLightsCmdAndReportBase: + msgs_rx = [] + node = None + sub = None + pub = None + sub_hazard_lights_status = None + executor = None + + @classmethod + def setup_class(cls) -> None: + QOS_RKL10TL = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + rclpy.init() + cls.msgs_rx = [] + cls.node = rclpy.create_node("test_hazard_lights_cmd_and_report_base") + cls.sub = cls.node.create_subscription( + HazardLightsCommand, + "/control/command/hazard_lights_cmd", + lambda msg: cls.msgs_rx.append(msg), + 10, + ) + cls.pub = cls.node.create_publisher( + HazardLightsCommand, "/control/command/hazard_lights_cmd", QOS_RKL10TL + ) + cls.sub_hazard_lights_status = SubscriberHazardLightsReport() + cls.executor = MultiThreadedExecutor() + cls.executor.add_node(cls.sub_hazard_lights_status) + cls.executor.add_node(cls.node) + + @classmethod + def teardown_class(cls) -> None: + cls.node.destroy_node() + cls.executor.shutdown() + rclpy.shutdown() + + @pytest.fixture + def setup_method(self): + self.msgs_rx.clear() + + def set_hazard_lights(self, command): + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + self.pub.publish(self.generate_hazard_lights_cmd_msg(command)) + if len(self.msgs_rx) > 2: + break + received = self.msgs_rx[-1] + assert received.command == command.value + + def generate_hazard_lights_cmd_msg(self, command): + stamp = self.node.get_clock().now().to_msg() + msg = HazardLightsCommand() + msg.stamp.sec = stamp.sec + msg.stamp.nanosec = stamp.nanosec + msg.command = command.value + return msg + + def get_hazard_lights_status(self): + time.sleep(1) + self.sub_hazard_lights_status.received.clear() + received = HazardLightsReport_Constants.DISABLE + try: + while rclpy.ok(): + self.executor.spin_once(timeout_sec=1) + if len(self.sub_hazard_lights_status.received) > 2: + break + received = self.sub_hazard_lights_status.received.pop() + finally: + return received diff --git a/simulator/simulator_compatibility_test/test_base/test_copyright.py b/simulator/simulator_compatibility_test/test_base/test_copyright.py new file mode 100644 index 00000000..f46f861d --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/simulator/simulator_compatibility_test/test_base/test_flake8.py b/simulator/simulator_compatibility_test/test_base/test_flake8.py new file mode 100644 index 00000000..49c1644f --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/simulator/simulator_compatibility_test/test_base/test_pep257.py b/simulator/simulator_compatibility_test/test_base/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_base/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/simulator/simulator_compatibility_test/test_morai_sim/__init__.py b/simulator/simulator_compatibility_test/test_morai_sim/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/test_morai_sim/test_01_control_mode_and_report.py b/simulator/simulator_compatibility_test/test_morai_sim/test_01_control_mode_and_report.py new file mode 100644 index 00000000..d664f1f3 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_morai_sim/test_01_control_mode_and_report.py @@ -0,0 +1,19 @@ +import pytest +from simulator_compatibility_test.publishers.control_mode_command import ( + ControlModeCommand_Constants, +) +from simulator_compatibility_test.subscribers.control_mode_report import ControlModeReport_Constants +from test_base.test_01_control_mode_and_report import Test01ControlModeAndReportBase + + +class Test01ControlModeAndReportMorai(Test01ControlModeAndReportBase): + @pytest.mark.skip(reason="manual mode is not required for now") + def test_1_manual_mode(self, setup_method): + self.set_control_mode(ControlModeCommand_Constants.MANUAL) + result = self.get_control_mode_report() + assert result == ControlModeReport_Constants.MANUAL.value + + def test_2_auto_mode(self, setup_method): + self.set_control_mode(ControlModeCommand_Constants.AUTONOMOUS) + result = self.get_control_mode_report() + assert result == ControlModeReport_Constants.AUTONOMOUS.value diff --git a/simulator/simulator_compatibility_test/test_morai_sim/test_02_change_gear_and_report.py b/simulator/simulator_compatibility_test/test_morai_sim/test_02_change_gear_and_report.py new file mode 100644 index 00000000..ea2f5ed0 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_morai_sim/test_02_change_gear_and_report.py @@ -0,0 +1,28 @@ +from test_base.test_02_change_gear_and_report import GearMode +from test_base.test_02_change_gear_and_report import Test02ChangeGearAndReportBase + + +class Test02ChangeGearAndReportMorai(Test02ChangeGearAndReportBase): + @classmethod + def setup_class(cls) -> None: + super().setup_class() + + def test_1_gear_park(self): + self.set_gear_mode(GearMode.PARK) + result = self.get_gear_mode() + assert result == GearMode.PARK.value + + def test_2_gear_neutral(self): + self.set_gear_mode(GearMode.NEUTRAL) + result = self.get_gear_mode() + assert result == GearMode.NEUTRAL.value + + def test_3_gear_reverse(self): + self.set_gear_mode(GearMode.REVERSE) + result = self.get_gear_mode() + assert result == GearMode.REVERSE.value + + def test_4_gear_drive(self): + self.set_gear_mode(GearMode.DRIVE) + result = self.get_gear_mode() + assert result == GearMode.DRIVE.value diff --git a/simulator/simulator_compatibility_test/test_morai_sim/test_03_longitudinal_command_and_report.py b/simulator/simulator_compatibility_test/test_morai_sim/test_03_longitudinal_command_and_report.py new file mode 100644 index 00000000..c49df58e --- /dev/null +++ b/simulator/simulator_compatibility_test/test_morai_sim/test_03_longitudinal_command_and_report.py @@ -0,0 +1,16 @@ +import time + +from test_base.test_03_longitudinal_command_and_report import Test03LongitudinalCommandAndReportBase + + +class Test03LongitudinalCommandAndReportMorai(Test03LongitudinalCommandAndReportBase): + @classmethod + def setup_class(cls) -> None: + super().setup_class() + + def test_1_longitudinal_control(self, setup_and_teardown): + self.set_speed(5.0) + self.set_acceleration(1.0) + time.sleep(10) + current_speed = self.get_velocity_report() + assert current_speed.longitudinal_velocity > 5.0 diff --git a/simulator/simulator_compatibility_test/test_morai_sim/test_04_lateral_command_and_report.py b/simulator/simulator_compatibility_test/test_morai_sim/test_04_lateral_command_and_report.py new file mode 100644 index 00000000..66742659 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_morai_sim/test_04_lateral_command_and_report.py @@ -0,0 +1,24 @@ +from math import pi +import time + +from test_base.test_04_lateral_command_and_report import Test04LateralCommandAndReportBase + + +class Test04LateralCommandAndReportMorai(Test04LateralCommandAndReportBase): + @classmethod + def setup_class(cls) -> None: + super().setup_class() + + def test_1_grater_than_zero(self, setup_and_teardown): + target_value = pi / 6 + self.set_steering_tire_angle(target_value) + time.sleep(3) + current_report = self.get_steering_report() + assert current_report.steering_tire_angle > 0.0 + + def test_2_less_than_zero(self, setup_and_teardown): + target_value = (-1) * pi / 6 + self.set_steering_tire_angle(target_value) + time.sleep(3) + current_report = self.get_steering_report() + assert current_report.steering_tire_angle < 0.0 diff --git a/simulator/simulator_compatibility_test/test_morai_sim/test_05_turn_indicators_cmd_and_report.py b/simulator/simulator_compatibility_test/test_morai_sim/test_05_turn_indicators_cmd_and_report.py new file mode 100644 index 00000000..557fbc54 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_morai_sim/test_05_turn_indicators_cmd_and_report.py @@ -0,0 +1,25 @@ +from simulator_compatibility_test.subscribers.turn_indicators_report import ( + TurnIndicatorsReport_Constants, +) +from test_base.test_05_turn_indicators_cmd_and_report import Test05TurnIndicatorsCmdAndReportBase +from test_base.test_05_turn_indicators_cmd_and_report import TurnIndicatorsCommand_Constants + + +class Test05TurnIndicatorsCmdAndReportMorai(Test05TurnIndicatorsCmdAndReportBase): + def test_1_enable_left(self, setup_method): + turn_indicators = TurnIndicatorsCommand_Constants.ENABLE_LEFT + self.set_turn_indicators(turn_indicators) + result = self.get_turn_indicators_status() + assert result.report == TurnIndicatorsReport_Constants.ENABLE_LEFT.value + + def test_2_enable_right(self, setup_method): + turn_indicators = TurnIndicatorsCommand_Constants.ENABLE_RIGHT + self.set_turn_indicators(turn_indicators) + result = self.get_turn_indicators_status() + assert result.report == TurnIndicatorsReport_Constants.ENABLE_RIGHT.value + + def test_3_disable(self, setup_method): + turn_indicators = TurnIndicatorsCommand_Constants.DISABLE + self.set_turn_indicators(turn_indicators) + result = self.get_turn_indicators_status() + assert result.report == TurnIndicatorsReport_Constants.DISABLE.value diff --git a/simulator/simulator_compatibility_test/test_morai_sim/test_06_hazard_lights_cmd_and_report.py b/simulator/simulator_compatibility_test/test_morai_sim/test_06_hazard_lights_cmd_and_report.py new file mode 100644 index 00000000..aed78878 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_morai_sim/test_06_hazard_lights_cmd_and_report.py @@ -0,0 +1,19 @@ +from simulator_compatibility_test.subscribers.hazard_lights_report import ( + HazardLightsReport_Constants, +) +from test_base.test_06_hazard_lights_cmd_and_report import HazardLightsCommand_Constants +from test_base.test_06_hazard_lights_cmd_and_report import Test06HazardLightsCmdAndReportBase + + +class Test06HazardLightsCmdAndReportMorai(Test06HazardLightsCmdAndReportBase): + def test_1_enable(self, setup_method): + hazard_lights = HazardLightsCommand_Constants.ENABLE + self.set_hazard_lights(hazard_lights) + result = self.get_hazard_lights_status() + assert result.report == HazardLightsReport_Constants.ENABLE.value + + def test_2_disable(self, setup_method): + hazard_lights = HazardLightsCommand_Constants.DISABLE + self.set_hazard_lights(hazard_lights) + result = self.get_hazard_lights_status() + assert result.report == HazardLightsReport_Constants.DISABLE.value diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/__init__.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_01_control_mode_and_report.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_01_control_mode_and_report.py new file mode 100644 index 00000000..35e33149 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_01_control_mode_and_report.py @@ -0,0 +1,19 @@ +import pytest +from simulator_compatibility_test.publishers.control_mode_command import ( + ControlModeCommand_Constants, +) +from simulator_compatibility_test.subscribers.control_mode_report import ControlModeReport_Constants +from test_base.test_01_control_mode_and_report import Test01ControlModeAndReportBase + + +class Test01ControlModeAndReportSim(Test01ControlModeAndReportBase): + @pytest.mark.skip(reason="manual mode is not required for now") + def test_1_manual_mode(self, setup_method): + self.set_control_mode(ControlModeCommand_Constants.MANUAL) + result = self.get_control_mode_report() + assert result == ControlModeReport_Constants.MANUAL.value + + def test_2_auto_mode(self, setup_method): + self.set_control_mode(ControlModeCommand_Constants.AUTONOMOUS) + result = self.get_control_mode_report() + assert result == ControlModeReport_Constants.AUTONOMOUS.value diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_02_change_gear_and_report.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_02_change_gear_and_report.py new file mode 100644 index 00000000..4be81e26 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_02_change_gear_and_report.py @@ -0,0 +1,28 @@ +from test_base.test_02_change_gear_and_report import GearMode +from test_base.test_02_change_gear_and_report import Test02ChangeGearAndReportBase + + +class Test02ChangeGearAndReportSim(Test02ChangeGearAndReportBase): + @classmethod + def setup_class(cls) -> None: + super().setup_class() + + def test_1_gear_park(self): + self.set_gear_mode(GearMode.PARK) + result = self.get_gear_mode() + assert result == GearMode.PARK.value + + def test_2_gear_neutral(self): + self.set_gear_mode(GearMode.NEUTRAL) + result = self.get_gear_mode() + assert result == GearMode.NEUTRAL.value + + def test_3_gear_reverse(self): + self.set_gear_mode(GearMode.REVERSE) + result = self.get_gear_mode() + assert result == GearMode.REVERSE.value + + def test_4_gear_drive(self): + self.set_gear_mode(GearMode.DRIVE) + result = self.get_gear_mode() + assert result == GearMode.DRIVE.value diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_03_longitudinal_command_and_report.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_03_longitudinal_command_and_report.py new file mode 100644 index 00000000..4379fc5d --- /dev/null +++ b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_03_longitudinal_command_and_report.py @@ -0,0 +1,16 @@ +import time + +from test_base.test_03_longitudinal_command_and_report import Test03LongitudinalCommandAndReportBase + + +class Test03LongitudinalCommandAndReportSim(Test03LongitudinalCommandAndReportBase): + @classmethod + def setup_class(cls) -> None: + super().setup_class() + + def test_1_longitudinal_control(self, setup_and_teardown): + self.set_speed(5.0) + self.set_acceleration(1.0) + time.sleep(10) + current_speed = self.get_velocity_report() + assert current_speed.longitudinal_velocity > 5.0 diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_04_lateral_command_and_report.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_04_lateral_command_and_report.py new file mode 100644 index 00000000..98188824 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_04_lateral_command_and_report.py @@ -0,0 +1,24 @@ +from math import pi +import time + +from test_base.test_04_lateral_command_and_report import Test04LateralCommandAndReportBase + + +class Test04LateralCommandAndReportSim(Test04LateralCommandAndReportBase): + @classmethod + def setup_class(cls) -> None: + super().setup_class() + + def test_1_grater_than_zero(self, setup_and_teardown): + target_value = pi / 6 + self.set_steering_tire_angle(target_value) + time.sleep(3) + current_report = self.get_steering_report() + assert current_report.steering_tire_angle > 0.0 + + def test_2_less_than_zero(self, setup_and_teardown): + target_value = (-1) * pi / 6 + self.set_steering_tire_angle(target_value) + time.sleep(3) + current_report = self.get_steering_report() + assert current_report.steering_tire_angle < 0.0 diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_05_turn_indicators_cmd_and_report.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_05_turn_indicators_cmd_and_report.py new file mode 100644 index 00000000..2c880f96 --- /dev/null +++ b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_05_turn_indicators_cmd_and_report.py @@ -0,0 +1,25 @@ +from simulator_compatibility_test.subscribers.turn_indicators_report import ( + TurnIndicatorsReport_Constants, +) +from test_base.test_05_turn_indicators_cmd_and_report import Test05TurnIndicatorsCmdAndReportBase +from test_base.test_05_turn_indicators_cmd_and_report import TurnIndicatorsCommand_Constants + + +class Test05TurnIndicatorsCmdAndReportSim(Test05TurnIndicatorsCmdAndReportBase): + def test_1_enable_left(self, setup_method): + turn_indicators = TurnIndicatorsCommand_Constants.ENABLE_LEFT + self.set_turn_indicators(turn_indicators) + result = self.get_turn_indicators_status() + assert result.report == TurnIndicatorsReport_Constants.ENABLE_LEFT.value + + def test_2_enable_right(self, setup_method): + turn_indicators = TurnIndicatorsCommand_Constants.ENABLE_RIGHT + self.set_turn_indicators(turn_indicators) + result = self.get_turn_indicators_status() + assert result.report == TurnIndicatorsReport_Constants.ENABLE_RIGHT.value + + def test_3_disable(self, setup_method): + turn_indicators = TurnIndicatorsCommand_Constants.DISABLE + self.set_turn_indicators(turn_indicators) + result = self.get_turn_indicators_status() + assert result.report == TurnIndicatorsReport_Constants.DISABLE.value diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_06_hazard_lights_cmd_and_report.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_06_hazard_lights_cmd_and_report.py new file mode 100644 index 00000000..cd9e0bbf --- /dev/null +++ b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_06_hazard_lights_cmd_and_report.py @@ -0,0 +1,19 @@ +from simulator_compatibility_test.subscribers.hazard_lights_report import ( + HazardLightsReport_Constants, +) +from test_base.test_06_hazard_lights_cmd_and_report import HazardLightsCommand_Constants +from test_base.test_06_hazard_lights_cmd_and_report import Test06HazardLightsCmdAndReportBase + + +class Test06HazardLightsCmdAndReportSim(Test06HazardLightsCmdAndReportBase): + def test_1_enable(self, setup_method): + hazard_lights = HazardLightsCommand_Constants.ENABLE + self.set_hazard_lights(hazard_lights) + result = self.get_hazard_lights_status() + assert result.report == HazardLightsReport_Constants.ENABLE.value + + def test_2_disable(self, setup_method): + hazard_lights = HazardLightsCommand_Constants.DISABLE + self.set_hazard_lights(hazard_lights) + result = self.get_hazard_lights_status() + assert result.report == HazardLightsReport_Constants.DISABLE.value diff --git a/vehicle/accel_brake_map_calibrator/CMakeLists.txt b/vehicle/accel_brake_map_calibrator/CMakeLists.txt new file mode 100644 index 00000000..f8aed0ab --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(accel_brake_map_calibrator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(accel_brake_map_calibrator + src/accel_brake_map_calibrator_node.cpp + src/main.cpp +) +ament_target_dependencies(accel_brake_map_calibrator) + +install( + PROGRAMS + scripts/__init__.py + scripts/actuation_cmd_publisher.py + scripts/accel_tester.py + scripts/brake_tester.py + scripts/config.py + scripts/delay_estimator.py + scripts/plotter.py + scripts/view_statistics.py + scripts/calc_utils.py + scripts/csv_reader.py + scripts/log_analyzer.py + scripts/view_plot.py + scripts/new_accel_brake_map_server.py + test/plot_csv_client_node.py + test/sim_actuation_status_publisher.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config + rviz) diff --git a/vehicle/accel_brake_map_calibrator/README.md b/vehicle/accel_brake_map_calibrator/README.md new file mode 100644 index 00000000..c8813280 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/README.md @@ -0,0 +1,260 @@ +# accel_brake_map_calibrator + +The role of this node is to automatically calibrate `accel_map.csv` / `brake_map.csv` used in the `raw_vehicle_cmd_converter` node. + +The base map, which is lexus's one by default, is updated iteratively with the loaded driving data. + +## How to calibrate + +### Launch Calibrator + +After launching Autoware, run the `accel_brake_map_calibrator` by the following command and then perform autonomous driving. Note: You can collect data with manual driving if it is possible to use the same vehicle interface as during autonomous driving (e.g. using a joystick). + +```sh +ros2 launch accel_brake_map_calibrator accel_brake_map_calibrator.launch.xml rviz:=true +``` + +Or if you want to use rosbag files, run the following commands. + +```sh +ros2 launch accel_brake_map_calibrator accel_brake_map_calibrator.launch.xml rviz:=true use_sim_time:=true +ros2 bag play --clock +``` + +During the calibration with setting the parameter `progress_file_output` to true, the log file is output in [directory of *accel_brake_map_calibrator*]/config/ . You can also see accel and brake maps in [directory of *accel_brake_map_calibrator*]/config/accel_map.csv and [directory of *accel_brake_map_calibrator*]/config/brake_map.csv after calibration. + +### Calibration plugin + +The `rviz:=true` option displays the RViz with a calibration plugin as below. + +

+ +

+ +The current status (velocity and pedal) is shown in the plugin. The color on the current cell varies green/red depending on the current data is valid/invalid. The data that doesn't satisfy the following conditions are considered invalid and will not be used for estimation since aggressive data (e.g. when the pedal is moving fast) causes bad calibration accuracy. + +- The velocity and pedal conditions are within certain ranges from the index values. +- The steer value, pedal speed, pitch value, etc. are less than corresponding thresholds. +- The velocity is higher than a threshold. + +The detailed parameters are described in the parameter section. + +Note: You don't need to worry about whether the current state is red or green during calibration. Just keep getting data until all the cells turn red. + +The value of each cell in the map is gray at first, and it changes from blue to red as the number of valid data in the cell accumulates. It is preferable to continue the calibration until each cell of the map becomes close to red. In particular, the performance near the stop depends strongly on the velocity of 0 ~ 6m/s range and the pedal value of +0.2 ~ -0.4, range so it is desirable to focus on those areas. + +### Diagnostics + +The `accel brake map_calibrator` publishes diagnostics message depending on the calibration status. +Diagnostic type `WARN` indicates that the current accel/brake map is estimated to be inaccurate. In this situation, it is strongly recommended to perform a re-calibration of the accel/brake map. + +| Status | Diagnostics Type | Diagnostics message | Description | +| ----------------------- | ---------------- | ------------------------------------------ | --------------------------------------------------- | +| No calibration required | `OK` | "OK" | | +| Calibration Required | `WARN` | "Accel/brake map Calibration is required." | The accuracy of current accel/brake map may be low. | + +This diagnostics status can be also checked on the following ROS topic. + +```sh +ros2 topic echo /accel_brake_map_calibrator/output/update_suggest +``` + +When the diagnostics type is `WARN`, `True` is published on this topic and the update of the accel/brake map is suggested. + +### Evaluation of the accel / brake map accuracy + +The accuracy of map is evaluated by the **Root Mean Squared Error (RMSE)** between the observed acceleration and predicted acceleration. + +**TERMS:** + +- `Observed acceleration`: the current vehicle acceleration which is calculated as a derivative value of the wheel speed. + +- `Predicted acceleration`: the output of the original accel/brake map, which the Autoware is expecting. The value is calculated using the current pedal and velocity. + +You can check additional error information with the following topics. + +- `/accel_brake_map_calibrator/output/current_map_error` : The error of the original map set in the `csv_path_accel/brake_map` path. The original map is not accurate if this value is large. +- `/accel_brake_map_calibrator/output/updated_map_error` : The error of the map calibrated in this node. The calibration quality is low if this value is large. +- `/accel_brake_map_calibrator/output/map_error_ratio` : The error ratio between the original map and updated map (ratio = updated / current). If this value is less than 1, it is desirable to update the map. + +### How to visualize calibration data + +The process of calibration can be visualized as below. Since these scripts need the log output of the calibration, the `pedal_accel_graph_output` parameter must be set to true while the calibration is running for the visualization. + +#### Visualize plot of relation between acceleration and pedal + +The following command shows the plot of used data in the calibration. In each plot of velocity ranges, you can see the distribution of the relationship between pedal and acceleration, and raw data points with colors according to their pitch angles. + +```sh +ros2 run accel_brake_map_calibrator view_plot.py +``` + +![sample pic](media/log_sample.png) + +#### Visualize statistics about acceleration/velocity/pedal data + +The following command shows the statistics of the calibration: + +- mean value +- standard deviation +- number of data + +of all data in each map cell. + +```sh +ros2 run accel_brake_map_calibrator view_statistics.py +``` + +![sample pic2](media/statistics_sample.png) + +### How to save the calibrated accel / brake map anytime you want + +You can save accel and brake map anytime with the following command. + +```sh +ros2 service call /accel_brake_map_calibrator/update_map_dir tier4_vehicle_msgs/srv/UpdateAccelBrakeMap "path: ''" +``` + +You can also save accel and brake map in the default directory where Autoware reads accel_map.csv/brake_map.csv using the RViz plugin (AccelBrakeMapCalibratorButtonPanel) as following. + +1. Click _Panels_ tab, and select AccelBrakeMapCalibratorButtonPanel. + + ![add_panel](./media/add_panel.png) + +2. Select the panel, and the button will appear at the bottom of RViz. + + ![calibrator_button_panel](./media/calibrator_button_panel.png) + +3. Press the button, and the accel / brake map will be saved. + (The button cannot be pressed in certain situations, such as when the calibrator node is not running.) + + ![push_calibration_button](./media/push_calibration_button.png) + +## Parameters + +## System Parameters + +| Name | Type | Description | Default value | +| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------------------------------------- | +| update_method | string | you can select map calibration method. "update_offset_each_cell" calculates offsets for each grid cells on the map. "update_offset_total" calculates the total offset of the map. | "update_offset_each_cell" | +| get_pitch_method | string | "tf": get pitch from tf, "none": unable to perform pitch validation and pitch compensation | "tf" | +| pedal_accel_graph_output | bool | if true, it will output a log of the pedal accel graph. | true | +| progress_file_output | bool | if true, it will output a log and csv file of the update process. | false | +| default_map_dir | str | directory of default map | [directory of *raw_vehicle_cmd_converter*]/data/default/ | +| calibrated_map_dir | str | directory of calibrated map | [directory of *accel_brake_map_calibrator*]/config/ | +| update_hz | double | hz for update | 10.0 | + +## Algorithm Parameters + +| Name | Type | Description | Default value | +| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| initial_covariance | double | Covariance of initial acceleration map (larger covariance makes the update speed faster) | 0.05 | +| velocity_min_threshold | double | Speeds smaller than this are not used for updating. | 0.1 | +| velocity_diff_threshold | double | When the velocity data is more than this threshold away from the grid reference speed (center value), the associated data is not used for updating. | 0.556 | +| max_steer_threshold | double | If the steer angle is greater than this value, the associated data is not used for updating. | 0.2 | +| max_pitch_threshold | double | If the pitch angle is greater than this value, the associated data is not used for updating. | 0.02 | +| max_jerk_threshold | double | If the ego jerk calculated from ego acceleration is greater than this value, the associated data is not used for updating. | 0.7 | +| pedal_velocity_thresh | double | If the pedal moving speed is greater than this value, the associated data is not used for updating. | 0.15 | +| pedal_diff_threshold | double | If the current pedal value is more then this threshold away from the previous value, the associated data is not used for updating. | 0.03 | +| max_accel | double | Maximum value of acceleration calculated from velocity source. | 5.0 | +| min_accel | double | Minimum value of acceleration calculated from velocity source. | -5.0 | +| pedal_to_accel_delay | double | The delay time between actuation_cmd to acceleration, considered in the update logic. | 0.3 | +| update_suggest_thresh | double | threshold of RMSE ratio that update suggest flag becomes true. ( RMSE ratio: [RMSE of new map] / [RMSE of original map] ) | 0.7 | +| max_data_count | int | For visualization. When the data num of each grid gets this value, the grid color gets red. | 100 | +| accel_brake_value_source | string | Whether to use actuation_status or actuation_command as accel/brake sources. value | status | + +## Test utility scripts + +### Constant accel/brake command test + +These scripts are useful to test for accel brake map calibration. These generate an `ActuationCmd` with a constant accel/brake value given interactively by a user through CLI. + +- accel_tester.py +- brake_tester.py +- actuation_cmd_publisher.py + +The `accel/brake_tester.py` receives a target accel/brake command from CLI. It sends a target value to `actuation_cmd_publisher.py` which generates the `ActuationCmd`. You can run these scripts by the following commands in the different terminals, and it will be as in the screenshot below. + +```bash +ros2 run accel_brake_map_calibrator accel_tester.py +ros2 run accel_brake_map_calibrator brake_tester.py +ros2 run accel_brake_map_calibrator actuation_cmd_publisher.py +``` + +![actuation_cmd_publisher_util](./media/actuation_cmd_publisher_util.png) + +## Calibration Method + +Two algorithms are selectable for the acceleration map update, [update_offset_four_cell_around](#update_offset_four_cell_around-1) and [update_offset_each_cell](#update_offset_each_cell). Please see the link for details. + +### Data Preprocessing + +Before calibration, missing or unusable data (e.g., too large handle angles) must first be eliminated. The following parameters are used to determine which data to remove. + +#### Parameters + +| Name | Description | Default Value | +| ---------------------- | ---------------------------- | ------------- | +| velocity_min_threshold | Exclude minimal velocity | 0.1 | +| max_steer_threshold | Exclude large steering angle | 0.2 | +| max_pitch_threshold | Exclude large pitch angle | 0.02 | +| max_jerk_threshold | Exclude large jerk | 0.7 | +| pedal_velocity_thresh | Exclude large pedaling speed | 0.15 | + +### update_offset_each_cell + +Update by Recursive Least Squares(RLS) method using data close enough to each grid. + +**Advantage** : Only data close enough to each grid is used for calibration, allowing accurate updates at each point. + +**Disadvantage** : Calibration is time-consuming due to a large amount of data to be excluded. + +#### Parameters + +Data selection is determined by the following thresholds. +| Name | Default Value | +| ----------------------- | ------------- | +| velocity_diff_threshold | 0.556 | +| pedal_diff_threshold | 0.03 | + +#### Update formula + +$$ +\begin{align} + \theta[n]=& + \theta[n-1]+\frac{p[n-1]x^{(n)}}{\lambda+p[n-1]{(x^{(n)})}^2}(y^{(n)}-\theta[n-1]x^{(n)})\\ + p[n]=&\frac{p[n-1]}{\lambda+p[n-1]{(x^{(n)})}^2} +\end{align} +$$ + +#### Variables + +| Variable name | Symbol | +| ------------------ | ----------- | +| covariance | $p[n-1]$ | +| map_offset | $\theta[n]$ | +| forgetting*factor* | $\lambda$ | +| phi | $x(=1)$ | +| measured_acc | $y$ | + +### update_offset_four_cell_around [1] + +Update the offsets by RLS in four grids around newly obtained data. By considering linear interpolation, the update takes into account appropriate weights. Therefore, there is no need to remove data by thresholding. + +**Advantage** : No data is wasted because updates are performed on the 4 grids around the data with appropriate weighting. +**Disadvantage** : Accuracy may be degraded due to extreme bias of the data. For example, if data $z(k)$ is biased near $Z_{RR}$ in Fig. 2, updating is performed at the four surrounding points ( $Z_{RR}$, $Z_{RL}$, $Z_{LR}$, and $Z_{LL}$), but accuracy at $Z_{LL}$ is not expected. + + +

+ +

+ +#### Implementation + +See eq.(7)-(10) in [1] for the updated formula. In addition, eq.(17),(18) from [1] are used for Anti-Windup. + +### References + + + +[1] [Gabrielle Lochrie, Michael Doljevic, Mario Nona, Yongsoon Yoon, Anti-Windup Recursive Least Squares Method for Adaptive Lookup Tables with Application to Automotive Powertrain Control Systems, IFAC-PapersOnLine, Volume 54, Issue 20, 2021, Pages 840-845](https://www.sciencedirect.com/science/article/pii/S240589632102320X) diff --git a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml new file mode 100644 index 00000000..c7ee0a3a --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + update_hz: 10.0 + initial_covariance: 0.05 + velocity_min_threshold: 0.1 + velocity_diff_threshold: 0.556 + pedal_diff_threshold: 0.03 + max_steer_threshold: 0.2 + max_pitch_threshold: 0.02 + max_jerk_threshold: 0.7 + pedal_velocity_thresh: 0.15 + pedal_to_accel_delay: 0.3 + max_accel: 5.0 + min_accel: -5.0 + max_data_count: 100 + precision: 3 + update_method: "update_offset_four_cell_around" # or "update_offset_each_cell", "update_offset_total", "update_offset_four_cell_around" + update_suggest_thresh: 0.7 # threshold of rmse rate that update suggest flag becomes true. ( rmse_rate: [rmse of update map] / [rmse of original map] ) + progress_file_output: false # flag to output log file + accel_brake_value_source: "status" # "status" or "command" diff --git a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml new file mode 100644 index 00000000..2f272494 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml @@ -0,0 +1,818 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + speed + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/3 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + raw accel + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/4 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + accel + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + accel pedal + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + brake pedal + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/12 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + success to update + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/13 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + jerk + + + + true + + 30 + main info + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + true + + 30 + Untitled Plot + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/9 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + raw pitch + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/10 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + pitch + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/5 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + raw accel speed + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/6 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + accel speed + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/7 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + raw brake speed + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/8 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + brake speed + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + data/11 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /accel_brake_map_calibrator/debug_values + std_msgs/Float32MultiArray + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + steer + + + + true + + 30 + sub info + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + true + + 30 + Untitled Plot + + + + false +
+ diff --git a/vehicle/accel_brake_map_calibrator/data/accel_map.csv b/vehicle/accel_brake_map_calibrator/data/accel_map.csv new file mode 100644 index 00000000..32e639ca --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/data/accel_map.csv @@ -0,0 +1,7 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 +0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 +0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 +0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 +0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 diff --git a/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv b/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv new file mode 100644 index 00000000..e5492886 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv @@ -0,0 +1,12 @@ +default,0,0.695,1.39,2.085,2.78,3.475,4.17,4.865,5.56,6.255,6.95,7.645,8.34,9.035,9.73,10.425,11.12,11.815,12.51,13.205,13.9 +0,0.3,0.125,-0.05,-0.175,-0.3,-0.345,-0.39,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5 +0.05,0.45,0.3175,0.185,0.0775,-0.03,-0.0675,-0.105,-0.1225,-0.14,-0.16,-0.18,-0.215,-0.25,-0.275,-0.3,-0.315,-0.33,-0.345,-0.36,-0.375,-0.39 +0.1,0.6,0.51,0.42,0.33,0.24,0.21,0.18,0.15,0.12,0.085,0.05,-0.015,-0.08,-0.12,-0.16,-0.18,-0.2,-0.22,-0.24,-0.26,-0.28 +0.15,0.875,0.7875,0.7,0.605,0.51,0.45,0.39,0.345,0.3,0.2475,0.195,0.1425,0.09,0.055,0.02,-0.015,-0.05,-0.0725,-0.095,-0.125,-0.155 +0.2,1.15,1.065,0.98,0.88,0.78,0.69,0.6,0.54,0.48,0.41,0.34,0.3,0.26,0.23,0.2,0.15,0.1,0.075,0.05,0.01,-0.03 +0.25,1.45,1.37,1.29,1.195,1.1,1.025,0.95,0.88,0.81,0.74,0.67,0.625,0.58,0.54,0.5,0.455,0.41,0.3775,0.345,0.31,0.275 +0.3,1.75,1.675,1.6,1.51,1.42,1.36,1.3,1.22,1.14,1.07,1,0.95,0.9,0.85,0.8,0.76,0.72,0.68,0.64,0.61,0.58 +0.35,2.2,2.12,2.04,1.95,1.86,1.7875,1.715,1.63,1.545,1.46,1.375,1.3075,1.24,1.1825,1.125,1.0725,1.02,0.97,0.92,0.88,0.84 +0.4,2.65,2.565,2.48,2.39,2.3,2.215,2.13,2.04,1.95,1.85,1.75,1.665,1.58,1.515,1.45,1.385,1.32,1.26,1.2,1.15,1.1 +0.45,2.975,2.92,2.865,2.7875,2.71,2.6175,2.525,2.42,2.315,2.1825,2.05,1.9625,1.875,1.795,1.715,1.6575,1.6,1.5325,1.465,1.41,1.355 +0.5,3.3,3.275,3.25,3.185,3.12,3.02,2.92,2.8,2.68,2.515,2.35,2.26,2.17,2.075,1.98,1.93,1.88,1.805,1.73,1.67,1.61 diff --git a/vehicle/accel_brake_map_calibrator/data/brake_map.csv b/vehicle/accel_brake_map_calibrator/data/brake_map.csv new file mode 100644 index 00000000..adf2c809 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/data/brake_map.csv @@ -0,0 +1,10 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51 +0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96 +0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633 +0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 +0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 +0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 +0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 +0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv b/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv new file mode 100644 index 00000000..4e9dfb16 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv @@ -0,0 +1,18 @@ +default,0,0.695,1.39,2.085,2.78,3.475,4.17,4.865,5.56,6.25,6.94,7.635,8.33,9.025,9.72,10.415,11.11,11.805,12.5,13.195,13.89 +0,0.3,0.125,-0.05,-0.175,-0.3,-0.345,-0.39,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5 +0.05,0.295,0.12,-0.055,-0.18,-0.305,-0.35,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.425,-0.435,-0.445,-0.455,-0.465,-0.475,-0.485,-0.495,-0.505 +0.1,0.29,0.115,-0.06,-0.185,-0.31,-0.355,-0.4,-0.405,-0.41,-0.415,-0.42,-0.425,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5,-0.51 +0.15,-0.045,-0.1375,-0.23,-0.3725,-0.515,-0.5575,-0.6,-0.6075,-0.615,-0.625,-0.635,-0.6425,-0.65,-0.66,-0.67,-0.68,-0.69,-0.7025,-0.715,-0.725,-0.735 +0.2,-0.38,-0.39,-0.4,-0.56,-0.72,-0.76,-0.8,-0.81,-0.82,-0.835,-0.85,-0.86,-0.87,-0.88,-0.89,-0.9,-0.91,-0.925,-0.94,-0.95,-0.96 +0.25,-0.69,-0.705,-0.72,-0.91,-1.1,-1.1375,-1.175,-1.185,-1.195,-1.2075,-1.22,-1.23,-1.24,-1.25,-1.26,-1.26525,-1.2705,-1.27825,-1.286,-1.29125,-1.2965 +0.3,-1,-1.02,-1.04,-1.26,-1.48,-1.515,-1.55,-1.56,-1.57,-1.58,-1.59,-1.6,-1.61,-1.62,-1.63,-1.6305,-1.631,-1.6315,-1.632,-1.6325,-1.633 +0.35,-1.24,-1.255,-1.27,-1.4675,-1.665,-1.7325,-1.8,-1.8175,-1.835,-1.84025,-1.8455,-1.85075,-1.856,-1.86125,-1.8665,-1.867,-1.8675,-1.868,-1.8685,-1.869,-1.8695 +0.4,-1.48,-1.49,-1.5,-1.675,-1.85,-1.95,-2.05,-2.075,-2.1,-2.1005,-2.101,-2.1015,-2.102,-2.1025,-2.103,-2.1035,-2.104,-2.1045,-2.105,-2.1055,-2.106 +0.45,-1.485,-1.495,-1.505,-1.68,-1.855,-1.955,-2.055,-2.08,-2.105,-2.1055,-2.106,-2.1065,-2.107,-2.1075,-2.108,-2.1085,-2.109,-2.1095,-2.11,-2.1105,-2.111 +0.5,-1.49,-1.5,-1.51,-1.685,-1.86,-1.96,-2.06,-2.085,-2.11,-2.1105,-2.111,-2.1115,-2.112,-2.1125,-2.113,-2.1135,-2.114,-2.1145,-2.115,-2.1155,-2.116 +0.55,-1.495,-1.505,-1.515,-1.69,-1.865,-1.965,-2.065,-2.09,-2.115,-2.1155,-2.116,-2.1165,-2.117,-2.1175,-2.118,-2.1185,-2.119,-2.1195,-2.12,-2.1205,-2.121 +0.6,-1.5,-1.51,-1.52,-1.695,-1.87,-1.97,-2.07,-2.095,-2.12,-2.1205,-2.121,-2.1215,-2.122,-2.1225,-2.123,-2.1235,-2.124,-2.1245,-2.125,-2.1255,-2.126 +0.65,-1.505,-1.515,-1.525,-1.7,-1.875,-1.975,-2.075,-2.1,-2.125,-2.1255,-2.126,-2.1265,-2.127,-2.1275,-2.128,-2.1285,-2.129,-2.1295,-2.13,-2.1305,-2.131 +0.7,-1.51,-1.52,-1.53,-1.705,-1.88,-1.98,-2.08,-2.105,-2.13,-2.1305,-2.131,-2.1315,-2.132,-2.1325,-2.133,-2.1335,-2.134,-2.1345,-2.135,-2.1355,-2.136 +0.75,-1.845,-1.855,-1.865,-2.0775,-2.29,-2.365,-2.44,-2.4775,-2.515,-2.52775,-2.5405,-2.541,-2.5415,-2.542,-2.5425,-2.543,-2.5435,-2.544,-2.5445,-2.545,-2.5455 +0.8,-2.18,-2.19,-2.2,-2.45,-2.7,-2.75,-2.8,-2.85,-2.9,-2.925,-2.95,-2.9505,-2.951,-2.9515,-2.952,-2.9525,-2.953,-2.9535,-2.954,-2.9545,-2.955 diff --git a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp new file mode 100644 index 00000000..3580f054 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -0,0 +1,381 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ +#define ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ + +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "raw_vehicle_cmd_converter/accel_map.hpp" +#include "raw_vehicle_cmd_converter/brake_map.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +#include + +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/multi_array_dimension.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include "tier4_external_api_msgs/msg/calibration_status.hpp" +#include "tier4_external_api_msgs/msg/calibration_status_array.hpp" +#include "tier4_external_api_msgs/srv/get_accel_brake_map_calibration_data.hpp" +#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" +#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" +#include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include +#include + +namespace accel_brake_map_calibrator +{ + +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using geometry_msgs::msg::TwistStamped; +using nav_msgs::msg::OccupancyGrid; +using raw_vehicle_cmd_converter::AccelMap; +using raw_vehicle_cmd_converter::BrakeMap; +using std_msgs::msg::Float32MultiArray; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_external_api_msgs::msg::CalibrationStatus; +using tier4_vehicle_msgs::msg::ActuationCommandStamped; +using tier4_vehicle_msgs::msg::ActuationStatusStamped; +using visualization_msgs::msg::MarkerArray; + +using tier4_vehicle_msgs::srv::UpdateAccelBrakeMap; + +using Map = std::vector>; + +struct DataStamped +{ + DataStamped(const double _data, const rclcpp::Time & _data_time) + : data{_data}, data_time{_data_time} + { + } + double data; + rclcpp::Time data_time; +}; +using DataStampedPtr = std::shared_ptr; + +class AccelBrakeMapCalibrator : public rclcpp::Node +{ +private: + std::shared_ptr transform_listener_; + std::string csv_default_map_dir_; + rclcpp::Publisher::SharedPtr original_map_occ_pub_; + rclcpp::Publisher::SharedPtr update_map_occ_pub_; + rclcpp::Publisher::SharedPtr original_map_raw_pub_; + rclcpp::Publisher::SharedPtr update_map_raw_pub_; + rclcpp::Publisher::SharedPtr offset_covariance_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr data_count_pub_; + rclcpp::Publisher::SharedPtr data_count_with_self_pose_pub_; + rclcpp::Publisher::SharedPtr data_ave_pub_; + rclcpp::Publisher::SharedPtr data_std_pub_; + rclcpp::Publisher::SharedPtr index_pub_; + rclcpp::Publisher::SharedPtr update_suggest_pub_; + rclcpp::Publisher::SharedPtr current_map_error_pub_; + rclcpp::Publisher::SharedPtr updated_map_error_pub_; + rclcpp::Publisher::SharedPtr map_error_ratio_pub_; + rclcpp::Publisher::SharedPtr calibration_status_pub_; + + rclcpp::Subscription::SharedPtr velocity_sub_; + rclcpp::Subscription::SharedPtr steer_sub_; + rclcpp::Subscription::SharedPtr actuation_status_sub_; + rclcpp::Subscription::SharedPtr actuation_cmd_sub_; + + // Service + rclcpp::Service::SharedPtr update_map_dir_server_; + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr timer_output_csv_; + void initTimer(double period_s); + void initOutputCSVTimer(double period_s); + + TwistStamped::ConstSharedPtr twist_ptr_; + std::vector> twist_vec_; + std::vector accel_pedal_vec_; // for delayed pedal + std::vector brake_pedal_vec_; // for delayed pedal + SteeringReport::ConstSharedPtr steer_ptr_; + DataStampedPtr accel_pedal_ptr_; + DataStampedPtr brake_pedal_ptr_; + DataStampedPtr delayed_accel_pedal_ptr_; + DataStampedPtr delayed_brake_pedal_ptr_; + + // Diagnostic Updater + std::shared_ptr updater_ptr_; + bool is_default_map_ = true; + + int get_pitch_method_; + int update_method_; + int accel_brake_value_source_; + double acceleration_ = 0.0; + double acceleration_time_; + double pre_acceleration_ = 0.0; + double pre_acceleration_time_; + double jerk_ = 0.0; + double accel_pedal_speed_ = 0.0; + double brake_pedal_speed_ = 0.0; + double pitch_ = 0.0; + double update_hz_; + double velocity_min_threshold_; + double velocity_diff_threshold_; + double pedal_diff_threshold_; + double max_steer_threshold_; + double max_pitch_threshold_; + double max_jerk_threshold_; + double pedal_velocity_thresh_; + double max_accel_; + double min_accel_; + double pedal_to_accel_delay_; + int precision_; + std::string csv_calibrated_map_dir_; + std::string output_accel_file_; + std::string output_brake_file_; + + // for calculating differential of msg + const double dif_twist_time_ = 0.2; // 200ms + const double dif_pedal_time_ = 0.16; // 160ms + const std::size_t twist_vec_max_size_ = 100; + const std::size_t pedal_vec_max_size_ = 100; + const double timeout_sec_ = 0.1; + int max_data_count_; + const int max_data_save_num_ = 10000; + const double map_resolution_ = 0.1; + const double max_jerk_ = 5.0; + bool pedal_accel_graph_output_ = false; + bool progress_file_output_ = false; + + // Algorithm + AccelMap accel_map_; + BrakeMap brake_map_; + + // for evaluation + AccelMap new_accel_map_; + BrakeMap new_brake_map_; + std::vector part_original_accel_mse_que_; + std::vector full_original_accel_mse_que_; + // std::vector full_original_accel_esm_que_; + std::vector full_original_accel_l1_que_; + std::vector full_original_accel_sq_l1_que_; + std::vector new_accel_mse_que_; + std::size_t full_mse_que_size_ = 100000; + std::size_t part_mse_que_size_ = 3000; + double full_original_accel_rmse_ = 0.0; + double full_original_accel_error_l1norm_ = 0.0; + double part_original_accel_rmse_ = 0.0; + double new_accel_rmse_ = 0.0; + double update_suggest_thresh_; + + // Accel / Brake Map update + Map accel_map_value_; + Map brake_map_value_; + Map update_accel_map_value_; + Map update_brake_map_value_; + Map accel_offset_covariance_value_; + Map brake_offset_covariance_value_; + std::vector map_value_data_; + std::vector accel_vel_index_; + std::vector brake_vel_index_; + std::vector accel_pedal_index_; + std::vector brake_pedal_index_; + bool update_success_; + int update_success_count_ = 0; + int update_count_ = 0; + int lack_of_data_count_ = 0; + int failed_to_get_pitch_count_ = 0; + int too_large_pitch_count_ = 0; + int too_low_speed_count_ = 0; + int too_large_steer_count_ = 0; + int too_large_jerk_count_ = 0; + int invalid_acc_brake_count_ = 0; + int too_large_pedal_spd_count_ = 0; + int update_fail_count_ = 0; + + // for map update + double map_offset_ = 0.0; + double map_coef_ = 1.0; + double covariance_; + const double forgetting_factor_ = 0.999; + const double coef_update_skip_thresh_ = 0.1; + + // output log + std::ofstream output_log_; + + bool getCurrentPitchFromTF(double * pitch); + void timerCallback(); + void timerCallbackOutputCSV(); + void executeUpdate( + const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, + const int brake_pedal_index, const int brake_vel_index); + bool updateFourCellAroundOffset( + const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, + const int brake_pedal_index, const int brake_vel_index, const double measured_acc); + bool updateEachValOffset( + const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, + const int brake_pedal_index, const int brake_vel_index, const double measured_acc, + const double map_acc); + void updateTotalMapOffset(const double measured_acc, const double map_acc); + void callbackActuation( + const std_msgs::msg::Header header, const double accel, const double brake); + void callbackActuationCommand(const ActuationCommandStamped::ConstSharedPtr msg); + void callbackActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg); + void callbackVelocity(const VelocityReport::ConstSharedPtr msg); + void callbackSteer(const SteeringReport::ConstSharedPtr msg); + bool callbackUpdateMapService( + const std::shared_ptr request_header, + UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res); + bool getAccFromMap(const double velocity, const double pedal); + double lowpass(const double original, const double current, const double gain = 0.8); + double getPedalSpeed( + const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, + const double prev_pedal_speed); + double getAccel( + const TwistStamped::ConstSharedPtr & prev_twist, + const TwistStamped::ConstSharedPtr & current_twist); + double getJerk(); + bool indexValueSearch( + const std::vector value_index, const double value, const double value_thresh, + int * searched_index); + int nearestValueSearch(const std::vector value_index, const double value); + int nearestPedalSearch(); + int nearestVelSearch(); + void takeConsistencyOfAccelMap(); + void takeConsistencyOfBrakeMap(); + bool updateAccelBrakeMap(); + void publishFloat32(const std::string publish_type, const double val); + void publishUpdateSuggestFlag(); + double getPitchCompensatedAcceleration(); + void executeEvaluation(); + double calculateEstimatedAcc( + const double throttle, const double brake, const double vel, AccelMap & accel_map, + BrakeMap & brake_map); + double calculateAccelSquaredError( + const double throttle, const double brake, const double vel, AccelMap & accel_map, + BrakeMap & brake_map); + double calculateAccelErrorL1Norm( + const double throttle, const double brake, const double vel, AccelMap & accel_map, + BrakeMap & brake_map); + std::vector getMapColumnFromUnifiedIndex( + const Map & accel_map_value, const Map & brake_map_value, const std::size_t index); + double getPedalValueFromUnifiedIndex(const std::size_t index); + int getUnifiedIndexFromAccelBrakeIndex(const bool accel_map, const std::size_t index); + void pushDataToQue( + const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, + std::queue * que); + template + void pushDataToVec(const T data, const std::size_t max_size, std::vector * vec); + template + T getNearestTimeDataFromVec( + const T base_data, const double back_time, const std::vector & vec); + DataStampedPtr getNearestTimeDataFromVec( + DataStampedPtr base_data, const double back_time, const std::vector & vec); + double getAverage(const std::vector & vec); + double getStandardDeviation(const std::vector & vec); + bool isTimeout(const builtin_interfaces::msg::Time & stamp, const double timeout_sec); + bool isTimeout(const DataStampedPtr & data_stamped, const double timeout_sec); + + /* for covariance calculation */ + // mean value on each cell (counting method depends on the update algorithm) + Eigen::MatrixXd accel_data_mean_mat_; + Eigen::MatrixXd brake_data_mean_mat_; + // calculated variance on each cell + Eigen::MatrixXd accel_data_covariance_mat_; + Eigen::MatrixXd brake_data_covariance_mat_; + // number of data on each cell (counting method depends on the update algorithm) + Eigen::MatrixXd accel_data_num_; + Eigen::MatrixXd brake_data_num_; + + OccupancyGrid getOccMsg( + const std::string frame_id, const double height, const double width, const double resolution, + const std::vector & map_value); + + /* Diag*/ + void checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /* Debug */ + void publishMap( + const Map accel_map_value, const Map brake_map_value, const std::string publish_type); + void publishOffsetCovMap(const Map accel_map_value, const Map brake_map_value); + void publishCountMap(); + void publishIndex(); + bool writeMapToCSV( + std::vector vel_index, std::vector pedal_index, Map value_map, + std::string filename); + void addIndexToCSV(std::ofstream * csv_file); + void addLogToCSV( + std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, + const double pitched_accel, const double accel_pedal, const double brake_pedal, + const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, + const double steer, const double jerk, const double full_original_accel_mse, + const double part_original_accel_mse, const double new_accel_mse); + + mutable Float32MultiArrayStamped debug_values_; + enum DBGVAL { + CURRENT_SPEED = 0, + CURRENT_ACCEL_PEDAL = 1, + CURRENT_BRAKE_PEDAL = 2, + CURRENT_RAW_ACCEL = 3, + CURRENT_ACCEL = 4, + CURRENT_RAW_ACCEL_SPEED = 5, + CURRENT_ACCEL_SPEED = 6, + CURRENT_RAW_BRAKE_SPEED = 7, + CURRENT_BRAKE_SPEED = 8, + CURRENT_RAW_PITCH = 9, + CURRENT_PITCH = 10, + CURRENT_STEER = 11, + SUCCESS_TO_UPDATE = 12, + CURRENT_JERK = 13, + }; + static constexpr unsigned int num_debug_values_ = 14; + + enum GET_PITCH_METHOD { TF = 0, FILE = 1, NONE = 2 }; + + enum UPDATE_METHOD { + UPDATE_OFFSET_EACH_CELL = 0, + UPDATE_OFFSET_TOTAL = 1, + UPDATE_OFFSET_FOUR_CELL_AROUND = 2, + }; + + enum ACCEL_BRAKE_SOURCE { + STATUS = 0, + COMMAND = 1, + }; + + std::unique_ptr logger_configure_; + +public: + explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); +}; + +} // namespace accel_brake_map_calibrator + +#endif // ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ diff --git a/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml new file mode 100644 index 00000000..c664158a --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png b/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png new file mode 100644 index 00000000..c29781b2 Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png differ diff --git a/vehicle/accel_brake_map_calibrator/media/add_panel.png b/vehicle/accel_brake_map_calibrator/media/add_panel.png new file mode 100644 index 00000000..f045042e Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/add_panel.png differ diff --git a/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png b/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png new file mode 100644 index 00000000..f21a2549 Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png differ diff --git a/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png b/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png new file mode 100644 index 00000000..28db957c Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png differ diff --git a/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png b/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png new file mode 100644 index 00000000..8929f3d6 Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png differ diff --git a/vehicle/accel_brake_map_calibrator/media/log_sample.png b/vehicle/accel_brake_map_calibrator/media/log_sample.png new file mode 100644 index 00000000..13887a42 Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/log_sample.png differ diff --git a/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png b/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png new file mode 100644 index 00000000..e57ed269 Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png differ diff --git a/vehicle/accel_brake_map_calibrator/media/statistics_sample.png b/vehicle/accel_brake_map_calibrator/media/statistics_sample.png new file mode 100644 index 00000000..c36ce9a5 Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/media/statistics_sample.png differ diff --git a/vehicle/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/package.xml new file mode 100644 index 00000000..64e7d1fb --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/package.xml @@ -0,0 +1,42 @@ + + + + accel_brake_map_calibrator + 0.1.0 + The accel_brake_map_calibrator + Tomoya Kimura + Taiki Tanaka + Takeshi Miura + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_vehicle_msgs + diagnostic_updater + geometry_msgs + motion_utils + raw_vehicle_cmd_converter + rclcpp + std_msgs + std_srvs + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + tier4_external_api_msgs + tier4_vehicle_msgs + visualization_msgs + + python3-matplotlib + python3-pandas + python3-scipy + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz b/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz new file mode 100644 index 00000000..ffcb63f4 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz @@ -0,0 +1,141 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Map1 + - /Axes1 + - /MarkerArray1 + Splitter Ratio: 0.5581113696098328 + Tree Height: 1615 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: /accel_brake_map_calibrator/debug/data_count_self_pose_occ_map + Unreliable: false + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 0.10000000149011612 + Name: Axes + Radius: 0.009999999776482582 + Reference Frame: + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + occ_pedal_index: true + occ_vel_index: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /accel_brake_map_calibrator/debug/occ_index + Value: true + Enabled: true + Global Options: + Background Color: 238; 238; 236 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_plugins/PedestrianInitialPoseTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Theta std deviation: 0.0872664600610733 + Velocity: 1 + X std deviation: 0.009999999776482582 + Y std deviation: 0.009999999776482582 + Z position: 0 + Z std deviation: 0.009999999776482582 + - Class: rviz_plugins/CarInitialPoseTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Theta std deviation: 0.0872664600610733 + Velocity: 3 + X std deviation: 0.009999999776482582 + Y std deviation: 0.009999999776482582 + Z position: 0 + Z std deviation: 0.009999999776482582 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 1.8697248697280884 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.48724469542503357 + Y: 0.3405919671058655 + Z: 5.960464477539062e-7 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 4.710395336151123 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1862 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000005070000068cfc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000068c000000c900fffffffb0000000a00560069006500770073000000030c000003bd000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000005470000012b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d00610067006501000005d6000000f30000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000000a0049006d00610067006500000002ef000003da0000000000000000000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006fb0000005afc0100000002fb0000000800540069006d00650100000000000006fb000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006fb0000068c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Views: + collapsed: false + Width: 1787 + X: 1193 + Y: 165 diff --git a/vehicle/accel_brake_map_calibrator/scripts/__init__.py b/vehicle/accel_brake_map_calibrator/scripts/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py b/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py new file mode 100755 index 00000000..f73718f7 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py @@ -0,0 +1,57 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2022 Tier IV, Inc. All rights reserved. +# +# 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 rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float32Stamped + +MAX_ACCEL = 1.0 # [-] +MIN_ACCEL = 0.0 # [-] + + +class AccelTester(Node): + def __init__(self): + super().__init__("vehicle_accel_tester") + self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/accel", 1) + + def run(self): + while rclpy.ok(): + value = float(input(f"target accel [{MIN_ACCEL} ~ {MAX_ACCEL} -] > ")) + if value > MAX_ACCEL: + print("input value is larger than max accel!" + f"input: {value} max: {MAX_ACCEL}") + value = MAX_ACCEL + elif value < MIN_ACCEL: + print("input value is smaller than min accel!" + f"input: {value} min: {MIN_ACCEL}") + value = MIN_ACCEL + + msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value) + + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + accel_tester = AccelTester() + accel_tester.run() + + accel_tester.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py new file mode 100755 index 00000000..749c9e42 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py @@ -0,0 +1,83 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2022 Tier IV, Inc. All rights reserved. +# +# 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 autoware_auto_vehicle_msgs.msg import GearCommand +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float32Stamped +from tier4_vehicle_msgs.msg import ActuationCommandStamped + + +class ActuationCmdPublisher(Node): + def __init__(self): + super().__init__("actuation_cmd_publisher") + self.target_brake = 0.0 + self.target_accel = 0.0 + + self.sub_acc = self.create_subscription( + Float32Stamped, "/vehicle/tester/accel", self.on_accel, 1 + ) + self.sub_brk = self.create_subscription( + Float32Stamped, "/vehicle/tester/brake", self.on_brake, 1 + ) + self.pub_actuation_cmd = self.create_publisher( + ActuationCommandStamped, "/control/command/actuation_cmd", 1 + ) + self.pub_gear_cmd = self.create_publisher(GearCommand, "/control/command/gear_cmd", 1) + timer_period = 0.02 # seconds + self.timer = self.create_timer(timer_period, self.on_timer) + + def on_brake(self, msg): + self.target_brake = msg.data + print(f"Set target brake : {self.target_brake}") + + def on_accel(self, msg): + self.target_accel = msg.data + print(f"Set target accel : {self.target_accel}") + + def on_timer(self): + msg_actuation_cmd = ActuationCommandStamped() + msg_actuation_cmd.actuation.steer_cmd = 0.0 + msg_actuation_cmd.header.stamp = self.get_clock().now().to_msg() + msg_actuation_cmd.header.frame_id = "base_link" + msg_actuation_cmd.actuation.accel_cmd = self.target_accel + msg_actuation_cmd.actuation.brake_cmd = self.target_brake + self.pub_actuation_cmd.publish(msg_actuation_cmd) + + msg_gear_cmd = GearCommand() + msg_gear_cmd.stamp = self.get_clock().now().to_msg() + msg_gear_cmd.command = GearCommand.DRIVE + self.pub_gear_cmd.publish(msg_gear_cmd) + + print( + f"publish ActuationCommand with accel: {self.target_accel}, brake: {self.target_brake}" + ) + + +def main(args=None): + rclpy.init(args=args) + + actuation_cmd_publisher = ActuationCmdPublisher() + + rclpy.spin(actuation_cmd_publisher) + + actuation_cmd_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py b/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py new file mode 100755 index 00000000..050f2323 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py @@ -0,0 +1,61 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2022 Tier IV, Inc. All rights reserved. +# +# 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 rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float32Stamped + +MAX_BRAKE = 1.0 # [-] +MIN_BRAKE = 0.0 # [-] + + +class BrakeTester(Node): + def __init__(self): + super().__init__("vehicle_brake_tester") + self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/brake", 1) + + def run(self): + while rclpy.ok(): + value = float( + input("target brake [" + str(MIN_BRAKE) + " ~ " + str(MAX_BRAKE) + "] > ") + ) + + if value > MAX_BRAKE: + print("input value is larger than max brake!" + f"input: {value} max: {MAX_BRAKE}") + value = MAX_BRAKE + elif value < MIN_BRAKE: + print("input value is smaller than min brake!" + f"input: {value} min: {MIN_BRAKE}") + value = MIN_BRAKE + + msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value) + + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + brake_tester = BrakeTester() + brake_tester.run() + + brake_tester.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py b/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py new file mode 100755 index 00000000..f4581699 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py @@ -0,0 +1,151 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 sys + +import numpy as np +from scipy import signal + +BIT = 1e-04 + + +def get_map_list(y_num, x_num): + data_list = [] + for yn in range(y_num): + child_data_list = [] + for xn in range(x_num): + child_data_list.append([]) + if xn == x_num - 1: + data_list.append(child_data_list) + return data_list + + +class CalcUtils: + @staticmethod + def average_filter(data, average_num): + if type(average_num) != int: + print( + "Error in average_filter(data, average_num):\ + Type of average_num must be int" + ) + sys.exit(1) + + if average_num % 2 == 0: + print( + "Error in average_filter(data, average_num):\ + average_num must be odd number" + ) + sys.exit(1) + + average_filter = np.ones(average_num) / float(average_num) + average_data = np.convolve(data, average_filter) + cut_num = (average_num - 1) / 2 + return average_data[cut_num : len(average_data) - cut_num] + + @staticmethod + # fp:pass Hz, #fs: block Hz, g_pass: pass dB, g_stop: stop DB + def lowpass_filter_scipy(x, sample_rate, fp, fs, g_pass, g_stop): + fn = sample_rate / 2 + wp = fp / fn + ws = fs / fn + N, Wn = signal.buttord(wp, ws, g_pass, g_stop) + b, a = signal.butter(N, Wn, "low") + y = signal.filtfilt(b, a, x) + return y + + @staticmethod + def create_2d_map( + x, + y, + data, + color_factor, + x_index_list, + x_thresh, + y_index_list, + y_thresh, + calibration_method="four_cell", + ): + if x.shape != y.shape or y.shape != data.shape: + print("Error: the shape of x, y, data must be same") + sys.exit() + data_size = len(x) + + x_num = len(x_index_list) + y_num = len(y_index_list) + data_list = get_map_list(y_num, x_num) + full_data_list = get_map_list(y_num, x_num) + + if calibration_method == "four_cell": + x_thresh = np.abs(x_index_list[1] - x_index_list[0]) / 2 + y_thresh = np.abs(y_index_list[1] - y_index_list[0]) / 2 + + for i in range(0, data_size): + x_index = None + y_index = None + for xi in range(0, x_num): + if np.abs(x_index_list[xi] - x[i]) < x_thresh: + x_index = xi + break + for yi in range(0, y_num): + if np.abs(y_index_list[yi] - y[i]) < y_thresh: + y_index = yi + break + + if x_index is not None and y_index is not None: + # append data + data_list[y_index][x_index].append(data[i]) + full_data_list[y_index][x_index].append([x[i], y[i], data[i], color_factor[i]]) + + return data_list, full_data_list + + @staticmethod + def extract_x_index_map(data_map, x_index): + y_num = len(data_map) + extracted_data = None + # x_num = len(data_map[0]) + for y in range(y_num): + data = np.array(data_map[y][x_index]) + if len(data) == 0: + continue + elif extracted_data is None: + extracted_data = data + else: + extracted_data = np.concatenate([extracted_data, data]) + return extracted_data + + @staticmethod + def create_stat_map(data_map, statistics_type="average"): + y_num = len(data_map) + x_num = len(data_map[0]) + count_map = np.zeros((y_num, x_num)) + average_map = np.zeros((y_num, x_num)) + stddev_map = np.zeros((y_num, x_num)) + for y in range(y_num): + for x in range(x_num): + data = np.array(data_map[y][x]) + count_map[y][x] = data.shape[0] + if count_map[y][x] == 0: + # print('Warn: data_map', y, x, 'is vacant list') + average_map[y][x] = 0.0 + stddev_map[y][x] = 0.0 + else: + if statistics_type == "average": + average_map[y][x] = np.average(data) + elif statistics_type == "median": + average_map[y][x] = np.median(data) + stddev_map[y][x] = np.std(data) + return count_map, average_map, stddev_map diff --git a/vehicle/accel_brake_map_calibrator/scripts/config.py b/vehicle/accel_brake_map_calibrator/scripts/config.py new file mode 100755 index 00000000..c554c406 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/config.py @@ -0,0 +1,44 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 numpy as np + +# config of file index +TS = "timestamp" +VEL = "velocity" +RAW_ACC = "accel" +PITCH_ACC = "pitch_comp_accel" +ACC = "final_accel" +A_PED = "accel_pedal" +B_PED = "brake_pedal" +A_PED_SPD = "accel_pedal_speed" +B_PED_SPD = "brake_pedal_speed" +PITCH = "pitch" +JERK = "jerk" +STEER = "steer" + +# config of accel / brake map +VEL_LIST = np.array([0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50]) # km +PEDAL_LIST = np.array( + [-0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5] +) +VEL_MIN = VEL_LIST[0] +VEL_MAX = VEL_LIST[-1] +VEL_SPAN = (VEL_MAX - VEL_MIN) / (len(VEL_LIST) - 1) +PEDAL_MIN = PEDAL_LIST[0] +PEDAL_MAX = PEDAL_LIST[-1] +PEDAL_SPAN = (PEDAL_MAX - PEDAL_MIN) / (len(PEDAL_LIST) - 1) diff --git a/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py b/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py new file mode 100755 index 00000000..c4494cdb --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py @@ -0,0 +1,132 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 glob + +import config as CF +import numpy as np +import pandas as pd + +# pre-defined +NUMERIC_LIMITS = 1e-02 + + +class CSVReader(object): + def __init__(self, csv, csv_type="dir"): + if csv_type == "dir": + csv_files = glob.glob(csv + "/*.csv") + csv_list = [] + for cf in csv_files: + csv_list.append(pd.read_csv(cf, engine="python")) + self.csv_data = pd.concat(csv_list, ignore_index=True) + else: + self.csv_data = pd.read_csv(csv, engine="python") + + def removeUnusedData( + self, + min_vel_thr, + max_steer_thr, + max_pitch_thr, + max_pedal_vel_thr, + max_jerk_thr, + remove_by_invalid_pedal=True, + remove_by_vel=True, + remove_by_steer=True, + remove_by_pitch=True, + remove_by_pedal_speed=True, + remove_by_jerk=True, + ): + # remove unused data + raw_size = len(self.csv_data) + + for i in range(0, raw_size)[::-1]: + vel = self.csv_data[CF.VEL][i] + steer = self.csv_data[CF.STEER][i] + accel_pedal = self.csv_data[CF.A_PED][i] + brake_pedal = self.csv_data[CF.B_PED][i] + accel_pedal_speed = self.csv_data[CF.A_PED_SPD][i] + brake_pedal_speed = self.csv_data[CF.B_PED_SPD][i] + jerk = self.csv_data[CF.JERK][i] + pitch = self.csv_data[CF.PITCH][i] + + # invalid pedal + if ( + remove_by_invalid_pedal + and accel_pedal > NUMERIC_LIMITS + and brake_pedal > NUMERIC_LIMITS + ): + self.csv_data = self.csv_data.drop(i) + continue + + # low velocity + if remove_by_vel and vel < min_vel_thr: + self.csv_data = self.csv_data.drop(i) + continue + + # high steer + if remove_by_steer and np.abs(steer) > max_steer_thr: + self.csv_data = self.csv_data.drop(i) + continue + + # high pitch + if remove_by_pitch and np.abs(pitch) > max_pitch_thr: + self.csv_data = self.csv_data.drop(i) + continue + + # high pedal speed + if ( + remove_by_pedal_speed + and np.abs(accel_pedal_speed) > max_pedal_vel_thr + or np.abs(brake_pedal_speed) > max_pedal_vel_thr + ): + self.csv_data = self.csv_data.drop(i) + continue + + # max_jerk_thr + if remove_by_jerk and np.abs(jerk) > max_jerk_thr: + self.csv_data = self.csv_data.drop(i) + continue + + return self.csv_data + + def getVelData(self): + vel_data = np.array(self.csv_data[CF.VEL]) + return vel_data + + def getPedalData(self): + pedal_data = np.array(self.csv_data[CF.A_PED]) - np.array(self.csv_data[CF.B_PED]) + return pedal_data + + def getAccData(self): + acc_data = np.array(self.csv_data[CF.ACC]) + return acc_data + + def getPitchData(self): + pitch_data = np.array(self.csv_data[CF.PITCH]) + return pitch_data + + def extractPedalRangeWithDelay(self, delay_step, pedal_value, pedal_diff_thr): + csv_data = self.csv_data.reset_index(drop=True) + + for i in range(delay_step, len(self.csv_data))[::-1]: + pedal = csv_data[CF.A_PED][i - delay_step] - csv_data[CF.B_PED][i - delay_step] + # extract data of pedal = pedal_value + if pedal > pedal_value + pedal_diff_thr or pedal < pedal_value - pedal_diff_thr: + csv_data = csv_data.drop(i) + continue + + return csv_data diff --git a/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py b/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py new file mode 100755 index 00000000..eb2bce38 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py @@ -0,0 +1,144 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 ament_index_python.packages import get_package_share_directory +from calc_utils import CalcUtils +import config as CF +from csv_reader import CSVReader +import numpy as np +from plotter import Plotter +import rclpy +from rclpy.node import Node + + +class DelayEstimator(Node): + def __init__(self): + # get parameter + super().__init__("delay_estimator") + package_path = get_package_share_directory("accel_brake_map_calibrator") + self.declare_parameter("log_file", package_path + "/config/log.csv") + log_file = self.get_parameter("log_file").get_parameter_value().string_value + self.declare_parameter("velocity_min_threshold", 0.1) + min_vel_thr = ( + self.get_parameter("velocity_min_threshold").get_parameter_value().double_value + ) + self.declare_parameter("velocity_diff_threshold", 0.556) + vel_diff_thr = ( + self.get_parameter("velocity_diff_threshold").get_parameter_value().double_value + ) + self.declare_parameter("pedal_diff_threshold", 0.03) + pedal_diff_thr = ( + self.get_parameter("pedal_diff_threshold").get_parameter_value().double_value + ) + self.declare_parameter("max_steer_threshold", 0.2) + max_steer_thr = self.get_parameter("max_steer_threshold").get_parameter_value().double_value + self.declare_parameter("max_pitch_threshold", 0.02) + max_pitch_thr = self.get_parameter("max_pitch_threshold").get_parameter_value().double_value + self.declare_parameter("max_jerk_threshold", 0.7) + max_jerk_thr = self.get_parameter("max_jerk_threshold").get_parameter_value().double_value + self.declare_parameter("pedal_velocity_thresh", 0.15) + max_pedal_vel_thr = ( + self.get_parameter("pedal_velocity_thresh").get_parameter_value().double_value + ) + self.declare_parameter("update_hz", 10.0) + update_hz = self.get_parameter("update_hz").get_parameter_value().double_value + + self.data_span = 1.0 / update_hz + + # read csv + self.cr = CSVReader(log_file, csv_type="file") + # remove unused_data + self.csv_data = self.cr.removeUnusedData( + min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr + ) + + # get statistics array + vel_data = self.cr.getVelData() + pedal_data = self.cr.getPedalData() + acc_data = self.cr.getAccData() + + # get color factor (pitch) array for plotting + color_data = self.cr.getPitchData() + + data, full_data = CalcUtils.create_2d_map( + vel_data, + pedal_data, + acc_data, + color_data, + CF.VEL_LIST / 3.6, + vel_diff_thr, + CF.PEDAL_LIST, + pedal_diff_thr, + ) + + # plot all data + # value to use for statistics + PEDAL_VALUE = 0.1 + VEL_VALUE_LIST = np.array([10, 15, 20]) / 3.6 + plotter = Plotter(3, 2) + max_delay_step = 5 + for delay_step in range(max_delay_step + 1): + print("data processing... " + str(delay_step) + " / " + str(max_delay_step)) + csv_data = self.cr.extractPedalRangeWithDelay(delay_step, PEDAL_VALUE, pedal_diff_thr) + + # get correlation coefficient + # extract data of velocity is VEL_VALUE + coef_list = [] + for vel_value in VEL_VALUE_LIST: + ex_csv_data = csv_data[csv_data[CF.VEL] < vel_value + vel_diff_thr] + ex_csv_data = csv_data[csv_data[CF.VEL] > vel_value - vel_diff_thr] + pedal_speed = ex_csv_data[CF.A_PED_SPD] - ex_csv_data[CF.B_PED_SPD] + accel = ex_csv_data[CF.ACC] + coef = self.getCorCoef(pedal_speed, accel) + coef_list.append(coef) + + print("delay_step: ", delay_step) + print("coef: ", coef_list) + + ave_coef = np.average(coef_list) + self.plotPedalSpeedAndAccel(csv_data, plotter, delay_step + 1, delay_step, ave_coef) + plotter.show() + + def getCorCoef(self, a, b): + coef = np.corrcoef(np.array(a), np.array(b)) + return coef[0, 1] + + def plotPedalSpeedAndAccel(self, csv_data, plotter, subplot_num, delay_step, coef): + pedal_speed = csv_data[CF.A_PED_SPD] - csv_data[CF.B_PED_SPD] + accel = csv_data[CF.ACC] + velocity = csv_data[CF.VEL] * 3.6 + fig = plotter.subplot(subplot_num) + plotter.scatter_color(pedal_speed, accel, velocity, "hsv", label=None) + delay_time_ms = delay_step * self.data_span * 1000 + plotter.add_label( + "pedal-spd-acc (delay = " + str(delay_time_ms) + " ms), R = " + str(coef), + "pedal-speed", + "accel", + ) + plotter.set_lim(fig, [-0.4, 0.4], [-1.0, 1.0]) + + +def main(args=None): + rclpy.init(args=args) + node = DelayEstimator() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py b/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py new file mode 100755 index 00000000..71cdb502 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py @@ -0,0 +1,303 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 calc_utils import CalcUtils +import config as CF +from csv_reader import CSVReader +import numpy as np +from plotter import Plotter + +# parameter for statistics +SCATTER_GRAPH_PEDAL_LIM = [-0.55, 0.55] +SCATTER_GRAPH_ACCEL_LIM = [-1.8, 1.8] +CONSIDER_PEDAL_VAL_FOR_STAT = [0.0, 0.1, 0.2] + +parser = argparse.ArgumentParser() +parser.add_argument("--csv-file-dir", help="log file directory") +parser.add_argument("--min-vel-thr", type=float, default=0.1, help="thresh of minimum velocity") +parser.add_argument( + "--max-pedal-vel-thr", type=float, default=0.15, help="thresh of maximum pedal velocity" +) +parser.add_argument("--max-steer-thr", type=float, default=0.2, help="thresh of maximum steer") +parser.add_argument("--max-pitch-thr", type=float, default=0.02, help="thresh of maximum pitch") +parser.add_argument("--max-jerk-thr", type=float, default=0.7, help="thresh of maximum jerk") +parser.add_argument( + "--pedal-diff-thr", + type=float, + default=0.03, + help="thresh of max delta-pedal to add statistics map", +) +parser.add_argument( + "--vel-diff-thr", + type=float, + default=0.556, + help="thresh of max delta-velocity to add statistics map", +) +parser.add_argument( + "--view-velocity-1", type=int, default=5, help="velocity(kmh) to visualize by plot (1)" +) +parser.add_argument( + "--view-velocity-2", type=int, default=20, help="velocity(kmh) to visualize by plot (2)" +) +parser.add_argument( + "--view-velocity-3", type=int, default=30, help="velocity(kmh) to visualize by plot (3)" +) +parser.add_argument("--output-stat-csv", default="stat.csv", help="output csv file name") +parser.add_argument("--output-plot-file", default="result.png", help="output plot file name") +parser.add_argument("--no-plot", action="store_true") + +args = parser.parse_args() +# get path +csv_file_dir = args.csv_file_dir +min_vel_thr = args.min_vel_thr +max_pedal_vel_thr = args.max_pedal_vel_thr +max_steer_thr = args.max_steer_thr +max_pitch_thr = args.max_pitch_thr +max_jerk_thr = args.max_jerk_thr +pedal_diff_thr = args.pedal_diff_thr +vel_diff_thr = args.vel_diff_thr +view_velocity_list = [] +view_velocity_list.append(args.view_velocity_1) +view_velocity_list.append(args.view_velocity_2) +view_velocity_list.append(args.view_velocity_3) +output_stat_csv = args.output_stat_csv +output_plot_file = args.output_plot_file +remove_by_invalid_pedal = True +remove_by_vel = True +remove_by_steer = True +remove_by_pitch = True +remove_by_pedal_speed = True +remove_by_jerk = True +is_plot = not args.no_plot + +view_velocity_idx_list = [] +for vv in view_velocity_list: + if not (vv in CF.VEL_LIST): + print("invalid view_velocity. velocity list is: ", CF.VEL_LIST) + sys.exit() + view_velocity_idx_list.append(CF.VEL_LIST.tolist().index(vv)) + +# search index of pedal to use for statistics +stat_pedal_list = [] +for val in CONSIDER_PEDAL_VAL_FOR_STAT: + if not (val in CF.PEDAL_LIST): + print("invalid CONSIDER_PEDAL_VAL_FOR_STAT. pedal list is:", CF.PEDAL_LIST) + stat_pedal_list.append(CF.PEDAL_LIST.tolist().index(val)) + +# read file +cr = CSVReader(csv_file_dir) + +# remove unused_data +csv_data = cr.removeUnusedData( + min_vel_thr, + max_steer_thr, + max_pitch_thr, + max_pedal_vel_thr, + max_jerk_thr, + remove_by_invalid_pedal, + remove_by_vel, + remove_by_steer, + remove_by_pitch, + remove_by_pedal_speed, + remove_by_jerk, +) + + +# get statistics array +vel_data = cr.getVelData() +pedal_data = cr.getPedalData() +acc_data = cr.getAccData() + +# get color factor (pitch) array for plotting +color_data = cr.getPitchData() + +data, full_data = CalcUtils.create_2d_map( + vel_data, + pedal_data, + acc_data, + color_data, + CF.VEL_LIST / 3.6, + vel_diff_thr, + CF.PEDAL_LIST, + pedal_diff_thr, +) + +count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) +velocity_map_list = [] +for v_idx in view_velocity_idx_list: + velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, v_idx)) + +# output statistics +f = open(output_stat_csv, "a") +count_list_over_v = [] +stddev_list_over_v = [] +for vi in view_velocity_idx_list: + count_list = [] + stddev_list = [] + for pi in stat_pedal_list: + count_list.append(count_map[pi][vi]) + if count_map[pi][vi] > 5: + stddev_list.append(stddev_map[pi][vi]) + else: + print("Warning: count is fewer than 5.") + count_sum = int(np.sum(count_list)) + stddev_ave = np.average(stddev_list) + count_list_over_v.append(count_sum) + stddev_list_over_v.append(stddev_ave) + f.write(str(count_sum) + ",") + f.write(str(stddev_ave) + ",") + print("velocity index:", vi) + print("\tcount: ", count_sum) + print("\tstddev: ", stddev_ave) +count_sum_over_v = int(np.sum(count_list_over_v)) +stddev_ave_over_v = np.average(stddev_list_over_v) +f.write(str(count_sum_over_v) + ",") +f.write(str(stddev_ave_over_v) + "\n") +f.close() +print("full data:") +print("\tcount: ", count_sum_over_v) +print("\tstddev: ", stddev_ave_over_v) + +# visualization +plotter = Plotter(2, 3) +plotter.subplot(1) +plotter.imshow( + average_map, CF.VEL_MIN, CF.VEL_MAX, CF.VEL_SPAN, CF.PEDAL_MIN, CF.PEDAL_MAX, CF.PEDAL_SPAN +) +plotter.add_label("average of accel", "velocity(kmh)", "throttle") + +plotter.subplot(2) +plotter.imshow( + stddev_map, CF.VEL_MIN, CF.VEL_MAX, CF.VEL_SPAN, CF.PEDAL_MIN, CF.PEDAL_MAX, CF.PEDAL_SPAN +) +plotter.add_label("std. dev. of accel", "velocity(kmh)", "throttle") + +plotter.subplot(3) +plotter.imshow( + count_map, + CF.VEL_MIN, + CF.VEL_MAX, + CF.VEL_SPAN, + CF.PEDAL_MIN, + CF.PEDAL_MAX, + CF.PEDAL_SPAN, + num_data_type="int", +) +plotter.add_label("number of accel data", "velocity(kmh)", "throttle") + +# view pedal-accel graph + + +def view_pedal_accel_graph(subplot_num, vel_list_idx): + # plot all data + fig = plotter.subplot(subplot_num) + plotter.scatter( + velocity_map_list[vel_list_idx][:, 1], + velocity_map_list[vel_list_idx][:, 2], + "blue", + label="all", + ) + + # plot average data + # remove average score of no data + average_map_v_avoid_0 = ( + average_map[:, view_velocity_idx_list[vel_list_idx]] + + np.where(average_map[:, view_velocity_idx_list[vel_list_idx]] == 0, True, False) * 1e03 + ) + + plotter.scatter(CF.PEDAL_LIST, average_map_v_avoid_0, "red", label="average") + + plotter.set_lim(fig, SCATTER_GRAPH_PEDAL_LIM, SCATTER_GRAPH_ACCEL_LIM) + plotter.add_label( + str(view_velocity_list[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" + ) + + +view_pedal_accel_graph(4, 0) +view_pedal_accel_graph(5, 1) +view_pedal_accel_graph(6, 2) + +if is_plot: + plotter.show() +plotter.save(output_plot_file) + + +# pedal-pitch plot +""" +cr = CSVReader(csv_file_dir) +csv_data = cr.removeUnusedData( + min_vel_thr, max_steer_thr, max_pitch_thr, + max_pedal_vel_thr, max_jerk_thr, + remove_by_invalid_pedal, + remove_by_vel=True, + remove_by_steer=False, + remove_by_pitch=False, + remove_by_pedal_speed=False, + remove_by_jerk=False) +pitch = csv_data[CF.PITCH] +pedal = csv_data[CF.A_PED] - csv_data[CF.B_PED] +velocity = csv_data[CF.VEL] * 3.6 # color +plotter.scatter_color(pedal, pitch, velocity, (0.0, 35.0), "hsv", label=None) +plotter.add_label("pedal-pitch relation", "pedal", "pitch") +plotter.show() +""" + +""" +# pedal-speed-accel plot +cr = CSVReader(csv_file_dir) +csv_data = cr.removeUnusedData( + min_vel_thr, max_steer_thr, max_pitch_thr, + max_pedal_vel_thr, max_jerk_thr, + remove_by_invalid_pedal, + remove_by_vel=True, + remove_by_steer=True, + remove_by_pitch=True, + remove_by_pedal_speed=False, + remove_by_jerk=False) + +csv_data = csv_data.reset_index(drop=True) +delay = 3 # accel delay (*100ms) +for i in range(delay, len(csv_data))[::-1]: + pedal = csv_data[CF.A_PED][i-delay] - csv_data[CF.B_PED][i-delay] + # extract data of pedal = 0.1 + tgt_ped = 0.1 + if pedal > tgt_ped + pedal_diff_thr or \ + pedal < tgt_ped - pedal_diff_thr: + csv_data = csv_data.drop(i) + continue + + velocity = csv_data[CF.VEL][i] + if velocity > 12.0 / 3.6 or \ + velocity < 8.0 / 3.6: + csv_data = csv_data.drop(i) + continue + +pedal_speed = csv_data[CF.A_PED_SPD] - csv_data[CF.B_PED_SPD] +accel = csv_data[CF.ACC] +velocity = csv_data[CF.VEL] * 3.6 +plotter = Plotter(1, 1) +fig = plotter.subplot(1) +plotter.scatter_color(pedal_speed, accel, velocity, + (0.0, 35.0), "hsv", label=None) +plotter.add_label("pedal-speed-accel relation (only pedal = 0.1)", + "pedal-speed", "accel") +plotter.set_lim(fig, [-0.4, 0.4], [-0.5, 1.0]) +plotter.show() +""" diff --git a/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py new file mode 100755 index 00000000..a2d82a46 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -0,0 +1,367 @@ +#! /usr/bin/env python3 + +# Copyright 2022 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 math +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +from calc_utils import CalcUtils +import config as CF +from csv_reader import CSVReader +import matplotlib.pyplot as plt +import numpy as np +from plotter import Plotter +import rclpy +from rclpy.node import Node +from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData +import yaml + + +class DrawGraph(Node): + calibrated_map_dir = "" + + def __init__(self): + super().__init__("plot_server") + + self.srv = self.create_service( + CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback + ) + + default_map_path = get_package_share_directory("raw_vehicle_cmd_converter") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_default_map_dir", default_map_path + "/data/default/" + ) + self.default_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") + .get_parameter_value() + .string_value + ) + + package_path = get_package_share_directory("accel_brake_map_calibrator") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" + ) + self.calibrated_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") + .get_parameter_value() + .string_value + ) + + self.declare_parameter("calibration_method", "each_cell") + self.calibration_method = ( + self.get_parameter("calibration_method").get_parameter_value().string_value + ) + if self.calibration_method is None: + self.calibration_method = "each_cell" + elif not ( + (self.calibration_method == "each_cell") | (self.calibration_method == "four_cell") + ): + print("invalid method.") + self.calibration_method = "each_cell" + + self.log_file = package_path + "/config/log.csv" + + config_file = package_path + "/config/accel_brake_map_calibrator.param.yaml" + if Path(config_file).exists(): + self.get_logger().info("config file exists") + with open(config_file) as yml: + data = yaml.safe_load(yml) + self.min_vel_thr = data["/**"]["ros__parameters"]["velocity_min_threshold"] + self.vel_diff_thr = data["/**"]["ros__parameters"]["velocity_diff_threshold"] + self.pedal_diff_thr = data["/**"]["ros__parameters"]["pedal_diff_threshold"] + self.max_steer_thr = data["/**"]["ros__parameters"]["max_steer_threshold"] + self.max_pitch_thr = data["/**"]["ros__parameters"]["max_pitch_threshold"] + self.max_jerk_thr = data["/**"]["ros__parameters"]["max_jerk_threshold"] + else: + self.get_logger().warning("config file is not found in {}".format(config_file)) + self.min_vel_thr = 0.1 + self.vel_diff_thr = 0.556 + self.pedal_diff_thr = 0.03 + self.max_steer_thr = 0.2 + self.max_pitch_thr = 0.02 + self.max_jerk_thr = 0.7 + + self.max_pedal_vel_thr = 0.7 + + # debug + self.get_logger().info("default map dir: {}".format(self.default_map_dir)) + self.get_logger().info("calibrated map dir: {}".format(self.calibrated_map_dir)) + self.get_logger().info("calibrated method: {}".format(self.calibration_method)) + self.get_logger().info("log file :{}".format(self.log_file)) + self.get_logger().info("min_vel_thr : {}".format(self.min_vel_thr)) + self.get_logger().info("vel_diff_thr : {}".format(self.vel_diff_thr)) + self.get_logger().info("pedal_diff_thr : {}".format(self.pedal_diff_thr)) + self.get_logger().info("max_steer_thr : {}".format(self.max_steer_thr)) + self.get_logger().info("max_pitch_thr : {}".format(self.max_pitch_thr)) + self.get_logger().info("max_jerk_thr : {}".format(self.max_jerk_thr)) + self.get_logger().info("max_pedal_vel_thr : {}".format(self.max_pedal_vel_thr)) + + def get_data_callback(self, request, response): + # read csv + # If log file doesn't exist, return empty data + if not Path(self.log_file).exists(): + response.graph_image = [] + self.get_logger().info("svg data is empty") + + response.accel_map = "" + self.get_logger().info("accel map is empty") + + response.brake_map = "" + self.get_logger().info("brake map is empty") + + return response + + self.cr = CSVReader(self.log_file, csv_type="file") + + # remove unused_data + self.csv_data = self.cr.removeUnusedData( + self.min_vel_thr, + self.max_steer_thr, + self.max_pitch_thr, + self.max_pedal_vel_thr, + self.max_jerk_thr, + ) + + # get statistics array + vel_data = self.cr.getVelData() + pedal_data = self.cr.getPedalData() + acc_data = self.cr.getAccData() + + # get color factor (pitch) array for plotting + color_data = self.cr.getPitchData() + + data, full_data = CalcUtils.create_2d_map( + vel_data, + pedal_data, + acc_data, + color_data, + CF.VEL_LIST / 3.6, + self.vel_diff_thr, + CF.PEDAL_LIST, + self.pedal_diff_thr, + self.calibration_method, + ) + + count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) + velocity_map_list = [] + for i in range(len(CF.VEL_LIST)): + velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, i)) + + default_pedal_list, default_acc_list = self.load_map(self.default_map_dir) + if len(default_pedal_list) == 0 or len(default_acc_list) == 0: + self.get_logger().warning( + "No default map file was found in {}".format(self.default_map_dir) + ) + + response.graph_image = [] + self.get_logger().info("svg data is empty") + + response.accel_map = "" + self.get_logger().info("accel map is empty") + + response.brake_map = "" + self.get_logger().info("brake map is empty") + + return response + + calibrated_pedal_list, calibrated_acc_list = self.load_map(self.calibrated_map_dir) + if len(calibrated_pedal_list) == 0 or len(calibrated_acc_list) == 0: + self.get_logger().warning( + "No calibrated map file was found in {}".format(self.calibrated_map_dir) + ) + + # visualize point from data + plot_width = 3 + plot_height = int(math.ceil(len(CF.VEL_LIST) / float(plot_width))) + plotter = Plotter(plot_height, plot_width) + for i in range(len(CF.VEL_LIST)): + self.view_pedal_accel_graph( + plotter, + i, + velocity_map_list, + i, + count_map, + average_map, + stddev_map, + default_pedal_list, + default_acc_list, + calibrated_pedal_list, + calibrated_acc_list, + ) + plt.savefig("plot.svg") + self.get_logger().info("svg saved") + + # pack response data + text = Path("plot.svg").read_text() + if text == "": + response.graph_image = [] + self.get_logger().info("svg data is empty") + else: + byte = text.encode() + for b in byte: + response.graph_image.append(b) + self.get_logger().info("svg data is packed") + + accel_map_name = Path(self.calibrated_map_dir + "accel_map.csv") + if accel_map_name.exists(): + with open(self.calibrated_map_dir + "accel_map.csv", "r") as calibrated_accel_map: + for accel_data in calibrated_accel_map: + response.accel_map += accel_data + self.get_logger().info("accel map is packed") + else: + response.accel_map = "" + self.get_logger().info("accel map is empty") + + brake_map_name = Path(self.calibrated_map_dir + "brake_map.csv") + if brake_map_name.exists(): + with open(self.calibrated_map_dir + "brake_map.csv", "r") as calibrated_brake_map: + for brake_data in calibrated_brake_map: + response.brake_map += brake_data + self.get_logger().info("brake map is packed") + else: + response.brake_map = "" + self.get_logger().info("brake map is empty") + + return response + + def plotter_function(self): + return self.plotter + + def view_pedal_accel_graph( + self, + plotter, + subplot_num, + velocity_map_list, + vel_list_idx, + count_map, + average_map, + stddev_map, + default_pedal_list, + default_acc_list, + calibrated_pedal_list, + calibrated_acc_list, + ): + fig = plotter.subplot_more(subplot_num) + + # calibrated map + if len(calibrated_pedal_list) != 0 and len(calibrated_acc_list) != 0: + plotter.plot( + calibrated_pedal_list[vel_list_idx], + calibrated_acc_list[vel_list_idx], + color="blue", + label="calibrated", + ) + + # default map + if len(default_pedal_list) != 0 and len(default_acc_list) != 0: + plotter.plot( + default_pedal_list[vel_list_idx], + default_acc_list[vel_list_idx], + color="orange", + label="default", + linestyle="dashed", + ) + + # plot all data + pedal_list = [0 for i in range(len(CF.PEDAL_LIST))] + if velocity_map_list[vel_list_idx] is not None: + plotter.scatter_color( + velocity_map_list[vel_list_idx][:, 1], + velocity_map_list[vel_list_idx][:, 2], + color=velocity_map_list[vel_list_idx][:, 3], + label="all", + ) + + for pedal in velocity_map_list[vel_list_idx][:, 1]: + min_pedal = 10 + for pedal_idx, ref_pedal in enumerate(CF.PEDAL_LIST): + if min_pedal > abs(pedal - ref_pedal): + min_pedal = abs(pedal - ref_pedal) + min_pedal_idx = pedal_idx + pedal_list[min_pedal_idx] += 1 + + # plot average data + plotter.scatter(CF.PEDAL_LIST, average_map[:, vel_list_idx], "red", label="average") + + # add label of standard deviation + plotter.scatter([], [], "black", label="std dev") + + # plot average text + for i in range(len(CF.PEDAL_LIST)): + if count_map[i, vel_list_idx] == 0: + continue + x = CF.PEDAL_LIST[i] + y = average_map[i, vel_list_idx] + y2 = stddev_map[i, vel_list_idx] + # plot average + plotter.plot_text(x, y + 1, y, color="red") + + # plot standard deviation + plotter.plot_text(x, y - 1, y2, color="black") + + # plot the number of all data + plotter.plot_text( + x, y - 2, "{}\npts".format(pedal_list[i]), num_data_type="str", color="green" + ) + + pedal_lim = [CF.PEDAL_LIST[0] - 0.05, CF.PEDAL_LIST[-1] + 0.05] + accel_lim = [-5.0, 5.0] + + plotter.set_lim(fig, pedal_lim, accel_lim) + plotter.add_label( + str(CF.VEL_LIST[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" + ) + + def load_map(self, csv_dir): + try: + accel_pedal_list = [] + accel_acc_list = [] + with open(csv_dir + "accel_map.csv") as f: + for l_idx, l in enumerate(f.readlines()): + w = l.split(",") + w[-1] = w[-1][:-1] + if l_idx != 0: + accel_pedal_list.append([float(w[0]) for e in w[1:]]) + accel_acc_list.append([float(e) for e in w[1:]]) + + brake_pedal_list = [] + brake_acc_list = [] + with open(csv_dir + "brake_map.csv") as f: + for l_idx, l in enumerate(f.readlines()): + w = l.split(",") + w[-1] = w[-1][:-1] + if l_idx != 0: + brake_pedal_list.append([-float(w[0]) for e in w[1:]]) + brake_acc_list.append([float(e) for e in w[1:]]) + + return np.hstack( + [np.fliplr(np.array(accel_pedal_list).T), np.array(brake_pedal_list).T.tolist()] + ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) + except OSError as e: + print(e) + return [], [] + + +def main(args=None): + rclpy.init(args=None) + node = DrawGraph() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/plotter.py b/vehicle/accel_brake_map_calibrator/scripts/plotter.py new file mode 100755 index 00000000..73e4cf54 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/plotter.py @@ -0,0 +1,123 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 matplotlib.pyplot as plt +import numpy as np + + +class Plotter(object): + def __init__(self, total_height, total_width, plot=True): + self.total_height = total_height + self.total_width = total_width + self.plot_flag = True + self.fig = plt.figure(dpi=100, figsize=(24, 18)) + plt.subplots_adjust(left=0.04, right=0.98, bottom=0.05, top=0.95, wspace=0.1, hspace=0.4) + + def subplot(self, plot_num): + fig = plt.subplot(self.total_height, self.total_width, plot_num) + return fig + + def subplot_more(self, plot_num): + width = plot_num % self.total_width + height = int((plot_num - width) / self.total_width) + fig = plt.subplot2grid((self.total_height, self.total_width), (height, width)) + return fig + + def plot_1d(self, data, color, label): + plt.plot(data, color, label=label) + + def plot(self, x_data, y_data, color, label=None, linestyle="solid"): + plt.plot(x_data, y_data, color, label=label, linestyle=linestyle, zorder=-1) + + def scatter(self, x, y, color, label=None, colorbar=False): + plt.scatter(x, y, c=color, label=label, s=3) + + def scatter_color(self, x, y, color, cmap=None, label=None): + sc = plt.scatter(x, y, c=color, label=label, cmap=cmap, s=3) + plt.colorbar(sc) + + def plot_text(self, x, y, val, num_data_type="float", color="black"): + if num_data_type == "float": + plt.text( + x, + y, + "{0:.2f}".format(float(val)), + horizontalalignment="center", + verticalalignment="center", + color=color, + clip_on=True, + fontsize=8, + ) + elif num_data_type == "int": + plt.text( + x, + y, + "{0:d}".format(int(val)), + horizontalalignment="center", + verticalalignment="center", + color=color, + clip_on=True, + fontsize=8, + ) + elif num_data_type == "str": + plt.text( + x, + y, + val, + horizontalalignment="center", + verticalalignment="center", + color=color, + clip_on=True, + fontsize=8, + ) + + def imshow(self, data, left, right, ws, bottom, top, hs, num_data_type="float"): + wm = (right - left) / data.shape[1] + hm = (top - bottom) / data.shape[0] + hwm = wm / 2.0 + hhm = hm / 2.0 + hws = ws / 2.0 + hhs = hs / 2.0 + width_range = np.linspace(bottom - hhs + hhm, top + hhs - hhm, data.shape[0]) + height_range = np.linspace(left - hws + hwm, right + hws - hwm, data.shape[1]) + plt.imshow( + data, + extent=(left - hws, right + hws, bottom - hhs, top + hhs), + aspect="auto", + origin="lower", + ) + ys, xs = np.meshgrid(width_range, height_range, indexing="ij") + for x, y, val in zip(xs.flatten(), ys.flatten(), data.flatten()): + self.plot_text(x, y, val, num_data_type) + plt.colorbar() + + def set_lim(self, fig, xlim, ylim): + fig.set_xlim(xlim) + fig.set_ylim(ylim) + + def add_label(self, title, xlabel, ylabel): + plt.title(title) + plt.xlabel(xlabel) + plt.ylabel(ylabel) + plt.legend(loc="upper left", fontsize=6, labelspacing=0) + + def show(self): + if self.plot_flag: + plt.show() + + def save(self, file_name): + self.fig.savefig(file_name) diff --git a/vehicle/accel_brake_map_calibrator/scripts/view_plot.py b/vehicle/accel_brake_map_calibrator/scripts/view_plot.py new file mode 100755 index 00000000..4d713cf4 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/view_plot.py @@ -0,0 +1,316 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 + +from ament_index_python.packages import get_package_share_directory +from calc_utils import CalcUtils +import config as CF +from csv_reader import CSVReader +import numpy as np +from plotter import Plotter +import rclpy +from rclpy.node import Node + + +class ViewPlot(Node): + def __init__(self, args): + super().__init__("plot_viewer") + default_map_dir = args.default_map_dir + calibrated_map_dir = args.calibrated_map_dir + calibration_method = args.method + scatter_only = args.scatter_only + log_file = args.log_file + min_vel_thr = args.min_vel_thr + vel_diff_thr = args.vel_diff_thr + pedal_diff_thr = args.pedal_diff_thr + max_steer_thr = args.max_steer_thr + max_pitch_thr = args.max_pitch_thr + max_jerk_thr = args.max_jerk_thr + max_pedal_vel_thr = args.max_pedal_vel_thr + + if default_map_dir is None: + package_path = get_package_share_directory("raw_vehicle_cmd_converter") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_default_map_dir", package_path + "/data/default/" + ) + default_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") + .get_parameter_value() + .string_value + ) + + if calibrated_map_dir is None: + package_path = get_package_share_directory("accel_brake_map_calibrator") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" + ) + calibrated_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") + .get_parameter_value() + .string_value + ) + + if calibration_method is None: + calibration_method = "each_cell" + elif not ((calibration_method == "each_cell") | (calibration_method == "four_cell")): + print("invalid method.") + calibration_method = "each_cell" + + print("default map dir: {}".format(default_map_dir)) + print("calibrated map dir: {}".format(calibrated_map_dir)) + print("calibration method: {}".format(calibration_method)) + + # read csv + self.cr = CSVReader(log_file, csv_type="file") + + # remove unused_data + self.csv_data = self.cr.removeUnusedData( + min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr + ) + + # get statistics array + vel_data = self.cr.getVelData() + pedal_data = self.cr.getPedalData() + acc_data = self.cr.getAccData() + + # get color factor (pitch) array for plotting + color_data = self.cr.getPitchData() + + data, full_data = CalcUtils.create_2d_map( + vel_data, + pedal_data, + acc_data, + color_data, + CF.VEL_LIST / 3.6, + vel_diff_thr, + CF.PEDAL_LIST, + pedal_diff_thr, + calibration_method, + ) + + count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) + velocity_map_list = [] + for i in range(len(CF.VEL_LIST)): + velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, i)) + + default_pedal_list, default_acc_list = self.load_map(default_map_dir) + if len(default_pedal_list) == 0 or len(default_acc_list) == 0: + self.get_logger().warning("No default map file was found in {}".format(default_map_dir)) + + calibrated_pedal_list, calibrated_acc_list = self.load_map(calibrated_map_dir) + if len(calibrated_pedal_list) == 0 or len(calibrated_acc_list) == 0: + self.get_logger().warning( + "No calibrated map file was found in {}".format(calibrated_map_dir) + ) + + # visualize point from data + plot_width = 3 + plot_height = int(math.ceil(len(CF.VEL_LIST) / float(plot_width))) + plotter = Plotter(plot_height, plot_width) + for i in range(len(CF.VEL_LIST)): + self.view_pedal_accel_graph( + plotter, + i, + velocity_map_list, + i, + count_map, + average_map, + stddev_map, + default_pedal_list, + default_acc_list, + calibrated_pedal_list, + calibrated_acc_list, + scatter_only, + ) + plotter.show() + + def plotter_function(self): + return self.plotter + + def view_pedal_accel_graph( + self, + plotter, + subplot_num, + velocity_map_list, + vel_list_idx, + count_map, + average_map, + stddev_map, + default_pedal_list, + default_acc_list, + calibrated_pedal_list, + calibrated_acc_list, + scatter_only, + ): + fig = plotter.subplot_more(subplot_num) + + if not scatter_only: + # calibrated map + if len(calibrated_pedal_list) != 0 and len(calibrated_acc_list) != 0: + plotter.plot( + calibrated_pedal_list[vel_list_idx], + calibrated_acc_list[vel_list_idx], + color="blue", + label="calibrated", + ) + + # default map + if len(default_pedal_list) != 0 and len(default_acc_list) != 0: + plotter.plot( + default_pedal_list[vel_list_idx], + default_acc_list[vel_list_idx], + color="orange", + label="default", + linestyle="dashed", + ) + + # plot all data + pedal_list = [0 for i in range(len(CF.PEDAL_LIST))] + if velocity_map_list[vel_list_idx] is not None: + plotter.scatter_color( + velocity_map_list[vel_list_idx][:, 1], + velocity_map_list[vel_list_idx][:, 2], + color=velocity_map_list[vel_list_idx][:, 3], + label="all", + ) + + for pedal in velocity_map_list[vel_list_idx][:, 1]: + min_pedal = 10 + for pedal_idx, ref_pedal in enumerate(CF.PEDAL_LIST): + if min_pedal > abs(pedal - ref_pedal): + min_pedal = abs(pedal - ref_pedal) + min_pedal_idx = pedal_idx + pedal_list[min_pedal_idx] += 1 + + # plot average data + plotter.scatter(CF.PEDAL_LIST, average_map[:, vel_list_idx], "red", label="average") + + # add label of standard deviation + plotter.scatter([], [], "black", label="std dev") + + # plot average text + for i in range(len(CF.PEDAL_LIST)): + if count_map[i, vel_list_idx] == 0: + continue + x = CF.PEDAL_LIST[i] + y = average_map[i, vel_list_idx] + y2 = stddev_map[i, vel_list_idx] + # plot average + plotter.plot_text(x, y + 1, y, color="red") + + # plot standard deviation + plotter.plot_text(x, y - 1, y2, color="black") + + # plot the number of all data + plotter.plot_text( + x, y - 2, "{}\npts".format(pedal_list[i]), num_data_type="str", color="green" + ) + + pedal_lim = [CF.PEDAL_LIST[0] - 0.05, CF.PEDAL_LIST[-1] + 0.05] + accel_lim = [-5.0, 5.0] + + plotter.set_lim(fig, pedal_lim, accel_lim) + plotter.add_label( + str(CF.VEL_LIST[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" + ) + + def load_map(self, csv_dir): + try: + accel_pedal_list = [] + accel_acc_list = [] + with open(csv_dir + "accel_map.csv") as f: + for l_idx, l in enumerate(f.readlines()): + w = l.split(",") + w[-1] = w[-1][:-1] + if l_idx != 0: + accel_pedal_list.append([float(w[0]) for e in w[1:]]) + accel_acc_list.append([float(e) for e in w[1:]]) + + brake_pedal_list = [] + brake_acc_list = [] + with open(csv_dir + "brake_map.csv") as f: + for l_idx, l in enumerate(f.readlines()): + w = l.split(",") + w[-1] = w[-1][:-1] + if l_idx != 0: + brake_pedal_list.append([-float(w[0]) for e in w[1:]]) + brake_acc_list.append([float(e) for e in w[1:]]) + + return np.hstack( + [np.fliplr(np.array(accel_pedal_list).T), np.array(brake_pedal_list).T.tolist()] + ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) + except OSError as e: + print(e) + return [], [] + + +def main(args=None): + rclpy.init(args=None) + node = ViewPlot(args) + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + package_path = get_package_share_directory("accel_brake_map_calibrator") + parser = argparse.ArgumentParser() + parser.add_argument( + "-d", "--default-map-dir", default=None, type=str, help="directory of default map" + ) + parser.add_argument( + "-c", "--calibrated-map-dir", default=None, type=str, help="directory of calibrated map" + ) + parser.add_argument( + "-m", + "--method", + default=None, + type=str, + help="calibration method : each_cell or four_cell", + ) + parser.add_argument("-s", "--scatter-only", action="store_true", help="show only scatters") + parser.add_argument( + "-l", + "--log-file", + default=package_path + "/config/log.csv", + type=str, + help="path of log.csv", + ) + parser.add_argument( + "--min-vel-thr", default=0.1, type=float, help="valid min velocity threshold" + ) + parser.add_argument( + "--vel-diff-thr", default=0.556, type=float, help="valid velocity diff threshold" + ) + parser.add_argument( + "--pedal-diff-thr", default=0.03, type=float, help="valid pedal diff threshold" + ) + parser.add_argument( + "--max-steer-thr", default=0.2, type=float, help="valid max steer threshold" + ) + parser.add_argument( + "--max-pitch-thr", default=0.02, type=float, help="valid max pitch threshold" + ) + parser.add_argument("--max-jerk-thr", default=0.7, type=float, help="valid max jerk threshold") + parser.add_argument( + "--max-pedal-vel-thr", default=0.7, type=float, help="valid max pedal velocity threshold" + ) + + args = parser.parse_args() + main(args) diff --git a/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py b/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py new file mode 100755 index 00000000..83cfb869 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py @@ -0,0 +1,135 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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 ament_index_python.packages import get_package_share_directory +from calc_utils import CalcUtils +import config as CF +from csv_reader import CSVReader +from plotter import Plotter +import rclpy +from rclpy.node import Node + + +class ViewPlot(Node): + def __init__(self): + super().__init__("statistics_viewer") + # get parameter + package_path = get_package_share_directory("accel_brake_map_calibrator") + self.declare_parameter("log_file", package_path + "/config/log.csv") + log_file = self.get_parameter("log_file").get_parameter_value().string_value + self.declare_parameter("velocity_min_threshold", 0.1) + min_vel_thr = ( + self.get_parameter("velocity_min_threshold").get_parameter_value().double_value + ) + self.declare_parameter("velocity_diff_threshold", 0.556) + vel_diff_thr = ( + self.get_parameter("velocity_diff_threshold").get_parameter_value().double_value + ) + self.declare_parameter("pedal_diff_threshold", 0.03) + pedal_diff_thr = ( + self.get_parameter("pedal_diff_threshold").get_parameter_value().double_value + ) + self.declare_parameter("max_steer_threshold", 0.2) + max_steer_thr = self.get_parameter("max_steer_threshold").get_parameter_value().double_value + self.declare_parameter("max_pitch_threshold", 0.02) + max_pitch_thr = self.get_parameter("max_pitch_threshold").get_parameter_value().double_value + self.declare_parameter("max_jerk_threshold", 0.7) + max_jerk_thr = self.get_parameter("max_jerk_threshold").get_parameter_value().double_value + self.declare_parameter("pedal_velocity_thresh", 0.15) + max_pedal_vel_thr = ( + self.get_parameter("pedal_velocity_thresh").get_parameter_value().double_value + ) + + # read csv + self.cr = CSVReader(log_file, csv_type="file") + # remove unused_data + self.csv_data = self.cr.removeUnusedData( + min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr + ) + + # get statistics array + vel_data = self.cr.getVelData() + pedal_data = self.cr.getPedalData() + acc_data = self.cr.getAccData() + + # get color factor (pitch) array for plotting + color_data = self.cr.getPitchData() + + data, full_data = CalcUtils.create_2d_map( + vel_data, + pedal_data, + acc_data, + color_data, + CF.VEL_LIST / 3.6, + vel_diff_thr, + CF.PEDAL_LIST, + pedal_diff_thr, + ) + + count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) + + # visualization + plotter = Plotter(1, 3) + plotter.subplot(1) + plotter.imshow( + average_map, + CF.VEL_MIN, + CF.VEL_MAX, + CF.VEL_SPAN, + CF.PEDAL_MIN, + CF.PEDAL_MAX, + CF.PEDAL_SPAN, + ) + plotter.add_label("average of accel", "velocity(kmh)", "throttle") + + plotter.subplot(2) + plotter.imshow( + stddev_map, + CF.VEL_MIN, + CF.VEL_MAX, + CF.VEL_SPAN, + CF.PEDAL_MIN, + CF.PEDAL_MAX, + CF.PEDAL_SPAN, + ) + plotter.add_label("std. dev. of accel", "velocity(kmh)", "throttle") + + plotter.subplot(3) + plotter.imshow( + count_map, + CF.VEL_MIN, + CF.VEL_MAX, + CF.VEL_SPAN, + CF.PEDAL_MIN, + CF.PEDAL_MAX, + CF.PEDAL_SPAN, + num_data_type="int", + ) + plotter.add_label("number of accel data", "velocity(kmh)", "throttle") + plotter.show() + + +def main(args=None): + rclpy.init(args=args) + node = ViewPlot() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp new file mode 100644 index 00000000..ecfc312d --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -0,0 +1,1692 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp" + +#include "rclcpp/logging.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace accel_brake_map_calibrator +{ + +AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options) +: Node("accel_brake_map_calibrator", node_options) +{ + transform_listener_ = std::make_shared(this); + // get parameter + update_hz_ = declare_parameter("update_hz", 10.0); + covariance_ = declare_parameter("initial_covariance", 0.05); + velocity_min_threshold_ = declare_parameter("velocity_min_threshold", 0.1); + velocity_diff_threshold_ = declare_parameter("velocity_diff_threshold", 0.556); + pedal_diff_threshold_ = declare_parameter("pedal_diff_threshold", 0.03); + max_steer_threshold_ = declare_parameter("max_steer_threshold", 0.2); + max_pitch_threshold_ = declare_parameter("max_pitch_threshold", 0.02); + max_jerk_threshold_ = declare_parameter("max_jerk_threshold", 0.7); + pedal_velocity_thresh_ = declare_parameter("pedal_velocity_thresh", 0.15); + max_accel_ = declare_parameter("max_accel", 5.0); + min_accel_ = declare_parameter("min_accel", -5.0); + pedal_to_accel_delay_ = declare_parameter("pedal_to_accel_delay", 0.3); + max_data_count_ = declare_parameter("max_data_count", 200); + pedal_accel_graph_output_ = declare_parameter("pedal_accel_graph_output", false); + progress_file_output_ = declare_parameter("progress_file_output", false); + precision_ = declare_parameter("precision", 3); + const auto get_pitch_method_str = declare_parameter("get_pitch_method", std::string("tf")); + if (get_pitch_method_str == std::string("tf")) { + get_pitch_method_ = GET_PITCH_METHOD::TF; + } else if (get_pitch_method_str == std::string("none")) { + get_pitch_method_ = GET_PITCH_METHOD::NONE; + } else { + RCLCPP_ERROR_STREAM(get_logger(), "update_method_ is wrong. (available method: tf, file, none"); + return; + } + const auto accel_brake_value_source_str = + declare_parameter("accel_brake_value_source", std::string("status")); + if (accel_brake_value_source_str == std::string("status")) { + accel_brake_value_source_ = ACCEL_BRAKE_SOURCE::STATUS; + } else if (accel_brake_value_source_str == std::string("command")) { + accel_brake_value_source_ = ACCEL_BRAKE_SOURCE::COMMAND; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "accel_brake_value_source is wrong. (available source: status, command"); + return; + } + + update_suggest_thresh_ = declare_parameter("update_suggest_thresh", 0.7); + csv_calibrated_map_dir_ = declare_parameter("csv_calibrated_map_dir", std::string("")); + output_accel_file_ = csv_calibrated_map_dir_ + "/accel_map.csv"; + output_brake_file_ = csv_calibrated_map_dir_ + "/brake_map.csv"; + const std::string update_method_str = + declare_parameter("update_method", std::string("update_offset_each_cell")); + if (update_method_str == std::string("update_offset_each_cell")) { + update_method_ = UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL; + } else if (update_method_str == std::string("update_offset_total")) { + update_method_ = UPDATE_METHOD::UPDATE_OFFSET_TOTAL; + } else if (update_method_str == std::string("update_offset_four_cell_around")) { + update_method_ = UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "update_method is wrong. (available method: update_offset_each_cell, update_offset_total"); + return; + } + // initializer + + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + + // Publisher for checkUpdateSuggest + calibration_status_pub_ = create_publisher( + "/accel_brake_map_calibrator/output/calibration_status", durable_qos); + + /* Diagnostic Updater */ + updater_ptr_ = std::make_shared(this, 1.0 / update_hz_); + updater_ptr_->setHardwareID("accel_brake_map_calibrator"); + updater_ptr_->add( + "accel_brake_map_calibrator", this, &AccelBrakeMapCalibrator::checkUpdateSuggest); + + { + csv_default_map_dir_ = declare_parameter("csv_default_map_dir", std::string("")); + + std::string csv_path_accel_map = csv_default_map_dir_ + "/accel_map.csv"; + std::string csv_path_brake_map = csv_default_map_dir_ + "/brake_map.csv"; + if ( + !accel_map_.readAccelMapFromCSV(csv_path_accel_map) || + !new_accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { + is_default_map_ = false; + RCLCPP_ERROR_STREAM( + get_logger(), + "Cannot read accelmap. csv path = " << csv_path_accel_map.c_str() << ". stop calculation."); + return; + } + if ( + !brake_map_.readBrakeMapFromCSV(csv_path_brake_map) || + !new_brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { + is_default_map_ = false; + RCLCPP_ERROR_STREAM( + get_logger(), + "Cannot read brakemap. csv path = " << csv_path_brake_map.c_str() << ". stop calculation."); + return; + } + } + + std::string output_log_file = declare_parameter("output_log_file", std::string("")); + output_log_.open(output_log_file); + addIndexToCSV(&output_log_); + + debug_values_.data.resize(num_debug_values_); + + // input map info + accel_map_value_ = accel_map_.getAccelMap(); + brake_map_value_ = brake_map_.getBrakeMap(); + accel_vel_index_ = accel_map_.getVelIdx(); + brake_vel_index_ = brake_map_.getVelIdx(); + accel_pedal_index_ = accel_map_.getThrottleIdx(); + brake_pedal_index_ = brake_map_.getBrakeIdx(); + update_accel_map_value_.resize((accel_map_value_.size())); + update_brake_map_value_.resize((brake_map_value_.size())); + for (auto & m : update_accel_map_value_) { + m.resize(accel_map_value_.at(0).size()); + } + for (auto & m : update_brake_map_value_) { + m.resize(brake_map_value_.at(0).size()); + } + accel_offset_covariance_value_.resize((accel_map_value_.size())); + brake_offset_covariance_value_.resize((brake_map_value_.size())); + for (auto & m : accel_offset_covariance_value_) { + m.resize(accel_map_value_.at(0).size(), covariance_); + } + for (auto & m : brake_offset_covariance_value_) { + m.resize(brake_map_value_.at(0).size(), covariance_); + } + map_value_data_.resize(accel_map_value_.size() + brake_map_value_.size() - 1); + for (auto & m : map_value_data_) { + m.resize(accel_map_value_.at(0).size()); + } + + std::copy(accel_map_value_.begin(), accel_map_value_.end(), update_accel_map_value_.begin()); + std::copy(brake_map_value_.begin(), brake_map_value_.end(), update_brake_map_value_.begin()); + + // initialize matrix for covariance calculation + { + const auto genConstMat = [](const Map & map, const auto val) { + return Eigen::MatrixXd::Constant(map.size(), map.at(0).size(), val); + }; + accel_data_mean_mat_ = genConstMat(accel_map_value_, map_offset_); + brake_data_mean_mat_ = genConstMat(brake_map_value_, map_offset_); + accel_data_covariance_mat_ = genConstMat(accel_map_value_, covariance_); + brake_data_covariance_mat_ = genConstMat(brake_map_value_, covariance_); + accel_data_num_ = genConstMat(accel_map_value_, 1); + brake_data_num_ = genConstMat(brake_map_value_, 1); + } + + // publisher + update_suggest_pub_ = + create_publisher("~/output/update_suggest", durable_qos); + original_map_occ_pub_ = create_publisher("~/debug/original_occ_map", durable_qos); + update_map_occ_pub_ = create_publisher("~/debug/update_occ_map", durable_qos); + data_ave_pub_ = create_publisher("~/debug/data_average_occ_map", durable_qos); + data_std_pub_ = create_publisher("~/debug/data_std_dev_occ_map", durable_qos); + data_count_pub_ = create_publisher("~/debug/data_count_occ_map", durable_qos); + data_count_with_self_pose_pub_ = + create_publisher("~/debug/data_count_self_pose_occ_map", durable_qos); + index_pub_ = create_publisher("~/debug/occ_index", durable_qos); + original_map_raw_pub_ = + create_publisher("~/debug/original_raw_map", durable_qos); + update_map_raw_pub_ = create_publisher("~/output/update_raw_map", durable_qos); + debug_pub_ = create_publisher("~/output/debug_values", durable_qos); + current_map_error_pub_ = + create_publisher("~/output/current_map_error", durable_qos); + updated_map_error_pub_ = + create_publisher("~/output/updated_map_error", durable_qos); + map_error_ratio_pub_ = create_publisher("~/output/map_error_ratio", durable_qos); + offset_covariance_pub_ = + create_publisher("~/debug/offset_covariance", durable_qos); + + // subscriber + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + + velocity_sub_ = create_subscription( + "~/input/velocity", queue_size, + std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1)); + steer_sub_ = create_subscription( + "~/input/steer", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackSteer, this, _1)); + if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { + actuation_status_sub_ = create_subscription( + "~/input/actuation_status", queue_size, + std::bind(&AccelBrakeMapCalibrator::callbackActuationStatus, this, _1)); + } + if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { + actuation_cmd_sub_ = create_subscription( + "~/input/actuation_cmd", queue_size, + std::bind(&AccelBrakeMapCalibrator::callbackActuationCommand, this, _1)); + } + + // Service + update_map_dir_server_ = create_service( + "~/input/update_map_dir", + std::bind(&AccelBrakeMapCalibrator::callbackUpdateMapService, this, _1, _2, _3)); + + // timer + initTimer(1.0 / update_hz_); + initOutputCSVTimer(30.0); + + logger_configure_ = std::make_unique(this); +} + +void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_output_csv_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&AccelBrakeMapCalibrator::timerCallbackOutputCSV, this)); +} + +void AccelBrakeMapCalibrator::initTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&AccelBrakeMapCalibrator::timerCallback, this)); +} + +bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) +{ + if (get_pitch_method_ == GET_PITCH_METHOD::NONE) { + // do not get pitch from tf + *pitch = 0.0; + return true; + } + + // get tf + const auto transform = transform_listener_->getTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); + if (!transform) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 5000, "cannot get map to base_link transform. "); + return false; + } + double roll, raw_pitch, yaw; + tf2::getEulerYPR(transform->transform.rotation, roll, raw_pitch, yaw); + debug_values_.data.at(CURRENT_RAW_PITCH) = raw_pitch; + *pitch = lowpass(*pitch, raw_pitch, 0.2); + debug_values_.data.at(CURRENT_PITCH) = *pitch; + return true; +} + +void AccelBrakeMapCalibrator::timerCallback() +{ + update_count_++; + + RCLCPP_DEBUG_STREAM_THROTTLE( + get_logger(), *get_clock(), 5000, + "map updating... count: " << update_success_count_ << " / " << update_count_ << "\n\t" + << "lack_of_data_count: " << lack_of_data_count_ << "\n\t" + << " failed_to_get_pitch_count: " << failed_to_get_pitch_count_ + << "\n\t" + << "too_large_pitch_count: " << too_large_pitch_count_ << "\n\t" + << " too_low_speed_count: " << too_low_speed_count_ << "\n\t" + << "too_large_steer_count: " << too_large_steer_count_ << "\n\t" + << "too_large_jerk_count: " << too_large_jerk_count_ << "\n\t" + << "invalid_acc_brake_count: " << invalid_acc_brake_count_ << "\n\t" + << "too_large_pedal_spd_count: " << too_large_pedal_spd_count_ + << "\n\t" + << "update_fail_count_: " << update_fail_count_ << "\n"); + + /* valid check */ + + // data check + if ( + !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ || + !delayed_accel_pedal_ptr_ || !delayed_brake_pedal_ptr_) { + // lack of data + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 5000, "lack of topics (twist, steer, accel, brake)"); + lack_of_data_count_++; + return; + } + + // data check 2 + if ( + isTimeout(twist_ptr_->header.stamp, timeout_sec_) || + isTimeout(steer_ptr_->stamp, timeout_sec_) || isTimeout(accel_pedal_ptr_, timeout_sec_) || + isTimeout(brake_pedal_ptr_, timeout_sec_)) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 5000, "timeout of topics (twist, steer, accel, brake)"); + lack_of_data_count_++; + return; + } + + // data check 3 + if (!getCurrentPitchFromTF(&pitch_)) { + // cannot get pitch + RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot get pitch"); + failed_to_get_pitch_count_++; + return; + } + + /* write data to log */ + if (pedal_accel_graph_output_) { + addLogToCSV( + &output_log_, rclcpp::Time(twist_ptr_->header.stamp).seconds(), twist_ptr_->twist.linear.x, + acceleration_, getPitchCompensatedAcceleration(), delayed_accel_pedal_ptr_->data, + delayed_brake_pedal_ptr_->data, accel_pedal_speed_, brake_pedal_speed_, pitch_, + steer_ptr_->steering_tire_angle, jerk_, full_original_accel_rmse_, part_original_accel_rmse_, + new_accel_rmse_); + } + + /* publish map & debug_values*/ + publishMap(accel_map_value_, brake_map_value_, "original"); + publishMap(update_accel_map_value_, update_brake_map_value_, "update"); + publishOffsetCovMap(accel_offset_covariance_value_, brake_offset_covariance_value_); + publishCountMap(); + publishIndex(); + publishUpdateSuggestFlag(); + debug_pub_->publish(debug_values_); + publishFloat32("current_map_error", part_original_accel_rmse_); + publishFloat32("updated_map_error", new_accel_rmse_); + publishFloat32( + "map_error_ratio", + part_original_accel_rmse_ != 0.0 ? new_accel_rmse_ / part_original_accel_rmse_ : 1.0); + + // -- processing start -- + + /* initialize */ + debug_values_.data.at(SUCCESS_TO_UPDATE) = false; + update_success_ = false; + + // twist check + if (twist_ptr_->twist.linear.x < velocity_min_threshold_) { + // too low speed ( or backward velocity) + too_low_speed_count_++; + return; + } + + // accel / brake map evaluation (do not evaluate when the car stops) + executeEvaluation(); + + // pitch check + if (std::fabs(pitch_) > max_pitch_threshold_) { + // too large pitch + too_large_pitch_count_++; + return; + } + + // steer check + if (std::fabs(steer_ptr_->steering_tire_angle) > max_steer_threshold_) { + // too large steer + too_large_steer_count_++; + return; + } + + // jerk check + if (std::fabs(jerk_) > max_jerk_threshold_) { + // too large jerk + too_large_jerk_count_++; + return; + } + + // pedal check + if ( + delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon() && + delayed_brake_pedal_ptr_->data > std::numeric_limits::epsilon()) { + // both (accel/brake) output + RCLCPP_DEBUG_STREAM_THROTTLE( + get_logger(), *get_clock(), 5000, + "invalid pedal value (Both of accel output and brake output area not zero. )"); + invalid_acc_brake_count_++; + return; + } + + // pedal speed check + if ( + std::fabs(accel_pedal_speed_) > pedal_velocity_thresh_ || + std::fabs(brake_pedal_speed_) > pedal_velocity_thresh_) { + // too large pedal speed + too_large_pedal_spd_count_++; + return; + } + + /* update map */ + if (updateAccelBrakeMap()) { + update_success_count_++; + debug_values_.data.at(SUCCESS_TO_UPDATE) = true; + update_success_ = true; + } else { + update_fail_count_++; + } +} + +void AccelBrakeMapCalibrator::timerCallbackOutputCSV() +{ + // write accel/ brake map to file + const auto ros_time = std::to_string(this->now().seconds()); + writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, output_accel_file_); + writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, output_brake_file_); + if (progress_file_output_) { + writeMapToCSV( + accel_vel_index_, accel_pedal_index_, update_accel_map_value_, + output_accel_file_ + "_" + ros_time); + writeMapToCSV( + brake_vel_index_, brake_pedal_index_, update_brake_map_value_, + output_brake_file_ + "_" + ros_time); + writeMapToCSV( + accel_vel_index_, accel_pedal_index_, accel_map_value_, output_accel_file_ + "_original"); + writeMapToCSV( + brake_vel_index_, brake_pedal_index_, brake_map_value_, output_brake_file_ + "_original"); + } + + // update newest accel / brake map + // check file existence + std::ifstream af(output_accel_file_); + std::ifstream bf(output_brake_file_); + if (!af.is_open() || !bf.is_open()) { + RCLCPP_WARN(get_logger(), "Accel map or brake map does not exist"); + return; + } + + new_accel_map_ = AccelMap(); + if (!new_accel_map_.readAccelMapFromCSV(output_accel_file_)) { + RCLCPP_WARN(get_logger(), "Cannot read accelmap. csv path = %s. ", output_accel_file_.c_str()); + } + new_brake_map_ = BrakeMap(); + if (!new_brake_map_.readBrakeMapFromCSV(output_brake_file_)) { + RCLCPP_WARN(get_logger(), "Cannot read brakemap. csv path = %s. ", output_brake_file_.c_str()); + } +} + +void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstSharedPtr msg) +{ + // convert velocity-report to twist-stamped + auto twist_msg = std::make_shared(); + twist_msg->header = msg->header; + twist_msg->twist.linear.x = msg->longitudinal_velocity; + twist_msg->twist.linear.y = msg->lateral_velocity; + twist_msg->twist.angular.z = msg->heading_rate; + + if (!twist_vec_.empty()) { + const auto past_msg = getNearestTimeDataFromVec(twist_msg, dif_twist_time_, twist_vec_); + const double raw_acceleration = getAccel(past_msg, twist_msg); + acceleration_ = lowpass(acceleration_, raw_acceleration, 0.25); + acceleration_time_ = rclcpp::Time(msg->header.stamp).seconds(); + debug_values_.data.at(CURRENT_RAW_ACCEL) = raw_acceleration; + debug_values_.data.at(CURRENT_ACCEL) = acceleration_; + + // calculate jerk + if ( + this->now().seconds() - pre_acceleration_time_ > timeout_sec_ || + (acceleration_time_ - pre_acceleration_time_) <= std::numeric_limits::epsilon()) { + RCLCPP_DEBUG_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot calculate jerk"); + // does not update jerk + } else { + const double raw_jerk = getJerk(); + // to avoid to get delayed jerk, set high gain + jerk_ = lowpass(jerk_, raw_jerk, 0.5); + } + debug_values_.data.at(CURRENT_JERK) = jerk_; + pre_acceleration_ = acceleration_; + pre_acceleration_time_ = acceleration_time_; + } + + debug_values_.data.at(CURRENT_SPEED) = twist_msg->twist.linear.x; + twist_ptr_ = twist_msg; + pushDataToVec(twist_msg, twist_vec_max_size_, &twist_vec_); +} + +void AccelBrakeMapCalibrator::callbackSteer(const SteeringReport::ConstSharedPtr msg) +{ + debug_values_.data.at(CURRENT_STEER) = msg->steering_tire_angle; + steer_ptr_ = msg; +} + +void AccelBrakeMapCalibrator::callbackActuation( + const std_msgs::msg::Header header, const double accel, const double brake) +{ + // get accel data + accel_pedal_ptr_ = std::make_shared(accel, rclcpp::Time(header.stamp)); + if (!accel_pedal_vec_.empty()) { + const auto past_accel_ptr = + getNearestTimeDataFromVec(accel_pedal_ptr_, dif_pedal_time_, accel_pedal_vec_); + const double raw_accel_pedal_speed = + getPedalSpeed(past_accel_ptr, accel_pedal_ptr_, accel_pedal_speed_); + accel_pedal_speed_ = lowpass(accel_pedal_speed_, raw_accel_pedal_speed, 0.5); + debug_values_.data.at(CURRENT_RAW_ACCEL_SPEED) = raw_accel_pedal_speed; + debug_values_.data.at(CURRENT_ACCEL_SPEED) = accel_pedal_speed_; + } + debug_values_.data.at(CURRENT_ACCEL_PEDAL) = accel_pedal_ptr_->data; + pushDataToVec(accel_pedal_ptr_, pedal_vec_max_size_, &accel_pedal_vec_); + delayed_accel_pedal_ptr_ = + getNearestTimeDataFromVec(accel_pedal_ptr_, pedal_to_accel_delay_, accel_pedal_vec_); + + // get brake data + brake_pedal_ptr_ = std::make_shared(brake, rclcpp::Time(header.stamp)); + if (!brake_pedal_vec_.empty()) { + const auto past_brake_ptr = + getNearestTimeDataFromVec(brake_pedal_ptr_, dif_pedal_time_, brake_pedal_vec_); + const double raw_brake_pedal_speed = + getPedalSpeed(past_brake_ptr, brake_pedal_ptr_, brake_pedal_speed_); + brake_pedal_speed_ = lowpass(brake_pedal_speed_, raw_brake_pedal_speed, 0.5); + debug_values_.data.at(CURRENT_RAW_BRAKE_SPEED) = raw_brake_pedal_speed; + debug_values_.data.at(CURRENT_BRAKE_SPEED) = brake_pedal_speed_; + } + debug_values_.data.at(CURRENT_BRAKE_PEDAL) = brake_pedal_ptr_->data; + pushDataToVec(brake_pedal_ptr_, pedal_vec_max_size_, &brake_pedal_vec_); + delayed_brake_pedal_ptr_ = + getNearestTimeDataFromVec(brake_pedal_ptr_, pedal_to_accel_delay_, brake_pedal_vec_); +} + +void AccelBrakeMapCalibrator::callbackActuationCommand( + const ActuationCommandStamped::ConstSharedPtr msg) +{ + const auto header = msg->header; + const auto accel = msg->actuation.accel_cmd; + const auto brake = msg->actuation.brake_cmd; + callbackActuation(header, accel, brake); +} + +void AccelBrakeMapCalibrator::callbackActuationStatus( + const ActuationStatusStamped::ConstSharedPtr msg) +{ + const auto header = msg->header; + const auto accel = msg->status.accel_status; + const auto brake = msg->status.brake_status; + callbackActuation(header, accel, brake); +} + +bool AccelBrakeMapCalibrator::callbackUpdateMapService( + [[maybe_unused]] const std::shared_ptr request_header, + UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res) +{ + // if req.path is input, use this as the directory to set updated maps + std::string update_map_dir = req->path.empty() ? csv_default_map_dir_ : req->path; + + RCLCPP_INFO_STREAM(get_logger(), "update accel/brake map. directory: " << update_map_dir); + const auto accel_map_file = update_map_dir + "/accel_map.csv"; + const auto brake_map_file = update_map_dir + "/brake_map.csv"; + if ( + writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, accel_map_file) && + writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, brake_map_file)) { + res->success = true; + res->message = + "Data has been successfully saved on " + update_map_dir + "/(accel/brake)_map.csv"; + } else { + res->success = false; + res->message = "Failed to save data. Maybe invalid path?"; + } + return true; +} + +double AccelBrakeMapCalibrator::lowpass( + const double original, const double current, const double gain) +{ + return current * gain + original * (1.0 - gain); +} + +double AccelBrakeMapCalibrator::getPedalSpeed( + const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, + const double prev_pedal_speed) +{ + const double dt = (current_pedal->data_time - prev_pedal->data_time).seconds(); + if (dt < 1e-03) { + // invalid pedal info. return prev pedal speed + return prev_pedal_speed; + } + + const double d_pedal = current_pedal->data - prev_pedal->data; + return d_pedal / dt; +} + +double AccelBrakeMapCalibrator::getAccel( + const TwistStamped::ConstSharedPtr & prev_twist, + const TwistStamped::ConstSharedPtr & current_twist) +{ + const double dt = + (rclcpp::Time(current_twist->header.stamp) - rclcpp::Time(prev_twist->header.stamp)).seconds(); + if (dt < 1e-03) { + // invalid twist. return prev acceleration + return acceleration_; + } + const double dv = current_twist->twist.linear.x - prev_twist->twist.linear.x; + return std::min(std::max(min_accel_, dv / dt), max_accel_); +} + +double AccelBrakeMapCalibrator::getJerk() +{ + const double jerk = + (acceleration_ - pre_acceleration_) / (acceleration_time_ - pre_acceleration_time_); + return std::min(std::max(-max_jerk_, jerk), max_jerk_); +} + +bool AccelBrakeMapCalibrator::indexValueSearch( + const std::vector value_index, const double value, const double value_thresh, + int * searched_index) +{ + if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { + /* return lower left index. + +-------+ +:grid + | * | *:given data + | | o:index to return + o-------+ + */ + if (value < 0) { + std::cerr << "error : invalid value." << std::endl; + return false; + } + double old_diff_value = -999; // TODO(Hirano) + for (std::size_t i = 0; i < value_index.size(); i++) { + const double diff_value = value_index.at(i) - value; + if (diff_value <= 0 && old_diff_value < diff_value) { + *searched_index = i; + old_diff_value = diff_value; + } else { + return true; + } + } + } else { + for (std::size_t i = 0; i < value_index.size(); i++) { + const double diff_value = std::fabs(value_index.at(i) - value); + if (diff_value <= value_thresh) { + *searched_index = i; + return true; + } + } + } + return false; +} + +int AccelBrakeMapCalibrator::nearestValueSearch( + const std::vector value_index, const double value) +{ + double max_dist = std::numeric_limits::max(); + int nearest_idx = 0; + + for (std::size_t i = 0; i < value_index.size(); i++) { + const double dist = std::fabs(value - value_index.at(i)); + if (max_dist > dist) { + nearest_idx = i; + max_dist = dist; + } + } + return nearest_idx; +} + +int AccelBrakeMapCalibrator::nearestPedalSearch() +{ + bool accel_mode; + + if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { + accel_mode = true; + } else { + accel_mode = false; + } + + int nearest_idx; + if (accel_mode) { + nearest_idx = nearestValueSearch(accel_pedal_index_, delayed_accel_pedal_ptr_->data); + } else { + nearest_idx = nearestValueSearch(brake_pedal_index_, delayed_brake_pedal_ptr_->data); + } + + return getUnifiedIndexFromAccelBrakeIndex(accel_mode, nearest_idx); +} + +int AccelBrakeMapCalibrator::nearestVelSearch() +{ + const double current_vel = twist_ptr_->twist.linear.x; + return nearestValueSearch(accel_vel_index_, current_vel); +} + +void AccelBrakeMapCalibrator::takeConsistencyOfAccelMap() +{ + const double bit = std::pow(1e-01, precision_); + for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { + for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { + const double current_acc = update_accel_map_value_.at(ped_idx).at(vel_idx); + const double next_ped_acc = update_accel_map_value_.at(ped_idx + 1).at(vel_idx); + const double next_vel_acc = update_accel_map_value_.at(ped_idx).at(vel_idx + 1); + + if (current_acc <= next_vel_acc) { + // the higher the velocity, the lower the acceleration + update_accel_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; + } + + if (current_acc >= next_ped_acc) { + // the higher the accel pedal, the higher the acceleration + update_accel_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc + bit; + } + } + } +} + +void AccelBrakeMapCalibrator::takeConsistencyOfBrakeMap() +{ + const double bit = std::pow(1e-01, precision_); + for (std::size_t ped_idx = 0; ped_idx < update_brake_map_value_.size() - 1; ped_idx++) { + for (std::size_t vel_idx = 0; vel_idx < update_brake_map_value_.at(0).size() - 1; vel_idx++) { + const double current_acc = update_brake_map_value_.at(ped_idx).at(vel_idx); + const double next_ped_acc = update_brake_map_value_.at(ped_idx + 1).at(vel_idx); + const double next_vel_acc = update_brake_map_value_.at(ped_idx).at(vel_idx + 1); + + if (current_acc <= next_vel_acc) { + // the higher the velocity, the lower the acceleration + update_brake_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; + } + + if (current_acc <= next_ped_acc) { + // the higher the brake pedal, the lower the acceleration + update_brake_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc - bit; + } + } + } +} + +bool AccelBrakeMapCalibrator::updateAccelBrakeMap() +{ + // get pedal index + bool accel_mode = false; + int accel_pedal_index = 0; + int brake_pedal_index = 0; + int accel_vel_index = 0; + int brake_vel_index = 0; + + if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { + accel_mode = true; + } else { + accel_mode = false; + } + + if ( + accel_mode && !indexValueSearch( + accel_pedal_index_, delayed_accel_pedal_ptr_->data, pedal_diff_threshold_, + &accel_pedal_index)) { + // not match accel pedal output to pedal value in index + return false; + } + + if ( + !accel_mode && !indexValueSearch( + brake_pedal_index_, delayed_brake_pedal_ptr_->data, pedal_diff_threshold_, + &brake_pedal_index)) { + // not match accel pedal output to pedal value in index + return false; + } + + if ( + accel_mode && + !indexValueSearch( + accel_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &accel_vel_index)) { + // not match current velocity to velocity value in index + return false; + } + + if ( + !accel_mode && + !indexValueSearch( + brake_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &brake_vel_index)) { + // not match current velocity to velocity value in index + return false; + } + + // update map + executeUpdate(accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index); + + // when update 0 pedal index, update another map + if (accel_mode && accel_pedal_index == 0) { + // copy accel map value to brake map value + update_brake_map_value_.at(accel_pedal_index).at(accel_vel_index) = + update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index); + if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { + update_brake_map_value_.at(accel_pedal_index).at(accel_vel_index + 1) = + update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index + 1); + } + } else if (!accel_mode && brake_pedal_index == 0) { + // copy brake map value to accel map value + update_accel_map_value_.at(brake_pedal_index).at(brake_vel_index) = + update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); + if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { + update_accel_map_value_.at(brake_pedal_index).at(brake_vel_index + 1) = + update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index + 1); + } + } + + // take consistency of map + takeConsistencyOfAccelMap(); + takeConsistencyOfBrakeMap(); + + return true; +} + +void AccelBrakeMapCalibrator::executeUpdate( + const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, + const int brake_pedal_index, const int brake_vel_index) +{ + const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); + const double map_acc = accel_mode + ? update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) + : update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); + RCLCPP_DEBUG_STREAM(get_logger(), "measured_acc: " << measured_acc << ", map_acc: " << map_acc); + + if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL) { + updateEachValOffset( + accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, + measured_acc, map_acc); + } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_TOTAL) { + updateTotalMapOffset(measured_acc, map_acc); + } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { + updateFourCellAroundOffset( + accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, + measured_acc); + } + + // add accel data to map + accel_mode ? map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(true, accel_pedal_index)) + .at(accel_vel_index) + .emplace_back(measured_acc) + : map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(false, brake_pedal_index)) + .at(brake_vel_index) + .emplace_back(measured_acc); +} + +bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( + const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, + const int brake_pedal_index, const int brake_vel_index, const double measured_acc) +{ + // pre-defined + static Map accel_map_offset_vec_( + accel_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); + static Map brake_map_offset_vec_( + brake_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); + static std::vector> accel_covariance_mat_( + accel_map_value_.size() - 1, + std::vector( + accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); + static std::vector> brake_covariance_mat_( + brake_map_value_.size() - 1, + std::vector( + accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); + + auto & update_map_value = accel_mode ? update_accel_map_value_ : update_brake_map_value_; + auto & offset_covariance_value = + accel_mode ? accel_offset_covariance_value_ : brake_offset_covariance_value_; + const auto & map_value = accel_mode ? accel_map_value_ : brake_map_value_; + const auto & pedal_index = accel_mode ? accel_pedal_index : brake_pedal_index; + const auto & pedal_index_ = accel_mode ? accel_pedal_index_ : brake_pedal_index_; + const auto & vel_index = accel_mode ? accel_vel_index : brake_vel_index; + const auto & vel_index_ = accel_mode ? accel_vel_index_ : brake_vel_index_; + const auto & delayed_pedal = + accel_mode ? delayed_accel_pedal_ptr_->data : delayed_brake_pedal_ptr_->data; + auto & map_offset_vec = accel_mode ? accel_map_offset_vec_ : brake_map_offset_vec_; + auto & covariance_mat = accel_mode ? accel_covariance_mat_ : brake_covariance_mat_; + auto & data_mean_mat = accel_mode ? accel_data_mean_mat_ : brake_data_mean_mat_; + auto & data_covariance_mat = accel_mode ? accel_data_covariance_mat_ : brake_data_covariance_mat_; + auto & data_weighted_num = accel_mode ? accel_data_num_ : brake_data_num_; + + const double zll = update_map_value.at(pedal_index + 0).at(vel_index + 0); + const double zhl = update_map_value.at(pedal_index + 1).at(vel_index + 0); + const double zlh = update_map_value.at(pedal_index + 0).at(vel_index + 1); + const double zhh = update_map_value.at(pedal_index + 1).at(vel_index + 1); + + const double xl = pedal_index_.at(pedal_index + 0); + const double xh = pedal_index_.at(pedal_index + 1); + const double rx = (delayed_pedal - xl) / (xh - xl); + const double yl = vel_index_.at(vel_index + 0); + const double yh = vel_index_.at(vel_index + 1); + const double ry = (twist_ptr_->twist.linear.x - yl) / (yh - yl); + + Eigen::Vector4d phi(4); + phi << (1 - rx) * (1 - ry), rx * (1 - ry), (1 - rx) * ry, rx * ry; + + Eigen::Vector4d theta(4); + theta << zll, zhl, zlh, zhh; + + Eigen::Vector4d weighted_sum(4); + weighted_sum << data_weighted_num(pedal_index + 0, vel_index + 0), + data_weighted_num(pedal_index + 1, vel_index + 0), + data_weighted_num(pedal_index + 0, vel_index + 1), + data_weighted_num(pedal_index + 1, vel_index + 1); + + Eigen::Vector4d sigma(4); + sigma << data_covariance_mat(pedal_index + 0, vel_index + 0), + data_covariance_mat(pedal_index + 1, vel_index + 0), + data_covariance_mat(pedal_index + 0, vel_index + 1), + data_covariance_mat(pedal_index + 1, vel_index + 1); + + Eigen::Vector4d mean(4); + mean << data_mean_mat(pedal_index + 0, vel_index + 0), + data_mean_mat(pedal_index + 1, vel_index + 0), data_mean_mat(pedal_index + 0, vel_index + 1), + data_mean_mat(pedal_index + 1, vel_index + 1); + + const int vel_idx_l = vel_index + 0; + const int vel_idx_h = vel_index + 1; + const int ped_idx_l = pedal_index + 0; + const int ped_idx_h = pedal_index + 1; + + Eigen::VectorXd map_offset(4); + map_offset(0) = map_offset_vec.at(ped_idx_l).at(vel_idx_l); + map_offset(1) = map_offset_vec.at(ped_idx_h).at(vel_idx_l); + map_offset(2) = map_offset_vec.at(ped_idx_l).at(vel_idx_h); + map_offset(3) = map_offset_vec.at(ped_idx_h).at(vel_idx_h); + + Eigen::VectorXd updated_map_offset(4); + + Eigen::MatrixXd covariance = covariance_mat.at(ped_idx_l).at(vel_idx_l); + + /* calculate adaptive map offset */ + Eigen::MatrixXd G(4, 4); + Eigen::RowVectorXd phiT(4); + phiT = phi.transpose(); + double rk = phiT * covariance * phi; + + G = covariance * phi / (forgetting_factor_ + rk); + double beta = rk > 0 ? (forgetting_factor_ - (1 - forgetting_factor_) / rk) : 1; + covariance = covariance - covariance * phi * phiT * covariance / (1 / beta + rk); // anti-windup + double eta_hat = phiT * theta; + + const double error_map_offset = measured_acc - eta_hat; + updated_map_offset = map_offset + G * error_map_offset; + + for (int i = 0; i < 4; i++) { + const double pre_mean = mean(i); + mean(i) = (weighted_sum(i) * pre_mean + error_map_offset) / (weighted_sum(i) + 1); + sigma(i) = + (weighted_sum(i) * (sigma(i) + pre_mean * pre_mean) + error_map_offset * error_map_offset) / + (weighted_sum(i) + 1) - + mean(i) * mean(i); + weighted_sum(i) = weighted_sum(i) + 1; + } + + /* input calculated result and update map */ + map_offset_vec.at(ped_idx_l).at(vel_idx_l) = updated_map_offset(0); + map_offset_vec.at(ped_idx_h).at(vel_idx_l) = updated_map_offset(1); + map_offset_vec.at(ped_idx_l).at(vel_idx_h) = updated_map_offset(2); + map_offset_vec.at(ped_idx_h).at(vel_idx_h) = updated_map_offset(3); + + data_covariance_mat(ped_idx_l, vel_idx_l) = sigma(0); + data_covariance_mat(ped_idx_h, vel_idx_l) = sigma(1); + data_covariance_mat(ped_idx_l, vel_idx_h) = sigma(2); + data_covariance_mat(ped_idx_h, vel_idx_h) = sigma(3); + + data_weighted_num(ped_idx_l, vel_idx_l) = weighted_sum(0); + data_weighted_num(ped_idx_h, vel_idx_l) = weighted_sum(1); + data_weighted_num(ped_idx_l, vel_idx_h) = weighted_sum(2); + data_weighted_num(ped_idx_h, vel_idx_h) = weighted_sum(3); + + data_mean_mat(ped_idx_l, vel_idx_l) = mean(0); + data_mean_mat(ped_idx_h, vel_idx_l) = mean(1); + data_mean_mat(ped_idx_l, vel_idx_h) = mean(2); + data_mean_mat(ped_idx_h, vel_idx_h) = mean(3); + + covariance_mat.at(ped_idx_l).at(vel_idx_l) = covariance; + + update_map_value.at(pedal_index + 0).at(vel_index + 0) = + map_value.at(pedal_index + 0).at(vel_index + 0) + map_offset(0); + update_map_value.at(pedal_index + 1).at(vel_index + 0) = + map_value.at(pedal_index + 1).at(vel_index + 0) + map_offset(1); + update_map_value.at(pedal_index + 0).at(vel_index + 1) = + map_value.at(pedal_index + 0).at(vel_index + 1) + map_offset(2); + update_map_value.at(pedal_index + 1).at(vel_index + 1) = + map_value.at(pedal_index + 1).at(vel_index + 1) + map_offset(3); + + offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(0)); + offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(1)); + offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(2)); + offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(3)); + + return true; +} + +bool AccelBrakeMapCalibrator::updateEachValOffset( + const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, + const int brake_pedal_index, const int brake_vel_index, const double measured_acc, + const double map_acc) +{ + // pre-defined + static Map map_offset_vec_( + accel_map_value_.size() + brake_map_value_.size() - 1, + std::vector(accel_map_value_.at(0).size(), map_offset_)); + static Map covariance_vec_( + accel_map_value_.size() + brake_map_value_.size() - 1, + std::vector(accel_map_value_.at(0).size(), covariance_)); + + const int vel_idx = accel_mode ? accel_vel_index : brake_vel_index; + int ped_idx = accel_mode ? accel_pedal_index : brake_pedal_index; + ped_idx = getUnifiedIndexFromAccelBrakeIndex(accel_mode, ped_idx); + double map_offset = map_offset_vec_.at(ped_idx).at(vel_idx); + double covariance = covariance_vec_.at(ped_idx).at(vel_idx); + + /* calculate adaptive map offset */ + const double phi = 1.0; + covariance = (covariance - (covariance * phi * phi * covariance) / + (forgetting_factor_ + phi * covariance * phi)) / + forgetting_factor_; + + const double coef = (covariance * phi) / (forgetting_factor_ + phi * covariance * phi); + + const double error_map_offset = measured_acc - map_acc; + map_offset = map_offset + coef * error_map_offset; + + RCLCPP_DEBUG_STREAM( + get_logger(), "index: " << ped_idx << ", " << vel_idx + << ": map_offset_ = " << map_offset_vec_.at(ped_idx).at(vel_idx) + << " -> " << map_offset << "\t" + << " covariance = " << covariance); + + /* input calculated result and update map */ + map_offset_vec_.at(ped_idx).at(vel_idx) = map_offset; + covariance_vec_.at(ped_idx).at(vel_idx) = covariance; + if (accel_mode) { + update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) = + accel_map_value_.at(accel_pedal_index).at(accel_vel_index) + map_offset; + } else { + update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index) = + brake_map_value_.at(brake_pedal_index).at(brake_vel_index) + map_offset; + } + return true; +} + +void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, const double map_acc) +{ + /* calculate adaptive map offset */ + const double phi = 1.0; + covariance_ = (covariance_ - (covariance_ * phi * phi * covariance_) / + (forgetting_factor_ + phi * covariance_ * phi)) / + forgetting_factor_; + + const double coef = (covariance_ * phi) / (forgetting_factor_ + phi * covariance_ * phi); + const double error_map_offset = measured_acc - map_acc; + map_offset_ = map_offset_ + coef * error_map_offset; + + RCLCPP_DEBUG_STREAM( + get_logger(), "map_offset_ = " << map_offset_ << "\t" + << "covariance = " << covariance_); + + /* update map */ + for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { + for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { + update_accel_map_value_.at(ped_idx).at(vel_idx) = + accel_map_value_.at(ped_idx).at(vel_idx) + map_offset_; + } + } + for (std::size_t ped_idx = 0; ped_idx < update_brake_map_value_.size() - 1; ped_idx++) { + for (std::size_t vel_idx = 0; vel_idx < update_brake_map_value_.at(0).size() - 1; vel_idx++) { + update_brake_map_value_.at(ped_idx).at(vel_idx) = + brake_map_value_.at(ped_idx).at(vel_idx) + map_offset_; + } + } +} + +std::vector AccelBrakeMapCalibrator::getMapColumnFromUnifiedIndex( + const Map & accel_map_value, const Map & brake_map_value, const std::size_t index) +{ + if (index < brake_map_value.size()) { + // input brake map value + return brake_map_value.at(brake_map_value.size() - index - 1); + } else { + // input accel map value + return accel_map_value.at(index - brake_map_value.size() + 1); + } +} + +double AccelBrakeMapCalibrator::getPedalValueFromUnifiedIndex(const std::size_t index) +{ + if (index < brake_pedal_index_.size()) { + // brake index ( minus ) + return -brake_pedal_index_.at(brake_pedal_index_.size() - index - 1); + } else { + // input accel map value + return accel_pedal_index_.at(index - brake_pedal_index_.size() + 1); + } +} + +int AccelBrakeMapCalibrator::getUnifiedIndexFromAccelBrakeIndex( + const bool accel_map, const std::size_t index) +{ + // accel_map=true: accel_map; accel_map=false: brake_map + if (accel_map) { + // accel map + return index + brake_map_value_.size() - 1; + } else { + // brake map + return brake_map_value_.size() - index - 1; + } +} + +double AccelBrakeMapCalibrator::getPitchCompensatedAcceleration() +{ + // It works correctly only when the velocity is positive. + constexpr double gravity = 9.80665; + return gravity * std::sin(pitch_); +} + +void AccelBrakeMapCalibrator::executeEvaluation() +{ + const double full_orig_accel_sq_error = calculateAccelSquaredError( + delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, + accel_map_, brake_map_); + pushDataToVec(full_orig_accel_sq_error, full_mse_que_size_, &full_original_accel_mse_que_); + full_original_accel_rmse_ = getAverage(full_original_accel_mse_que_); + // std::cerr << "rmse : " << sqrt(full_original_accel_rmse_) << std::endl; + + const double full_orig_accel_l1_error = calculateAccelErrorL1Norm( + delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, + accel_map_, brake_map_); + const double full_orig_accel_sq_l1_error = full_orig_accel_l1_error * full_orig_accel_l1_error; + pushDataToVec(full_orig_accel_l1_error, full_mse_que_size_, &full_original_accel_l1_que_); + pushDataToVec(full_orig_accel_sq_l1_error, full_mse_que_size_, &full_original_accel_sq_l1_que_); + full_original_accel_error_l1norm_ = getAverage(full_original_accel_l1_que_); + + /*calculate l1norm_covariance*/ + // const double full_original_accel_error_sql1_ = getAverage(full_original_accel_sq_l1_que_); + // std::cerr << "error_l1norm : " << full_original_accel_error_l1norm_ << std::endl; + // std::cerr << "error_l1_cov : " << + // full_original_accel_error_sql1_-full_original_accel_error_l1norm_*full_original_accel_error_l1norm_ + // << std::endl; + + const double part_orig_accel_sq_error = calculateAccelSquaredError( + delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, + accel_map_, brake_map_); + pushDataToVec(part_orig_accel_sq_error, part_mse_que_size_, &part_original_accel_mse_que_); + part_original_accel_rmse_ = getAverage(part_original_accel_mse_que_); + + const double new_accel_sq_error = calculateAccelSquaredError( + delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, + new_accel_map_, new_brake_map_); + pushDataToVec(new_accel_sq_error, part_mse_que_size_, &new_accel_mse_que_); + new_accel_rmse_ = getAverage(new_accel_mse_que_); +} + +double AccelBrakeMapCalibrator::calculateEstimatedAcc( + const double throttle, const double brake, const double vel, AccelMap & accel_map, + BrakeMap & brake_map) +{ + const double pedal = throttle - brake; + + double estimated_acc = 0.0; + if (pedal > 0.0) { + accel_map.getAcceleration(pedal, vel, estimated_acc); + } else { + brake_map.getAcceleration(-pedal, vel, estimated_acc); + } + + return estimated_acc; +} + +double AccelBrakeMapCalibrator::calculateAccelSquaredError( + const double throttle, const double brake, const double vel, AccelMap & accel_map, + BrakeMap & brake_map) +{ + const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); + const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); + const double dif_acc = measured_acc - estimated_acc; + return dif_acc * dif_acc; +} + +double AccelBrakeMapCalibrator::calculateAccelErrorL1Norm( + const double throttle, const double brake, const double vel, AccelMap & accel_map, + BrakeMap & brake_map) +{ + const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); + const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); + const double dif_acc = measured_acc - estimated_acc; + return abs(dif_acc); +} + +void AccelBrakeMapCalibrator::pushDataToQue( + const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, + std::queue * que) +{ + que->push(data); + while (que->size() > max_size) { + que->pop(); + } +} + +template +void AccelBrakeMapCalibrator::pushDataToVec( + const T data, const std::size_t max_size, std::vector * vec) +{ + vec->emplace_back(data); + while (vec->size() > max_size) { + vec->erase(vec->begin()); + } +} + +template +T AccelBrakeMapCalibrator::getNearestTimeDataFromVec( + const T base_data, const double back_time, const std::vector & vec) +{ + double nearest_time = std::numeric_limits::max(); + const double target_time = rclcpp::Time(base_data->header.stamp).seconds() - back_time; + T nearest_time_data; + for (const auto & data : vec) { + const double data_time = rclcpp::Time(data->header.stamp).seconds(); + const auto delta_time = std::abs(target_time - data_time); + if (nearest_time > delta_time) { + nearest_time_data = data; + nearest_time = delta_time; + } + } + return nearest_time_data; +} + +DataStampedPtr AccelBrakeMapCalibrator::getNearestTimeDataFromVec( + DataStampedPtr base_data, const double back_time, const std::vector & vec) +{ + double nearest_time = std::numeric_limits::max(); + const double target_time = base_data->data_time.seconds() - back_time; + DataStampedPtr nearest_time_data; + for (const auto & data : vec) { + const double data_time = data->data_time.seconds(); + const auto delta_time = std::abs(target_time - data_time); + if (nearest_time > delta_time) { + nearest_time_data = data; + nearest_time = delta_time; + } + } + return nearest_time_data; +} + +double AccelBrakeMapCalibrator::getAverage(const std::vector & vec) +{ + if (vec.empty()) { + return 0.0; + } + + double sum = 0.0; + for (const auto num : vec) { + sum += num; + } + return sum / vec.size(); +} + +double AccelBrakeMapCalibrator::getStandardDeviation(const std::vector & vec) +{ + if (vec.empty()) { + return 0.0; + } + + const double ave = getAverage(vec); + + double sum = 0.0; + for (const auto num : vec) { + sum += std::pow(num - ave, 2); + } + return std::sqrt(sum / vec.size()); +} + +bool AccelBrakeMapCalibrator::isTimeout( + const builtin_interfaces::msg::Time & stamp, const double timeout_sec) +{ + const double dt = this->now().seconds() - rclcpp::Time(stamp).seconds(); + return dt > timeout_sec; +} + +bool AccelBrakeMapCalibrator::isTimeout( + const DataStampedPtr & data_stamped, const double timeout_sec) +{ + const double dt = (this->now() - data_stamped->data_time).seconds(); + return dt > timeout_sec; +} + +OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( + const std::string frame_id, const double height, const double width, const double resolution, + const std::vector & map_value) +{ + OccupancyGrid occ; + occ.header.frame_id = frame_id; + occ.header.stamp = this->now(); + occ.info.height = height; + occ.info.width = width; + occ.info.map_load_time = this->now(); + occ.info.origin.position.x = 0; + occ.info.origin.position.y = 0; + occ.info.origin.position.z = 0; + occ.info.origin.orientation.x = 0; + occ.info.origin.orientation.y = 0; + occ.info.origin.orientation.z = 0; + occ.info.origin.orientation.w = 1; + occ.info.resolution = resolution; + occ.data = map_value; + return occ; +} + +// function for diagnostics +void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; + using CalibrationStatus = CalibrationStatus; + CalibrationStatus accel_brake_map_status; + int8_t level; + std::string msg; + + accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; + if (is_default_map_ == true) { + accel_brake_map_status.status = CalibrationStatus::NORMAL; + level = DiagStatus::OK; + msg = "OK"; + } else { + accel_brake_map_status.status = CalibrationStatus::UNAVAILABLE; + level = DiagStatus::ERROR; + msg = "Default map is not found in " + csv_default_map_dir_; + } + + if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { + // lack of data + stat.summary(level, msg); + calibration_status_pub_->publish(accel_brake_map_status); + return; + } + + const double rmse_rate = new_accel_rmse_ / part_original_accel_rmse_; + if (rmse_rate < update_suggest_thresh_) { + // The accuracy of original accel/brake map is low. + // Suggest to update accel brake map + level = DiagStatus::WARN; + msg = "Accel/brake map Calibration is required."; + accel_brake_map_status.status = CalibrationStatus::CALIBRATION_REQUIRED; + } + + stat.summary(level, msg); + calibration_status_pub_->publish(accel_brake_map_status); +} + +// function for debug + +void AccelBrakeMapCalibrator::publishMap( + const Map accel_map_value, const Map brake_map_value, const std::string publish_type) +{ + if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid map. The number of velocity index of accel map and brake map is different."); + return; + } + const double h = accel_map_value.size() + brake_map_value.size() - + 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) + const double w = accel_map_value.at(0).size(); // velocity + + // publish occupancy map + const int8_t MAX_OCC_VALUE = 100; + std::vector int_map_value; + int_map_value.resize(h * w); + for (int i = 0; i < h; i++) { + for (int j = 0; j < w; j++) { + const double value = + getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j); + // convert acc to 0~100 int value + int8_t int_value = + static_cast(MAX_OCC_VALUE * ((value - min_accel_) / (max_accel_ - min_accel_))); + int_map_value.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_value), (int8_t)0); + } + } + + if (publish_type == "original") { + original_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); + } else { + update_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); + } + + // publish raw map + Float32MultiArray float_map; + + float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + float_map.layout.dim[0].label = "height"; + float_map.layout.dim[1].label = "width"; + float_map.layout.dim[0].size = h; + float_map.layout.dim[1].size = w; + float_map.layout.dim[0].stride = h * w; + float_map.layout.dim[1].stride = w; + float_map.layout.data_offset = 0; + std::vector vec(h * w, 0); + for (int i = 0; i < h; i++) { + for (int j = 0; j < w; j++) { + vec[i * w + j] = static_cast( + getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j)); + } + } + float_map.data = vec; + if (publish_type == "original") { + original_map_raw_pub_->publish(float_map); + } else { + update_map_raw_pub_->publish(float_map); + } +} + +void AccelBrakeMapCalibrator::publishOffsetCovMap( + const Map accel_map_value, const Map brake_map_value) +{ + if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid map. The number of velocity index of accel map and brake map is different."); + return; + } + const double h = accel_map_value.size() + brake_map_value.size() - + 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) + const double w = accel_map_value.at(0).size(); // velocity + + // publish raw map + Float32MultiArray float_map; + + float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + float_map.layout.dim[0].label = "height"; + float_map.layout.dim[1].label = "width"; + float_map.layout.dim[0].size = h; + float_map.layout.dim[1].size = w; + float_map.layout.dim[0].stride = h * w; + float_map.layout.dim[1].stride = w; + float_map.layout.data_offset = 0; + std::vector vec(h * w, 0); + for (int i = 0; i < h; i++) { + for (int j = 0; j < w; j++) { + vec[i * w + j] = + static_cast(getMapColumnFromUnifiedIndex(accel_map_value, brake_map_value, i).at(j)); + } + } + float_map.data = vec; + offset_covariance_pub_->publish(float_map); +} + +void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, const double val) +{ + Float32Stamped msg; + msg.stamp = this->now(); + msg.data = val; + if (publish_type == "current_map_error") { + current_map_error_pub_->publish(msg); + } else if (publish_type == "updated_map_error") { + updated_map_error_pub_->publish(msg); + } else { + map_error_ratio_pub_->publish(msg); + } +} + +void AccelBrakeMapCalibrator::publishCountMap() +{ + if (accel_map_value_.at(0).size() != brake_map_value_.at(0).size()) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid map. The number of velocity index of accel map and brake map is different."); + return; + } + const double h = accel_map_value_.size() + brake_map_value_.size() - + 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) + const double w = accel_map_value_.at(0).size(); // velocity + const int8_t MAX_OCC_VALUE = 100; + + std::vector count_map, ave_map, std_map; + count_map.resize(h * w); + ave_map.resize(h * w); + std_map.resize(h * w); + + for (int i = 0; i < h; i++) { + for (int j = 0; j < w; j++) { + const auto data_vec = map_value_data_.at(i).at(j); + if (data_vec.empty()) { + // input *UNKNOWN* value + count_map.at(i * w + j) = -1; + ave_map.at(i * w + j) = -1; + } else { + const auto count_rate = + MAX_OCC_VALUE * (static_cast(data_vec.size()) / max_data_count_); + count_map.at(i * w + j) = static_cast( + std::max(std::min(static_cast(MAX_OCC_VALUE), static_cast(count_rate)), 0)); + // calculate average + { + const double average = getAverage(data_vec); + int8_t int_average = static_cast( + MAX_OCC_VALUE * ((average - min_accel_) / (max_accel_ - min_accel_))); + ave_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_average), (int8_t)0); + } + // calculate standard deviation + { + const double std_dev = getStandardDeviation(data_vec); + const double max_std_dev = 0.2; + const double min_std_dev = 0.0; + int8_t int_std_dev = static_cast( + MAX_OCC_VALUE * ((std_dev - min_std_dev) / (max_std_dev - min_std_dev))); + std_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_std_dev), (int8_t)0); + } + } + } + } + + // publish average map / standard dev map / count map + data_ave_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, ave_map)); + data_std_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, std_map)); + data_count_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); + + // publish count with self pos map + int nearest_pedal_idx = nearestPedalSearch(); + int nearest_vel_idx = nearestVelSearch(); + + // input self pose (to max value / min value ) and publish this + update_success_ + ? count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::max() + : count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::min(); + data_count_with_self_pose_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); +} + +void AccelBrakeMapCalibrator::publishIndex() +{ + MarkerArray markers; + const double h = accel_map_value_.size() + brake_map_value_.size() - + 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) + const double w = accel_map_value_.at(0).size(); // velocity + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "base_link"; + marker.header.stamp = this->now(); + marker.type = marker.TEXT_VIEW_FACING; + marker.action = marker.ADD; + std_msgs::msg::ColorRGBA color; + color.a = 0.999; + color.b = 0.1; + color.g = 0.1; + color.r = 0.1; + marker.color = color; + marker.frame_locked = true; + marker.pose.position.z = 0.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + + // pedal value + for (int pedal_idx = 0; pedal_idx < h; pedal_idx++) { + const double pedal_value = getPedalValueFromUnifiedIndex(pedal_idx); + marker.ns = "occ_pedal_index"; + marker.id = pedal_idx; + marker.pose.position.x = -map_resolution_ * 0.5; + marker.pose.position.y = map_resolution_ * (0.5 + pedal_idx); + marker.scale.z = 0.03; + std::stringstream stream; + stream << std::fixed << std::setprecision(2) << pedal_value; + marker.text = stream.str(); + markers.markers.push_back(marker); + } + + // pedal index name + marker.ns = "occ_pedal_index"; + marker.id = h; + marker.pose.position.x = -map_resolution_ * 0.5; + marker.pose.position.y = map_resolution_ * (0.5 + h); + marker.scale.z = 0.03; + marker.text = "(accel_pedal/brake_pedal)"; + markers.markers.push_back(marker); + + // velocity value + for (int vel_idx = 0; vel_idx < w; vel_idx++) { + const double vel_value = accel_vel_index_.at(vel_idx); + marker.ns = "occ_vel_index"; + marker.id = vel_idx; + marker.pose.position.x = map_resolution_ * (0.5 + vel_idx); + marker.pose.position.y = -map_resolution_ * 0.2; + marker.scale.z = 0.02; + std::stringstream stream; + stream << std::fixed << std::setprecision(2) << vel_value; + marker.text = stream.str() + "m/s"; + markers.markers.push_back(marker); + } + + // velocity index name + marker.ns = "occ_vel_index"; + marker.id = w; + marker.pose.position.x = map_resolution_ * (0.5 + w); + marker.pose.position.y = -map_resolution_ * 0.2; + marker.scale.z = 0.02; + marker.text = "(velocity)"; + markers.markers.push_back(marker); + + index_pub_->publish(markers); +} + +void AccelBrakeMapCalibrator::publishUpdateSuggestFlag() +{ + std_msgs::msg::Bool update_suggest; + + if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { + // lack of data + update_suggest.data = false; + } else { + const double rmse_rate = new_accel_rmse_ / part_original_accel_rmse_; + update_suggest.data = (rmse_rate < update_suggest_thresh_); + if (update_suggest.data) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 5000, + "suggest to update accel/brake map. evaluation score = " << rmse_rate); + } + } + + update_suggest_pub_->publish(update_suggest); +} + +bool AccelBrakeMapCalibrator::writeMapToCSV( + std::vector vel_index, std::vector pedal_index, Map value_map, + std::string filename) +{ + if (update_success_count_ == 0) { + return false; + } + + std::ofstream csv_file(filename); + + if (!csv_file.is_open()) { + RCLCPP_WARN(get_logger(), "Failed to open csv file : %s", filename.c_str()); + return false; + } + + csv_file << "default,"; + for (std::size_t v = 0; v < vel_index.size(); v++) { + csv_file << vel_index.at(v); + if (v != vel_index.size() - 1) { + csv_file << ","; + } + } + csv_file << "\n"; + + for (std::size_t p = 0; p < pedal_index.size(); p++) { + csv_file << pedal_index.at(p) << ","; + for (std::size_t v = 0; v < vel_index.size(); v++) { + csv_file << std::fixed << std::setprecision(precision_) << value_map.at(p).at(v); + if (v != vel_index.size() - 1) { + csv_file << ","; + } + } + csv_file << "\n"; + } + csv_file.close(); + RCLCPP_DEBUG_STREAM(get_logger(), "output map to " << filename); + return true; +} + +void AccelBrakeMapCalibrator::addIndexToCSV(std::ofstream * csv_file) +{ + *csv_file << "timestamp,velocity,accel,pitch_comp_accel,final_accel,accel_pedal,brake_pedal," + << "accel_pedal_speed,brake_pedal_speed,pitch,steer,jerk,full_original_accel_rmse, " + "part_original_accel_rmse,new_accel_rmse, rmse_rate" + << std::endl; +} + +void AccelBrakeMapCalibrator::addLogToCSV( + std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, + const double pitched_accel, const double accel_pedal, const double brake_pedal, + const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, + const double steer, const double jerk, const double full_original_accel_mse, + const double part_original_accel_mse, const double new_accel_mse) +{ + const double rmse_rate = + part_original_accel_mse == 0.0 ? 1.0 : (new_accel_mse / part_original_accel_mse); + *csv_file << timestamp << ", " << velocity << ", " << accel << ", " << pitched_accel << ", " + << accel - pitched_accel << ", " << accel_pedal << ", " << brake_pedal << ", " + << accel_pedal_speed << ", " << brake_pedal_speed << ", " << pitch << ", " << steer + << ", " << jerk << ", " << full_original_accel_mse << ", " << part_original_accel_mse + << ", " << new_accel_mse << "," << rmse_rate << std::endl; +} + +} // namespace accel_brake_map_calibrator diff --git a/vehicle/accel_brake_map_calibrator/src/main.cpp b/vehicle/accel_brake_map_calibrator/src/main.cpp new file mode 100644 index 00000000..01dfe203 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/src/main.cpp @@ -0,0 +1,28 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + rclcpp::spin(std::make_shared(node_options)); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py new file mode 100755 index 00000000..1bbb5820 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -0,0 +1,122 @@ +#! /usr/bin/env python3 + +# Copyright 2022 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 os + +from ament_index_python.packages import get_package_share_directory +import rclpy +from rclpy.node import Node +from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData + + +class CalibrationDataRelay(Node): + def __init__(self, args): + super().__init__("plot_client") + self.cli = self.create_client( + CalibData, "/api/external/get/accel_brake_map_calibrator/data" + ) + + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available, waiting again...") + + self.request = CalibData.Request() + + def send_request(self): + self.future = self.cli.call_async(self.request) + + +def main(args=None): + rclpy.init(args=None) + client = CalibrationDataRelay(args) + client.send_request() + while rclpy.ok(): + rclpy.spin_once(client) + if client.future.done(): + try: + response = client.future.result() + save_dir = "./test_data_save" + if not os.path.exists(save_dir): + os.makedirs(save_dir) + svg_name = save_dir + "/graph.svg" + + f_svg = open(svg_name, "w") + svg_byte = bytes(response.graph_image) + text = svg_byte.decode() + f_svg.write(text) + + print("svg done") + + acc_map_name = save_dir + "/accel_map.csv" + f_acc = open(acc_map_name, "w") + f_acc.write(response.accel_map) + + print("accel map done") + + brk_map_name = save_dir + "/brake_map.csv" + f_brk = open(brk_map_name, "w") + f_brk.write(response.brake_map) + + print("brake map done") + + except Exception as e: + client.get_logger().info("Service call failed %r" % (e,)) + + break + + client.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + package_path = get_package_share_directory("accel_brake_map_calibrator") + parser = argparse.ArgumentParser() + parser.add_argument( + "-d", "--default-map-dir", default=None, type=str, help="directory of default map" + ) + parser.add_argument( + "-c", "--calibrated-map-dir", default=None, type=str, help="directory of calibrated map" + ) + parser.add_argument("-s", "--scatter-only", action="store_true", help="show only scatters") + parser.add_argument( + "-l", + "--log-file", + default=package_path + "/config/log.csv", + type=str, + help="path of log.csv", + ) + parser.add_argument( + "--min-vel-thr", default=0.1, type=float, help="valid min velocity threshold" + ) + parser.add_argument( + "--vel-diff-thr", default=0.556, type=float, help="valid velocity diff threshold" + ) + parser.add_argument( + "--pedal-diff-thr", default=0.03, type=float, help="valid pedal diff threshold" + ) + parser.add_argument( + "--max-steer-thr", default=0.2, type=float, help="valid max steer threshold" + ) + parser.add_argument( + "--max-pitch-thr", default=0.02, type=float, help="valid max pitch threshold" + ) + parser.add_argument("--max-jerk-thr", default=0.7, type=float, help="valid max jerk threshold") + parser.add_argument( + "--max-pedal-vel-thr", default=0.7, type=float, help="valid max pedal velocity threshold" + ) + + args = parser.parse_args() + main(args) diff --git a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py new file mode 100755 index 00000000..9e57813b --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -0,0 +1,62 @@ +#! /usr/bin/env python3 + +# Copyright 2022 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 autoware_auto_vehicle_msgs.msg import VelocityReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from tier4_vehicle_msgs.msg import ActuationStatusStamped + + +class ActuationStatusPublisher(Node): + def __init__(self): + super().__init__("actuation_status_publisher") + + qos_profile = QoSProfile(depth=1) + qos_profile.reliability = QoSReliabilityPolicy.RELIABLE + qos_profile.history = QoSHistoryPolicy.KEEP_LAST + qos_profile.durability = QoSDurabilityPolicy.VOLATILE + + self.pub = self.create_publisher( + ActuationStatusStamped, "/vehicle/status/actuation_status", qos_profile + ) + self.sub = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.callback, qos_profile + ) + + def callback(self, msg): + data = ActuationStatusStamped() + data.header = msg.header + data.status.accel_status = msg.longitudinal_velocity * 0.1 + data.status.brake_status = 0.1 + data.status.steer_status = 0.1 + self.pub.publish(data) + + +def main(args=None): + rclpy.init(args=args) + node = ActuationStatusPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()