From 3e25bc582242baf353855deb17082c871af54886 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Sun, 30 Jan 2022 03:16:38 +0900 Subject: [PATCH 01/33] ci: add sync-upstream.yaml (#4) Signed-off-by: Kenji Miyake --- .github/workflows/sync-upstream.yaml | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 .github/workflows/sync-upstream.yaml diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml new file mode 100644 index 0000000000000..cd0d5af4883a3 --- /dev/null +++ b/.github/workflows/sync-upstream.yaml @@ -0,0 +1,28 @@ +name: sync-upstream + +on: + schedule: + - cron: 0 19 * * * # run at 4 AM JST + workflow_dispatch: + +jobs: + sync-upstream: + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run sync-branches + uses: autowarefoundation/autoware-github-actions/sync-branches@tier4/proposal + with: + token: ${{ steps.generate-token.outputs.token }} + base-branch: tier4/main + sync-pr-branch: sync-upstream + sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git + sync-target-branch: tier4/proposal + pr-title: "chore: sync upstream" + auto-merge-method: merge From bfb6ec83ff70ea2b4323cbcd11c1d05fa611fca9 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 3 Mar 2022 16:34:55 +0900 Subject: [PATCH 02/33] ci(sync-upstream): update settings (#19) Signed-off-by: Kenji Miyake --- .github/workflows/sync-upstream.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml index cd0d5af4883a3..3e164c07dbec1 100644 --- a/.github/workflows/sync-upstream.yaml +++ b/.github/workflows/sync-upstream.yaml @@ -2,7 +2,7 @@ name: sync-upstream on: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: @@ -17,12 +17,12 @@ jobs: private_key: ${{ secrets.PRIVATE_KEY }} - name: Run sync-branches - uses: autowarefoundation/autoware-github-actions/sync-branches@tier4/proposal + uses: autowarefoundation/autoware-github-actions/sync-branches@v1 with: token: ${{ steps.generate-token.outputs.token }} base-branch: tier4/main sync-pr-branch: sync-upstream sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git - sync-target-branch: tier4/proposal + sync-target-branch: main pr-title: "chore: sync upstream" auto-merge-method: merge From 41a9d49d9d3ae53191bde4025aa8b071375c3946 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 1 Apr 2022 10:51:31 +0900 Subject: [PATCH 03/33] (editting) add node for creating calibration data Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator_node.hpp | 11 + .../scripts/plot_csv_server_node.py | 315 ++++++++++++++++++ .../src/accel_brake_map_calibrator_node.cpp | 44 ++- 3 files changed, 368 insertions(+), 2 deletions(-) create mode 100755 vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 1874e77c868bc..4ca4ae7f90601 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -37,6 +37,9 @@ #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" #include "tier4_vehicle_msgs/srv/update_accel_brake_map.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 #include @@ -77,6 +80,8 @@ class AccelBrakeMapCalibrator : public rclcpp::Node 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_; @@ -85,6 +90,8 @@ class AccelBrakeMapCalibrator : public rclcpp::Node // Service rclcpp::Service::SharedPtr update_map_dir_server_; + rclcpp::Service::SharedPtr + calibration_data_server_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_output_csv_; @@ -212,6 +219,10 @@ class AccelBrakeMapCalibrator : public rclcpp::Node const std::shared_ptr request_header, tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Request::SharedPtr req, tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Response::SharedPtr res); + bool callbackUpdateCalibrationDataService( + const std::shared_ptr request_header, + tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::Request::SharedPtr req, + tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::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( diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py new file mode 100755 index 0000000000000..7c2f5766b8db3 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -0,0 +1,315 @@ +#! /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 +import matplotlib.pyplot as plt +from tier4_autoware_msgs.tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData + + +class DrawGraph(Node): + def __init__(self, args): + super().__init__("plot_viewer") + default_map_dir = args.default_map_dir + calibrated_map_dir = args.calibrated_map_dir + 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 + ) + + print("default map dir: {}".format(default_map_dir)) + print("calibrated map dir: {}".format(calibrated_map_dir)) + + # 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) + 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, + ) + plt.savefig("plot.svg") + + svg = open('plot.svg', 'r') + while True: + data = svg.readline() + # stringにした場合は以下のfor文を消して下記の処理を行う + # Calib.graph_image.append(data) + for c in data: + CalibData.graph_image.append(ord(c)) + + if data == '': + break + + + 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 = DrawGraph(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("-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/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 60873f674fbd4..2fb1795299bbe 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -174,6 +174,8 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod "/accel_brake_map_calibrator/output/updated_map_error", durable_qos); map_error_ratio_pub_ = create_publisher( "/accel_brake_map_calibrator/output/map_error_ratio", durable_qos); + calibration_status_pub_ = create_publisher( + "/accel_brake_map_calibrator/output/calibration_status", durable_qos); // subscriber using std::placeholders::_1; @@ -193,7 +195,9 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod update_map_dir_server_ = create_service( "~/input/update_map_dir", std::bind(&AccelBrakeMapCalibrator::callbackUpdateMapService, this, _1, _2, _3)); - + calibration_data_server_ = create_service( + "~/input/calibration_data_request", + std::bind(&AccelBrakeMapCalibrator::callbackUpdateCalibrationDataService, this, _1, _2, _3)); // timer initTimer(1.0 / update_hz_); initOutputCSVTimer(30.0); @@ -547,6 +551,35 @@ bool AccelBrakeMapCalibrator::callbackUpdateMapService( return true; } +bool AccelBrakeMapCalibrator::callbackUpdateCalibrationDataService( + const std::shared_ptr request_header, + tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::Request::SharedPtr req, + tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::Response::SharedPtr res) + { + /* 処理の内容メモ(リクエストを受けたら) + ・グラフを作成 + ・グラフを変数に格納 + ・マップをCSVに書き込み(または書き込まずにそのまま) + ・CSVに書き込んだ場合は読み込む + ・読み込んだCSVを変数に格納 + ・ + */ + uint8 graph_image; + string accel_map; + string brake_map; + std::string update_map_dir = ""; + std::string graph_dir = ""; + const auto accel_map_file = update_map_dir + "/accel_map.csv"; + const auto brake_map_file = update_map_dir + "/brake_map.csv"; + drawCalibrationGraph(graph_dir); + 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); + // writeMapToCSVの処理を変えて、入れている値を文字に変換しながら string に詰める方が良い + + + return true; + } + double AccelBrakeMapCalibrator::lowpass( const double original, const double current, const double gain) { @@ -1083,10 +1116,15 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS int8_t level = DiagStatus::OK; std::string msg = "OK"; + using CalibrationStatus = tier4_external_api_msgs::msg::CalibrationStatus; + CalibrationStatus accel_brake_map_status; + accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; + accel_brake_map_status.status = CalibrationStatus::NORMAL; + 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; } @@ -1096,9 +1134,11 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS // Suggest to update accel brake map level = DiagStatus::WARN; msg = "Accel/brake map Calibration is required."; + accel_brake_map_status.status = CalibrationStatus::WARNING; } stat.summary(level, msg); + calibration_status_pub_->publish(accel_brake_map_status); } // function for debug From 2cf85285fc49ea442a758cd8f1abff807b7a08b6 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Mon, 4 Apr 2022 14:45:21 +0900 Subject: [PATCH 04/33] (editting) modify server node Signed-off-by: TakumiKozaka-T4 --- .../scripts/plot_csv_server_node.py | 44 ++++++++++++------- 1 file changed, 29 insertions(+), 15 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index 7c2f5766b8db3..b2e2e3927012b 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -17,6 +17,7 @@ import argparse import math +import rospy from ament_index_python.packages import get_package_share_directory from calc_utils import CalcUtils @@ -31,10 +32,13 @@ class DrawGraph(Node): + calibrated_map_dir='' def __init__(self, args): - super().__init__("plot_viewer") + super().__init__("plot_server") + self.srv=self.create_service(CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback) + default_map_dir = args.default_map_dir - calibrated_map_dir = args.calibrated_map_dir + self.calibrated_map_dir = args.calibrated_map_dir scatter_only = args.scatter_only log_file = args.log_file min_vel_thr = args.min_vel_thr @@ -56,19 +60,19 @@ def __init__(self, args): .string_value ) - if calibrated_map_dir is None: + if self.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.calibrated_map_dir = ( self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") .get_parameter_value() .string_value ) print("default map dir: {}".format(default_map_dir)) - print("calibrated map dir: {}".format(calibrated_map_dir)) + print("calibrated map dir: {}".format(self.calibrated_map_dir)) # read csv self.cr = CSVReader(log_file, csv_type="file") @@ -106,10 +110,10 @@ def __init__(self, args): 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) + calibrated_pedal_list, calibrated_acc_list = self.load_map(DrawGraph.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) + "No calibrated map file was found in {}".format(DrawGraph.calibrated_map_dir) ) # visualize point from data @@ -133,17 +137,27 @@ def __init__(self, args): ) plt.savefig("plot.svg") - svg = open('plot.svg', 'r') - while True: - data = svg.readline() + def get_data_callback(self, response): + with open('plot.svg', 'r') as svg: + + for data in svg # stringにした場合は以下のfor文を消して下記の処理を行う # Calib.graph_image.append(data) - for c in data: - CalibData.graph_image.append(ord(c)) + for c in data: + response.graph_image.append(ord(c)) - if data == '': - break - + if data == '': + break + + with open(self.calibrated_map_dir+"accel_map.csv", 'r') as calibrated_accel_map: + for accel_data in calibrated_accel_map: + response.accel_map.append(accel_data) + + with open(self.calibrated_map_dir+"brake_map.csv", 'r') as calibrated_brake_map: + for brake_data in calibrated_brake_map : + response.brake_map.append(brake_data) + + return response def plotter_function(self): return self.plotter From b222a9cc1b74d0cfa333b8354bac3147c2dd7330 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Tue, 12 Apr 2022 15:20:38 +0900 Subject: [PATCH 05/33] (editting) add server and client Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator_node.hpp | 9 +- .../scripts/plot_csv_client_node.py | 128 ++++++++++++++++++ .../scripts/plot_csv_server_node.py | 33 +++-- .../src/accel_brake_map_calibrator_node.cpp | 33 +---- 4 files changed, 154 insertions(+), 49 deletions(-) create mode 100755 vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 4ca4ae7f90601..b4f1f4e94af51 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -41,6 +41,7 @@ #include "tier4_external_api_msgs/msg/calibration_status_array.hpp" #include "tier4_external_api_msgs/srv/get_accel_brake_map_calibration_data.hpp" + #include #include #include @@ -90,8 +91,6 @@ class AccelBrakeMapCalibrator : public rclcpp::Node // Service rclcpp::Service::SharedPtr update_map_dir_server_; - rclcpp::Service::SharedPtr - calibration_data_server_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_output_csv_; @@ -218,11 +217,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node bool callbackUpdateMapService( const std::shared_ptr request_header, tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Request::SharedPtr req, - tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Response::SharedPtr res); - bool callbackUpdateCalibrationDataService( - const std::shared_ptr request_header, - tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::Request::SharedPtr req, - tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::Response::SharedPtr res); + tier4_vehicle_msgs::srv::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( diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py new file mode 100755 index 0000000000000..fbe92a32fbff1 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py @@ -0,0 +1,128 @@ +#! /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 +# from http import client +# import math +import os + +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 +from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData + +class CalibrationDataRelay(Node): + def __init__(self, args): + super().__init__("plot_server") + self.cli=self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") + + 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" + + print(svg_name) #for debug + + # f_svg = open(svg_name, "w") + # f_svg.write(response.graph_image) + + # print("svg done") #for debug + + 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") #for debug + + 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") #for debug + + 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/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index b2e2e3927012b..105a54a14d796 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -17,7 +17,6 @@ import argparse import math -import rospy from ament_index_python.packages import get_package_share_directory from calc_utils import CalcUtils @@ -28,7 +27,7 @@ import rclpy from rclpy.node import Node import matplotlib.pyplot as plt -from tier4_autoware_msgs.tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData +from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData class DrawGraph(Node): @@ -110,10 +109,10 @@ def __init__(self, args): 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(DrawGraph.calibrated_map_dir) + 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(DrawGraph.calibrated_map_dir) + "No calibrated map file was found in {}".format(self.calibrated_map_dir) ) # visualize point from data @@ -137,25 +136,39 @@ def __init__(self, args): ) plt.savefig("plot.svg") - def get_data_callback(self, response): + def get_data_callback(self, request, response): with open('plot.svg', 'r') as svg: + count=0 - for data in svg + for data in svg: # stringにした場合は以下のfor文を消して下記の処理を行う # Calib.graph_image.append(data) + data = str(data) for c in data: - response.graph_image.append(ord(c)) + # for debug + count += 1 + print("%d, %s, %s", count, c, type(c)) + + # data processing + # response.graph_image.append(ord(c)) if data == '': - break + print("Data end") + break + + print("svg data end") #for debug with open(self.calibrated_map_dir+"accel_map.csv", 'r') as calibrated_accel_map: for accel_data in calibrated_accel_map: - response.accel_map.append(accel_data) + response.accel_map += accel_data + + print("accel map end") #for debug with open(self.calibrated_map_dir+"brake_map.csv", 'r') as calibrated_brake_map: for brake_data in calibrated_brake_map : - response.brake_map.append(brake_data) + response.brake_map += brake_data + + print("brake map end") #for debug return response diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 2fb1795299bbe..7adef163e0a82 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -195,9 +195,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod update_map_dir_server_ = create_service( "~/input/update_map_dir", std::bind(&AccelBrakeMapCalibrator::callbackUpdateMapService, this, _1, _2, _3)); - calibration_data_server_ = create_service( - "~/input/calibration_data_request", - std::bind(&AccelBrakeMapCalibrator::callbackUpdateCalibrationDataService, this, _1, _2, _3)); + // timer initTimer(1.0 / update_hz_); initOutputCSVTimer(30.0); @@ -551,35 +549,6 @@ bool AccelBrakeMapCalibrator::callbackUpdateMapService( return true; } -bool AccelBrakeMapCalibrator::callbackUpdateCalibrationDataService( - const std::shared_ptr request_header, - tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::Request::SharedPtr req, - tier4_vehicle_msgs::srv::GetAccelBrakeMapCalibrationData::Response::SharedPtr res) - { - /* 処理の内容メモ(リクエストを受けたら) - ・グラフを作成 - ・グラフを変数に格納 - ・マップをCSVに書き込み(または書き込まずにそのまま) - ・CSVに書き込んだ場合は読み込む - ・読み込んだCSVを変数に格納 - ・ - */ - uint8 graph_image; - string accel_map; - string brake_map; - std::string update_map_dir = ""; - std::string graph_dir = ""; - const auto accel_map_file = update_map_dir + "/accel_map.csv"; - const auto brake_map_file = update_map_dir + "/brake_map.csv"; - drawCalibrationGraph(graph_dir); - 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); - // writeMapToCSVの処理を変えて、入れている値を文字に変換しながら string に詰める方が良い - - - return true; - } - double AccelBrakeMapCalibrator::lowpass( const double original, const double current, const double gain) { From 791ddb9bda6d3ff71f75520b54790e389fbf0e59 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Tue, 12 Apr 2022 15:22:03 +0900 Subject: [PATCH 06/33] (editting) add client and server Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator/CMakeLists.txt | 2 ++ .../accel_brake_map_calibrator/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt index 9d713ecaf84d7..69bb18bdeb659 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt @@ -35,6 +35,8 @@ install( scripts/csv_reader.py scripts/log_analyzer.py scripts/view_plot.py + scripts/plot_csv_client_node.py + scripts/plot_csv_server_node.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml index d8971e55a3976..859ed5f2f156a 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/package.xml @@ -19,6 +19,7 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs + tier4_external_api_msgs tier4_vehicle_msgs visualization_msgs From f30d3dd330872e46160d8de218915c2525010992 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Wed, 13 Apr 2022 15:34:49 +0900 Subject: [PATCH 07/33] modify server and client Signed-off-by: TakumiKozaka-T4 --- .../scripts/plot_csv_client_node.py | 8 ++-- .../scripts/plot_csv_server_node.py | 38 +++++++------------ 2 files changed, 18 insertions(+), 28 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py index fbe92a32fbff1..6000d765fecb0 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py @@ -59,10 +59,12 @@ def main(args=None): print(svg_name) #for debug - # f_svg = open(svg_name, "w") - # f_svg.write(response.graph_image) + f_svg = open(svg_name, "w") + for i in response.graph_image: + c = chr(i) + f_svg.write(c) - # print("svg done") #for debug + print("svg done") #for debug acc_map_name = save_dir + "/accel_map.csv" f_acc = open(acc_map_name, "w") diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index 105a54a14d796..4824b22e5d25b 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -135,40 +135,28 @@ def __init__(self, args): scatter_only, ) plt.savefig("plot.svg") + print("svg saved") def get_data_callback(self, request, response): - with open('plot.svg', 'r') as svg: - count=0 - - for data in svg: - # stringにした場合は以下のfor文を消して下記の処理を行う - # Calib.graph_image.append(data) - data = str(data) - for c in data: - # for debug - count += 1 - print("%d, %s, %s", count, c, type(c)) - - # data processing - # response.graph_image.append(ord(c)) - - if data == '': - print("Data end") - break - - print("svg data end") #for debug + with open('plot.svg', 'r') as f: + svg = f.read() + for c in svg: + # The character "-" (minus) in graph label is saved as the unicode letter U+2212 for some reason. + # U+2212 is changed to minus here for packing data + if c == '−': + c = '-' + response.graph_image.append(ord(c)) + print("svg data packed") 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 - - print("accel map end") #for debug - + print("accel map packed") + 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 - - print("brake map end") #for debug + print("brake map packed") return response From ee4a6febd8b976f3a1e44fa3454a56da9fad8a53 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 15 Apr 2022 19:56:24 +0900 Subject: [PATCH 08/33] modify client and server Signed-off-by: TakumiKozaka-T4 --- .../scripts/plot_csv_client_node.py | 14 +++++------ .../scripts/plot_csv_server_node.py | 24 ++++++++++++------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py index 6000d765fecb0..113b6aca95859 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py @@ -57,26 +57,24 @@ def main(args=None): os.makedirs(save_dir) svg_name = save_dir + "/graph.svg" - print(svg_name) #for debug - f_svg = open(svg_name, "w") - for i in response.graph_image: - c = chr(i) - f_svg.write(c) + svg_byte = bytes(response.graph_image) + text = svg_byte.decode() + f_svg.write(text) - print("svg done") #for debug + 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") #for debug + 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") #for debug + print("brake map done") except Exception as e: client.get_logger().info( diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index 4824b22e5d25b..2faa5810d2342 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -27,6 +27,7 @@ import rclpy from rclpy.node import Node import matplotlib.pyplot as plt +from pathlib import Path from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData @@ -138,14 +139,21 @@ def __init__(self, args): print("svg saved") def get_data_callback(self, request, response): - with open('plot.svg', 'r') as f: - svg = f.read() - for c in svg: - # The character "-" (minus) in graph label is saved as the unicode letter U+2212 for some reason. - # U+2212 is changed to minus here for packing data - if c == '−': - c = '-' - response.graph_image.append(ord(c)) + + text = Path("plot.svg").read_text() + byte = text.encode() + for b in byte: + print(b, chr(b)) + response.graph_image.append(b) + # 成功している手順 + # with open('plot.svg', 'r') as f: + # svg = f.read() + # for c in svg: + # # The character "-" (minus) in graph label is saved as the unicode letter U+2212 for some reason. + # # U+2212 is changed to minus here for packing data + # if c == '−': + # c = '-' + # response.graph_image.append(ord(c)) print("svg data packed") with open(self.calibrated_map_dir+"accel_map.csv", 'r') as calibrated_accel_map: From df9a1d94a196b4e8911068c54a904a5b27b83905 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 15 Apr 2022 19:57:22 +0900 Subject: [PATCH 09/33] modify server node Signed-off-by: TakumiKozaka-T4 --- .../scripts/plot_csv_server_node.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index 2faa5810d2342..89ae512787789 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -145,15 +145,6 @@ def get_data_callback(self, request, response): for b in byte: print(b, chr(b)) response.graph_image.append(b) - # 成功している手順 - # with open('plot.svg', 'r') as f: - # svg = f.read() - # for c in svg: - # # The character "-" (minus) in graph label is saved as the unicode letter U+2212 for some reason. - # # U+2212 is changed to minus here for packing data - # if c == '−': - # c = '-' - # response.graph_image.append(ord(c)) print("svg data packed") with open(self.calibrated_map_dir+"accel_map.csv", 'r') as calibrated_accel_map: From 13a705b98844c9c6d24af037902299c9e75cd461 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 15 Apr 2022 19:58:27 +0900 Subject: [PATCH 10/33] delete debug codes Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator/scripts/plot_csv_server_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index 89ae512787789..328305182f853 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -143,7 +143,6 @@ def get_data_callback(self, request, response): text = Path("plot.svg").read_text() byte = text.encode() for b in byte: - print(b, chr(b)) response.graph_image.append(b) print("svg data packed") From 775b6a798a36900248c00a2fc383d6cd9529da73 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Mon, 18 Apr 2022 09:49:54 +0900 Subject: [PATCH 11/33] run pre-commit Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator_node.hpp | 9 +++--- .../scripts/plot_csv_client_node.py | 20 +++++++------ .../scripts/plot_csv_server_node.py | 29 ++++++++++--------- .../src/accel_brake_map_calibrator_node.cpp | 2 +- 4 files changed, 32 insertions(+), 28 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index b4f1f4e94af51..e607197550414 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -35,12 +35,11 @@ #include "std_msgs/msg/string.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" -#include "tier4_vehicle_msgs/srv/update_accel_brake_map.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_status_stamped.hpp" +#include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" #include #include @@ -81,7 +80,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr current_map_error_pub_; rclcpp::Publisher::SharedPtr updated_map_error_pub_; rclcpp::Publisher::SharedPtr map_error_ratio_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr calibration_status_pub_; rclcpp::Subscription::SharedPtr velocity_sub_; @@ -217,7 +216,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node bool callbackUpdateMapService( const std::shared_ptr request_header, tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Request::SharedPtr req, - tier4_vehicle_msgs::srv::UpdateAccelBrakeMap::Response::SharedPtr res); + tier4_vehicle_msgs::srv::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( diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py index 113b6aca95859..4f74a8c628cc5 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py @@ -16,11 +16,13 @@ # limitations under the License. import argparse + # from http import client # import math import os from ament_index_python.packages import get_package_share_directory + # from calc_utils import CalcUtils # import config as CF # from csv_reader import CSVReader @@ -30,18 +32,20 @@ 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_server") - self.cli=self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") + self.cli = self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + 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) + self.future = self.cli.call_async(self.request) + def main(args=None): rclpy.init(args=None) @@ -62,24 +66,22 @@ def main(args=None): text = svg_byte.decode() f_svg.write(text) - print("svg done") + 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") + 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") + print("brake map done") except Exception as e: - client.get_logger().info( - 'Service call failed %r' % (e,)) - + client.get_logger().info("Service call failed %r" % (e,)) break diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index 328305182f853..d27c61b2327d3 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -17,25 +17,28 @@ import argparse 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 -import matplotlib.pyplot as plt -from pathlib import Path from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData class DrawGraph(Node): - calibrated_map_dir='' + calibrated_map_dir = "" + def __init__(self, args): super().__init__("plot_server") - self.srv=self.create_service(CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback) + self.srv = self.create_service( + CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback + ) default_map_dir = args.default_map_dir self.calibrated_map_dir = args.calibrated_map_dir @@ -144,18 +147,18 @@ def get_data_callback(self, request, response): byte = text.encode() for b in byte: response.graph_image.append(b) - print("svg data packed") + print("svg data packed") - with open(self.calibrated_map_dir+"accel_map.csv", 'r') as calibrated_accel_map: - for accel_data in calibrated_accel_map: + 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 print("accel map packed") - - with open(self.calibrated_map_dir+"brake_map.csv", 'r') as calibrated_brake_map: - for brake_data in calibrated_brake_map : + + 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 - print("brake map packed") - + print("brake map packed") + return response def plotter_function(self): @@ -276,7 +279,7 @@ def load_map(self, csv_dir): ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) except OSError as e: print(e) - return [], [] + return [], [] def main(args=None): diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 7adef163e0a82..5b9875cd58ab6 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -195,7 +195,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod 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); From ad6a8113174b53c454ace0b1f669b4d679fef5c3 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Thu, 21 Apr 2022 17:22:45 +0900 Subject: [PATCH 12/33] rebase to autowarefoundation/autoware/main Signed-off-by: TakumiKozaka-T4 From 1b0ad75b57a6d53f43b8cc9cbbe4935b0324ec5f Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 <70260442+TakumiKozaka-T4@users.noreply.github.com> Date: Mon, 25 Apr 2022 17:03:03 +0900 Subject: [PATCH 13/33] Delete sync-upstream.yaml This file is added by mistake. --- .github/workflows/sync-upstream.yaml | 28 ---------------------------- 1 file changed, 28 deletions(-) delete mode 100644 .github/workflows/sync-upstream.yaml diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml deleted file mode 100644 index 3e164c07dbec1..0000000000000 --- a/.github/workflows/sync-upstream.yaml +++ /dev/null @@ -1,28 +0,0 @@ -name: sync-upstream - -on: - schedule: - - cron: 0 0 * * * - workflow_dispatch: - -jobs: - sync-upstream: - runs-on: ubuntu-latest - steps: - - name: Generate token - id: generate-token - uses: tibdex/github-app-token@v1 - with: - app_id: ${{ secrets.APP_ID }} - private_key: ${{ secrets.PRIVATE_KEY }} - - - name: Run sync-branches - uses: autowarefoundation/autoware-github-actions/sync-branches@v1 - with: - token: ${{ steps.generate-token.outputs.token }} - base-branch: tier4/main - sync-pr-branch: sync-upstream - sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git - sync-target-branch: main - pr-title: "chore: sync upstream" - auto-merge-method: merge From f25352d6b03db69b732bb2d5fd39da56966baa29 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Thu, 12 May 2022 15:07:19 +0900 Subject: [PATCH 14/33] merge the latest version of universe main From dcf3f5d768c02f70077d50f065317756e4910300 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Tue, 17 May 2022 12:23:47 +0900 Subject: [PATCH 15/33] return empty data when error occurs Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator.launch.xml | 2 + .../scripts/plot_csv_server_node.py | 108 ++++++++++++------ 2 files changed, 73 insertions(+), 37 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml index d8184e86f94bf..57136876e439c 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml @@ -24,6 +24,8 @@ + + diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index d27c61b2327d3..bde19acc719e9 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -40,24 +40,26 @@ def __init__(self, args): CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback ) - default_map_dir = args.default_map_dir + self.default_map_dir = args.default_map_dir self.calibrated_map_dir = args.calibrated_map_dir - 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: + self.scatter_only = args.scatter_only + self.log_file = args.log_file + self.min_vel_thr = args.min_vel_thr + self.vel_diff_thr = args.vel_diff_thr + self.pedal_diff_thr = args.pedal_diff_thr + self.max_steer_thr = args.max_steer_thr + self.max_pitch_thr = args.max_pitch_thr + self.max_jerk_thr = args.max_jerk_thr + self.max_pedal_vel_thr = args.max_pedal_vel_thr + + def get_data_callback(self, request, response): + + if self.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.default_map_dir = ( self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") .get_parameter_value() .string_value @@ -74,15 +76,32 @@ def __init__(self, args): .string_value ) - print("default map dir: {}".format(default_map_dir)) + print("default map dir: {}".format(self.default_map_dir)) print("calibrated map dir: {}".format(self.calibrated_map_dir)) # read csv - self.cr = CSVReader(log_file, csv_type="file") + # If log file doesn't exsist, return empty data + if not Path(self.log_file).exists(): + response.graph_image = [] + print("svg data is empty") + + response.accel_map = "" + print("accel map is empty") + + response.brake_map = "" + print("brake map is empty") + + return response + + self.cr = CSVReader(self.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 + self.min_vel_thr, + self.max_steer_thr, + self.max_pitch_thr, + self.max_pedal_vel_thr, + self.max_jerk_thr, ) # get statistics array @@ -99,9 +118,9 @@ def __init__(self, args): acc_data, color_data, CF.VEL_LIST / 3.6, - vel_diff_thr, + self.vel_diff_thr, CF.PEDAL_LIST, - pedal_diff_thr, + self.pedal_diff_thr, ) count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) @@ -109,9 +128,11 @@ def __init__(self, args): 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) + 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(default_map_dir)) + self.get_logger().warning( + "No default map file was found in {}".format(self.default_map_dir) + ) 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: @@ -136,28 +157,41 @@ def __init__(self, args): default_acc_list, calibrated_pedal_list, calibrated_acc_list, - scatter_only, + self.scatter_only, ) plt.savefig("plot.svg") print("svg saved") - def get_data_callback(self, request, response): - + # pack response data text = Path("plot.svg").read_text() - byte = text.encode() - for b in byte: - response.graph_image.append(b) - print("svg data packed") - - 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 - print("accel map packed") - - 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 - print("brake map packed") + if text == "": + response.graph_image = [] + print("svg data is empty") + else: + byte = text.encode() + for b in byte: + response.graph_image.append(b) + print("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 + print("accel map is packed") + else: + response.accel_map = "" + print("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 + print("brake map is packed") + else: + response.brake_map = "" + print("brake map is empty") return response From 004ef8aba28df1eb178eecc20691816508edcfbb Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 20 May 2022 18:18:27 +0900 Subject: [PATCH 16/33] modify plot_csv_server for adding it to launcher Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator.launch.xml | 2 +- .../scripts/plot_csv_server_node.py | 155 +++++++----------- 2 files changed, 60 insertions(+), 97 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml index 57136876e439c..a3b207e4f04fb 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml @@ -25,7 +25,7 @@ - + diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py index bde19acc719e9..a6847a98530a1 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py @@ -15,7 +15,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import argparse import math from pathlib import Path @@ -29,56 +28,60 @@ 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, args): + def __init__(self): super().__init__("plot_server") self.srv = self.create_service( CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback ) - self.default_map_dir = args.default_map_dir - self.calibrated_map_dir = args.calibrated_map_dir - self.scatter_only = args.scatter_only - self.log_file = args.log_file - self.min_vel_thr = args.min_vel_thr - self.vel_diff_thr = args.vel_diff_thr - self.pedal_diff_thr = args.pedal_diff_thr - self.max_steer_thr = args.max_steer_thr - self.max_pitch_thr = args.max_pitch_thr - self.max_jerk_thr = args.max_jerk_thr - self.max_pedal_vel_thr = args.max_pedal_vel_thr - - def get_data_callback(self, request, response): - - if self.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/" - ) - 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") + default_map_path = get_package_share_directory("raw_vehicle_cmd_converter") + + self.default_map_dir = default_map_path + "/data/default/" + self.calibrated_map_dir = package_path + "/config/" + self.log_file = package_path + "/config/log.csv" + + config_file = package_path + "/config/accel_brake_map_calibrator.param.yaml" + if Path(config_file).exists(): + print("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: + print("config file is not found") + 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 - if self.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/" - ) - self.calibrated_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") - .get_parameter_value() - .string_value - ) + self.max_pedal_vel_thr = 0.7 + # debug print("default map dir: {}".format(self.default_map_dir)) print("calibrated map dir: {}".format(self.calibrated_map_dir)) + print("log file :", self.log_file) + print("min_vel_thr : ", self.min_vel_thr) + print("vel_diff_thr : ", self.vel_diff_thr) + print("pedal_diff_thr : ", self.pedal_diff_thr) + print("max_steer_thr : ", self.max_steer_thr) + print("max_pitch_thr : ", self.max_pitch_thr) + print("max_jerk_thr : ", self.max_jerk_thr) + print("max_pedal_vel_thr : ", self.max_pedal_vel_thr) + def get_data_callback(self, request, response): # read csv # If log file doesn't exsist, return empty data if not Path(self.log_file).exists(): @@ -157,7 +160,6 @@ def get_data_callback(self, request, response): default_acc_list, calibrated_pedal_list, calibrated_acc_list, - self.scatter_only, ) plt.savefig("plot.svg") print("svg saved") @@ -211,30 +213,28 @@ def view_pedal_accel_graph( 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", - ) + # 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))] @@ -318,48 +318,11 @@ def load_map(self, csv_dir): def main(args=None): rclpy.init(args=None) - node = DrawGraph(args) + node = DrawGraph() 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("-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) + main() From e096cb9b3e2066cdd565f8fd243286e07477245c Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 20 May 2022 18:57:16 +0900 Subject: [PATCH 17/33] pull the latest universe main From f6d95605d121e06d40e530b05df297d348927061 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Thu, 26 May 2022 11:12:48 +0900 Subject: [PATCH 18/33] modify test script Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator/scripts/plot_csv_client_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py index 4f74a8c628cc5..f584c24e6e981 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py @@ -36,7 +36,8 @@ class CalibrationDataRelay(Node): def __init__(self, args): super().__init__("plot_server") - self.cli = self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") + # self.cli = self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") + 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...") From 3196d1f84e89a38166b049b46f22b8e91bd17557 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 26 May 2022 02:14:21 +0000 Subject: [PATCH 19/33] ci(pre-commit): autofix --- .../scripts/plot_csv_client_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py index f584c24e6e981..c860b8f8587b8 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py @@ -37,7 +37,9 @@ class CalibrationDataRelay(Node): def __init__(self, args): super().__init__("plot_server") # self.cli = self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") - self.cli = self.create_client(CalibData, "/api/external/get/accel_brake_map_calibrator/data") + 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...") From 7fbea7da3d69394d4abe7914a36df845110bc26d Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Mon, 30 May 2022 16:27:22 +0900 Subject: [PATCH 20/33] rename server file, add test script for calibration status change Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator/CMakeLists.txt | 5 +- .../accel_brake_map_calibrator.launch.xml | 2 +- ..._node.py => new_accel_brake_map_server.py} | 0 .../src/accel_brake_map_calibrator_node.cpp | 2 +- .../{scripts => test}/plot_csv_client_node.py | 10 +-- .../test/sim_actuation_status_publisher.py | 61 +++++++++++++++++++ 6 files changed, 67 insertions(+), 13 deletions(-) rename vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/{plot_csv_server_node.py => new_accel_brake_map_server.py} (100%) rename vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/{scripts => test}/plot_csv_client_node.py (94%) create mode 100755 vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt index dbdcba92c42f2..68180c2078e75 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt @@ -21,8 +21,9 @@ install( scripts/csv_reader.py scripts/log_analyzer.py scripts/view_plot.py - scripts/plot_csv_client_node.py - scripts/plot_csv_server_node.py + scripts/new_accel_brake_map_server.py + test/plot_csv_client_node.py + test/sim_actuation_status_publisher.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml index a3b207e4f04fb..ca0f8c4ccaf9d 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml @@ -25,7 +25,7 @@ - + diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py similarity index 100% rename from vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_server_node.py rename to vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 67672e46f27f8..cd65abc930430 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -1092,7 +1092,7 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { // lack of data - stat.summary(level, msg); + stat.summary(level, msg); calibration_status_pub_->publish(accel_brake_map_status); return; } diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py similarity index 94% rename from vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py rename to vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py index c860b8f8587b8..fa0e5b82310e7 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -16,18 +16,10 @@ # limitations under the License. import argparse - -# from http import client -# import math import os 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 from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData @@ -35,7 +27,7 @@ class CalibrationDataRelay(Node): def __init__(self, args): - super().__init__("plot_server") + super().__init__("plot_client") # self.cli = self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") self.cli = self.create_client( CalibData, "/api/external/get/accel_brake_map_calibrator/data" diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py new file mode 100755 index 0000000000000..b1bac7444949e --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -0,0 +1,61 @@ +#! /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 os +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + +from tier4_vehicle_msgs.msg import ActuationStatusStamped +from autoware_auto_vehicle_msgs.msg import VelocityReport + +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() \ No newline at end of file From 28a87c691e862728ca8eb93fd42cc23768768cae Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 30 May 2022 07:28:49 +0000 Subject: [PATCH 21/33] ci(pre-commit): autofix --- .../accel_brake_map_calibrator/CMakeLists.txt | 2 +- .../src/accel_brake_map_calibrator_node.cpp | 2 +- .../test/plot_csv_client_node.py | 1 - .../test/sim_actuation_status_publisher.py | 15 +++++++++------ 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt index 68180c2078e75..37d42e5a57a2a 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt @@ -23,7 +23,7 @@ install( scripts/view_plot.py scripts/new_accel_brake_map_server.py test/plot_csv_client_node.py - test/sim_actuation_status_publisher.py + test/sim_actuation_status_publisher.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index cd65abc930430..67672e46f27f8 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -1092,7 +1092,7 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { // lack of data - stat.summary(level, msg); + stat.summary(level, msg); calibration_status_pub_->publish(accel_brake_map_status); return; } diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py index fa0e5b82310e7..ceb9053117694 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -19,7 +19,6 @@ 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 diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py index b1bac7444949e..e33cac1471f7a 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -16,13 +16,16 @@ # limitations under the License. import os + +from autoware_auto_vehicle_msgs.msg import VelocityReport import rclpy from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +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 -from autoware_auto_vehicle_msgs.msg import VelocityReport + class ActuationStatusPublisher(Node): def __init__(self): @@ -33,7 +36,6 @@ def __init__(self): qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.durability = QoSDurabilityPolicy.VOLATILE - self.pub = self.create_publisher( ActuationStatusStamped, "/vehicle/status/actuation_status", qos_profile ) @@ -41,7 +43,7 @@ def __init__(self): VelocityReport, "/vehicle/status/velocity_status", self.callback, qos_profile ) - def callback(self,msg): + def callback(self, msg): data = ActuationStatusStamped() data.header = msg.header data.status.accel_status = msg.longitudinal_velocity * 0.1 @@ -57,5 +59,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + if __name__ == "__main__": - main() \ No newline at end of file + main() From bc5d09e0b8833ae9c83aec8d285b7daf33917c84 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Mon, 30 May 2022 16:47:14 +0900 Subject: [PATCH 22/33] change node name Signed-off-by: TakumiKozaka-T4 --- .../launch/accel_brake_map_calibrator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml index ca0f8c4ccaf9d..c3379ee67f7ca 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml @@ -25,7 +25,7 @@ - + From aa0500a94c8589a121caaea80b723cfe92f3a659 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 3 Jun 2022 20:04:00 +0900 Subject: [PATCH 23/33] Fix status when default accel/brake map is not found Signed-off-by: TakumiKozaka-T4 --- .../accel_brake_map_calibrator_node.hpp | 1 + .../src/accel_brake_map_calibrator_node.cpp | 32 +++++++++++++------ 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 6b8d9d6bd35c5..0182b6858e0e2 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -115,6 +115,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node // Diagnostic Updater std::shared_ptr updater_ptr_; + bool is_default_map_ = true; int get_pitch_method_; int update_method_; diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 67672e46f27f8..767ef6c222228 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -81,6 +81,14 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod // 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"); @@ -96,6 +104,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod if ( !accel_map_.readAccelMapFromCSV(csv_path_accel_map) || !new_accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { + is_default_map_ = false; RCLCPP_ERROR_STREAM( rclcpp::get_logger("accel_brake_map_calibrator"), "Cannot read accelmap. csv path = " << csv_path_accel_map.c_str() << ". stop calculation."); @@ -104,6 +113,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod if ( !brake_map_.readBrakeMapFromCSV(csv_path_brake_map) || !new_brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { + is_default_map_ = false; RCLCPP_ERROR_STREAM( rclcpp::get_logger("accel_brake_map_calibrator"), "Cannot read brakemap. csv path = " << csv_path_brake_map.c_str() << ". stop calculation."); @@ -141,10 +151,6 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod 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()); - // QoS setup - static constexpr std::size_t queue_size = 1; - rclcpp::QoS durable_qos(queue_size); - // publisher update_suggest_pub_ = create_publisher("~/output/update_suggest", durable_qos); @@ -174,8 +180,6 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod "/accel_brake_map_calibrator/output/updated_map_error", durable_qos); map_error_ratio_pub_ = create_publisher( "/accel_brake_map_calibrator/output/map_error_ratio", durable_qos); - calibration_status_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/calibration_status", durable_qos); // subscriber using std::placeholders::_1; @@ -1082,13 +1086,21 @@ nav_msgs::msg::OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat) { using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; - int8_t level = DiagStatus::OK; - std::string msg = "OK"; - using CalibrationStatus = tier4_external_api_msgs::msg::CalibrationStatus; CalibrationStatus accel_brake_map_status; + int8_t level; + std::string msg; + accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; - accel_brake_map_status.status = CalibrationStatus::NORMAL; + if(is_default_map_ == true){ + accel_brake_map_status.status = CalibrationStatus::NORMAL; + level = DiagStatus::OK; + msg = "OK"; + }else{ + accel_brake_map_status.status = CalibrationStatus::ERROR; + 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 From cb8fbaf20f6fc26e486c34bc8712c58b4f37f886 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Fri, 3 Jun 2022 20:05:10 +0900 Subject: [PATCH 24/33] run pre-commit Signed-off-by: TakumiKozaka-T4 --- .../src/accel_brake_map_calibrator_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 767ef6c222228..2031b022ed729 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -1092,11 +1092,11 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS std::string msg; accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; - if(is_default_map_ == true){ + if (is_default_map_ == true) { accel_brake_map_status.status = CalibrationStatus::NORMAL; level = DiagStatus::OK; msg = "OK"; - }else{ + } else { accel_brake_map_status.status = CalibrationStatus::ERROR; level = DiagStatus::ERROR; msg = "Default map is not found in " + csv_default_map_dir_; From 8f765f32d10a7bb0f996f6d2c1f439ab6f8a0d15 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Mon, 6 Jun 2022 15:01:20 +0900 Subject: [PATCH 25/33] return blank data if default map is not found. Signed-off-by: TakumiKozaka-T4 --- .../scripts/new_accel_brake_map_server.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index a6847a98530a1..898f28f2fdbe0 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -136,6 +136,18 @@ def get_data_callback(self, request, response): self.get_logger().warning( "No default map file was found in {}".format(self.default_map_dir) ) + + response.graph_image = [] + print("svg data is empty") + + response.accel_map = "" + print("accel map is empty") + + response.brake_map = "" + print("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: From d59d2b6239754b7c67c2aefca506faae40963494 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 6 Jun 2022 06:02:38 +0000 Subject: [PATCH 26/33] ci(pre-commit): autofix --- .../scripts/new_accel_brake_map_server.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index 898f28f2fdbe0..211029d785a2f 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -136,7 +136,7 @@ def get_data_callback(self, request, response): self.get_logger().warning( "No default map file was found in {}".format(self.default_map_dir) ) - + response.graph_image = [] print("svg data is empty") @@ -148,7 +148,6 @@ def get_data_callback(self, request, response): 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( From 4fd525fff03b97d89d8a804a80c53f1738fa620c Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Mon, 6 Jun 2022 20:10:11 +0900 Subject: [PATCH 27/33] change calibration status definition Signed-off-by: TakumiKozaka-T4 --- .../src/accel_brake_map_calibrator_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 2031b022ed729..6d907f4430281 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -1097,7 +1097,7 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS level = DiagStatus::OK; msg = "OK"; } else { - accel_brake_map_status.status = CalibrationStatus::ERROR; + accel_brake_map_status.status = CalibrationStatus::UNAVAILABLE; level = DiagStatus::ERROR; msg = "Default map is not found in " + csv_default_map_dir_; } @@ -1115,7 +1115,7 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS // Suggest to update accel brake map level = DiagStatus::WARN; msg = "Accel/brake map Calibration is required."; - accel_brake_map_status.status = CalibrationStatus::WARNING; + accel_brake_map_status.status = CalibrationStatus::CALIBRATION_REQUIRED; } stat.summary(level, msg); From 694a6fc1541e64992e7427dee7bb6b1e397edca1 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Wed, 8 Jun 2022 16:35:04 +0900 Subject: [PATCH 28/33] Change print to get_logger(). Delete unnecessary comment. Signed-off-by: TakumiKozaka-T4 --- .../scripts/new_accel_brake_map_server.py | 50 +++++++++---------- .../test/plot_csv_client_node.py | 1 - 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index 211029d785a2f..92948b88c96b4 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -49,7 +49,7 @@ def __init__(self): config_file = package_path + "/config/accel_brake_map_calibrator.param.yaml" if Path(config_file).exists(): - print("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"] @@ -59,7 +59,7 @@ def __init__(self): self.max_pitch_thr = data["/**"]["ros__parameters"]["max_pitch_threshold"] self.max_jerk_thr = data["/**"]["ros__parameters"]["max_jerk_threshold"] else: - print("config file is not found") + 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 @@ -70,29 +70,29 @@ def __init__(self): self.max_pedal_vel_thr = 0.7 # debug - print("default map dir: {}".format(self.default_map_dir)) - print("calibrated map dir: {}".format(self.calibrated_map_dir)) - print("log file :", self.log_file) - print("min_vel_thr : ", self.min_vel_thr) - print("vel_diff_thr : ", self.vel_diff_thr) - print("pedal_diff_thr : ", self.pedal_diff_thr) - print("max_steer_thr : ", self.max_steer_thr) - print("max_pitch_thr : ", self.max_pitch_thr) - print("max_jerk_thr : ", self.max_jerk_thr) - print("max_pedal_vel_thr : ", self.max_pedal_vel_thr) + 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("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 exsist, return empty data if not Path(self.log_file).exists(): response.graph_image = [] - print("svg data is empty") + self.get_logger().info("svg data is empty") response.accel_map = "" - print("accel map is empty") + self.get_logger().info("accel map is empty") response.brake_map = "" - print("brake map is empty") + self.get_logger().info("brake map is empty") return response @@ -138,13 +138,13 @@ def get_data_callback(self, request, response): ) response.graph_image = [] - print("svg data is empty") + self.get_logger().info("svg data is empty") response.accel_map = "" - print("accel map is empty") + self.get_logger().info("accel map is empty") response.brake_map = "" - print("brake map is empty") + self.get_logger().info("brake map is empty") return response @@ -173,38 +173,38 @@ def get_data_callback(self, request, response): calibrated_acc_list, ) plt.savefig("plot.svg") - print("svg saved") + self.get_logger().info("svg saved") # pack response data text = Path("plot.svg").read_text() if text == "": response.graph_image = [] - print("svg data is empty") + self.get_logger().info("svg data is empty") else: byte = text.encode() for b in byte: response.graph_image.append(b) - print("svg data is packed") + 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 - print("accel map is packed") + self.get_logger().info("accel map is packed") else: response.accel_map = "" - print("accel map is empty") + 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 - print("brake map is packed") + self.get_logger().info("brake map is packed") else: response.brake_map = "" - print("brake map is empty") + self.get_logger().info("brake map is empty") return response diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py index ceb9053117694..3feda1896342c 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -27,7 +27,6 @@ class CalibrationDataRelay(Node): def __init__(self, args): super().__init__("plot_client") - # self.cli = self.create_client(CalibData, "/accel_brake_map_calibrator/get_data_service") self.cli = self.create_client( CalibData, "/api/external/get/accel_brake_map_calibrator/data" ) From 7ff3f344df239f793a9646587e0ba2e3295d08a3 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Wed, 8 Jun 2022 17:11:12 +0900 Subject: [PATCH 29/33] pull the latest foundation main Signed-off-by: TakumiKozaka-T4 From 86cb2da5bd522dd4c50b5ab92774a08cacb5d432 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Mon, 13 Jun 2022 16:03:35 +0900 Subject: [PATCH 30/33] modified as comment in PR Signed-off-by: TakumiKozaka-T4 --- .../scripts/new_accel_brake_map_server.py | 3 +-- .../accel_brake_map_calibrator/test/plot_csv_client_node.py | 3 +-- .../test/sim_actuation_status_publisher.py | 4 +--- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index 92948b88c96b4..450f59d9b4a99 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -1,7 +1,6 @@ #! /usr/bin/env python3 -# -*- coding: utf-8 -*- -# Copyright 2021 Tier IV, Inc. All rights reserved. +# 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. diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py index 3feda1896342c..3f4131acbae42 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -1,7 +1,6 @@ #! /usr/bin/env python3 -# -*- coding: utf-8 -*- -# Copyright 2021 Tier IV, Inc. All rights reserved. +# 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. diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py index e33cac1471f7a..75867630ada0f 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -1,7 +1,6 @@ #! /usr/bin/env python3 -# -*- coding: utf-8 -*- -# Copyright 2021 Tier IV, Inc. All rights reserved. +# 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. @@ -15,7 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os from autoware_auto_vehicle_msgs.msg import VelocityReport import rclpy From 240355575a4bb957e05e458f0939e79d31efdb2c Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Tue, 14 Jun 2022 09:20:11 +0900 Subject: [PATCH 31/33] Delete unnecessary sentences. Signed-off-by: TakumiKozaka-T4 --- .../scripts/new_accel_brake_map_server.py | 2 +- .../accel_brake_map_calibrator/test/plot_csv_client_node.py | 2 +- .../test/sim_actuation_status_publisher.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index 450f59d9b4a99..e8a61150b4bf9 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. All rights reserved. +# 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. diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py index 3f4131acbae42..f35502f1cb030 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. All rights reserved. +# 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. diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py index 75867630ada0f..7cbe9a34798f3 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. All rights reserved. +# 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. From eb514fdc6e0808ce4cb66dffa54b72db64086bff Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jun 2022 00:21:37 +0000 Subject: [PATCH 32/33] ci(pre-commit): autofix --- .../scripts/new_accel_brake_map_server.py | 2 +- .../accel_brake_map_calibrator/test/plot_csv_client_node.py | 2 +- .../test/sim_actuation_status_publisher.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index e8a61150b4bf9..ca65b332093a7 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py index f35502f1cb030..1bbb5820bd24a 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py index 7cbe9a34798f3..9e57813bd4010 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2022 Tier IV, Inc. +# 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. From 40b59dfca5e1dee142cd922f04f10d7299712146 Mon Sep 17 00:00:00 2001 From: TakumiKozaka-T4 Date: Wed, 15 Jun 2022 14:46:50 +0900 Subject: [PATCH 33/33] pull the latest foundation/main Signed-off-by: TakumiKozaka-T4