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 97d35f282042c..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 @@ -21,6 +21,9 @@ install( scripts/csv_reader.py scripts/log_analyzer.py scripts/view_plot.py + scripts/new_accel_brake_map_server.py + test/plot_csv_client_node.py + test/sim_actuation_status_publisher.py DESTINATION lib/${PROJECT_NAME} ) 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 05ce550d3095c..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 @@ -42,6 +42,9 @@ #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_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" @@ -84,6 +87,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_; @@ -110,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/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..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 @@ -24,6 +24,8 @@ + + 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 472d492c52908..4ba403bb14b9f 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 @@ -22,6 +22,7 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs + tier4_external_api_msgs tier4_vehicle_msgs visualization_msgs 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 new file mode 100755 index 0000000000000..ca65b332093a7 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -0,0 +1,338 @@ +#! /usr/bin/env python3 + +# Copyright 2022 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +from calc_utils import CalcUtils +import config as CF +from csv_reader import CSVReader +import matplotlib.pyplot as plt +import numpy as np +from plotter import Plotter +import rclpy +from rclpy.node import Node +from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData +import yaml + + +class DrawGraph(Node): + calibrated_map_dir = "" + + def __init__(self): + super().__init__("plot_server") + self.srv = self.create_service( + CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback + ) + + 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(): + self.get_logger().info("config file exists") + with open(config_file) as yml: + data = yaml.safe_load(yml) + self.min_vel_thr = data["/**"]["ros__parameters"]["velocity_min_threshold"] + self.vel_diff_thr = data["/**"]["ros__parameters"]["velocity_diff_threshold"] + self.pedal_diff_thr = data["/**"]["ros__parameters"]["pedal_diff_threshold"] + self.max_steer_thr = data["/**"]["ros__parameters"]["max_steer_threshold"] + self.max_pitch_thr = data["/**"]["ros__parameters"]["max_pitch_threshold"] + self.max_jerk_thr = data["/**"]["ros__parameters"]["max_jerk_threshold"] + else: + self.get_logger().warning("config file is not found in {}".format(config_file)) + self.min_vel_thr = 0.1 + self.vel_diff_thr = 0.556 + self.pedal_diff_thr = 0.03 + self.max_steer_thr = 0.2 + self.max_pitch_thr = 0.02 + self.max_jerk_thr = 0.7 + + self.max_pedal_vel_thr = 0.7 + + # debug + self.get_logger().info("default map dir: {}".format(self.default_map_dir)) + self.get_logger().info("calibrated map dir: {}".format(self.calibrated_map_dir)) + self.get_logger().info("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 = [] + self.get_logger().info("svg data is empty") + + response.accel_map = "" + self.get_logger().info("accel map is empty") + + response.brake_map = "" + self.get_logger().info("brake map is empty") + + return response + + self.cr = CSVReader(self.log_file, csv_type="file") + + # remove unused_data + self.csv_data = self.cr.removeUnusedData( + self.min_vel_thr, + self.max_steer_thr, + self.max_pitch_thr, + self.max_pedal_vel_thr, + self.max_jerk_thr, + ) + + # get statistics array + vel_data = self.cr.getVelData() + pedal_data = self.cr.getPedalData() + acc_data = self.cr.getAccData() + + # get color factor (pitch) array for plotting + color_data = self.cr.getPitchData() + + data, full_data = CalcUtils.create_2d_map( + vel_data, + pedal_data, + acc_data, + color_data, + CF.VEL_LIST / 3.6, + self.vel_diff_thr, + CF.PEDAL_LIST, + self.pedal_diff_thr, + ) + + count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) + velocity_map_list = [] + for i in range(len(CF.VEL_LIST)): + velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, i)) + + default_pedal_list, default_acc_list = self.load_map(self.default_map_dir) + if len(default_pedal_list) == 0 or len(default_acc_list) == 0: + self.get_logger().warning( + "No default map file was found in {}".format(self.default_map_dir) + ) + + response.graph_image = [] + self.get_logger().info("svg data is empty") + + response.accel_map = "" + self.get_logger().info("accel map is empty") + + response.brake_map = "" + self.get_logger().info("brake map is empty") + + return response + + calibrated_pedal_list, calibrated_acc_list = self.load_map(self.calibrated_map_dir) + if len(calibrated_pedal_list) == 0 or len(calibrated_acc_list) == 0: + self.get_logger().warning( + "No calibrated map file was found in {}".format(self.calibrated_map_dir) + ) + + # visualize point from data + plot_width = 3 + plot_height = int(math.ceil(len(CF.VEL_LIST) / float(plot_width))) + plotter = Plotter(plot_height, plot_width) + for i in range(len(CF.VEL_LIST)): + self.view_pedal_accel_graph( + plotter, + i, + velocity_map_list, + i, + count_map, + average_map, + stddev_map, + default_pedal_list, + default_acc_list, + calibrated_pedal_list, + calibrated_acc_list, + ) + plt.savefig("plot.svg") + self.get_logger().info("svg saved") + + # pack response data + text = Path("plot.svg").read_text() + if text == "": + response.graph_image = [] + self.get_logger().info("svg data is empty") + else: + byte = text.encode() + for b in byte: + response.graph_image.append(b) + self.get_logger().info("svg data is packed") + + accel_map_name = Path(self.calibrated_map_dir + "accel_map.csv") + if accel_map_name.exists(): + with open(self.calibrated_map_dir + "accel_map.csv", "r") as calibrated_accel_map: + for accel_data in calibrated_accel_map: + response.accel_map += accel_data + self.get_logger().info("accel map is packed") + else: + response.accel_map = "" + self.get_logger().info("accel map is empty") + + brake_map_name = Path(self.calibrated_map_dir + "brake_map.csv") + if brake_map_name.exists(): + with open(self.calibrated_map_dir + "brake_map.csv", "r") as calibrated_brake_map: + for brake_data in calibrated_brake_map: + response.brake_map += brake_data + self.get_logger().info("brake map is packed") + else: + response.brake_map = "" + self.get_logger().info("brake map is empty") + + return response + + def plotter_function(self): + return self.plotter + + def view_pedal_accel_graph( + self, + plotter, + subplot_num, + velocity_map_list, + vel_list_idx, + count_map, + average_map, + stddev_map, + default_pedal_list, + default_acc_list, + calibrated_pedal_list, + calibrated_acc_list, + ): + + fig = plotter.subplot_more(subplot_num) + + # calibrated map + if len(calibrated_pedal_list) != 0 and len(calibrated_acc_list) != 0: + plotter.plot( + calibrated_pedal_list[vel_list_idx], + calibrated_acc_list[vel_list_idx], + color="blue", + label="calibrated", + ) + + # default map + if len(default_pedal_list) != 0 and len(default_acc_list) != 0: + plotter.plot( + default_pedal_list[vel_list_idx], + default_acc_list[vel_list_idx], + color="orange", + label="default", + linestyle="dashed", + ) + + # plot all data + pedal_list = [0 for i in range(len(CF.PEDAL_LIST))] + if velocity_map_list[vel_list_idx] is not None: + plotter.scatter_color( + velocity_map_list[vel_list_idx][:, 1], + velocity_map_list[vel_list_idx][:, 2], + color=velocity_map_list[vel_list_idx][:, 3], + label="all", + ) + + for pedal in velocity_map_list[vel_list_idx][:, 1]: + min_pedal = 10 + for pedal_idx, ref_pedal in enumerate(CF.PEDAL_LIST): + if min_pedal > abs(pedal - ref_pedal): + min_pedal = abs(pedal - ref_pedal) + min_pedal_idx = pedal_idx + pedal_list[min_pedal_idx] += 1 + + # plot average data + plotter.scatter(CF.PEDAL_LIST, average_map[:, vel_list_idx], "red", label="average") + + # add label of standard deviation + plotter.scatter([], [], "black", label="std dev") + + # plot average text + for i in range(len(CF.PEDAL_LIST)): + if count_map[i, vel_list_idx] == 0: + continue + x = CF.PEDAL_LIST[i] + y = average_map[i, vel_list_idx] + y2 = stddev_map[i, vel_list_idx] + # plot average + plotter.plot_text(x, y + 1, y, color="red") + + # plot standard deviation + plotter.plot_text(x, y - 1, y2, color="black") + + # plot the number of all data + plotter.plot_text( + x, y - 2, "{}\npts".format(pedal_list[i]), num_data_type="str", color="green" + ) + + pedal_lim = [CF.PEDAL_LIST[0] - 0.05, CF.PEDAL_LIST[-1] + 0.05] + accel_lim = [-5.0, 5.0] + + plotter.set_lim(fig, pedal_lim, accel_lim) + plotter.add_label( + str(CF.VEL_LIST[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" + ) + + def load_map(self, csv_dir): + try: + accel_pedal_list = [] + accel_acc_list = [] + with open(csv_dir + "accel_map.csv") as f: + for l_idx, l in enumerate(f.readlines()): + w = l.split(",") + w[-1] = w[-1][:-1] + if l_idx != 0: + accel_pedal_list.append([float(w[0]) for e in w[1:]]) + accel_acc_list.append([float(e) for e in w[1:]]) + + brake_pedal_list = [] + brake_acc_list = [] + with open(csv_dir + "brake_map.csv") as f: + for l_idx, l in enumerate(f.readlines()): + w = l.split(",") + w[-1] = w[-1][:-1] + if l_idx != 0: + brake_pedal_list.append([-float(w[0]) for e in w[1:]]) + brake_acc_list.append([float(e) for e in w[1:]]) + + return np.hstack( + [np.fliplr(np.array(accel_pedal_list).T), np.array(brake_pedal_list).T.tolist()] + ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) + except OSError as e: + print(e) + return [], [] + + +def main(args=None): + rclpy.init(args=None) + node = DrawGraph() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/accel_brake_map_calibrator/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 2d7ddb588c424..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 @@ -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); @@ -1080,13 +1086,26 @@ 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; + if (is_default_map_ == true) { + accel_brake_map_status.status = CalibrationStatus::NORMAL; + level = DiagStatus::OK; + msg = "OK"; + } else { + accel_brake_map_status.status = CalibrationStatus::UNAVAILABLE; + level = DiagStatus::ERROR; + msg = "Default map is not found in " + csv_default_map_dir_; + } if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { // lack of data stat.summary(level, msg); - + calibration_status_pub_->publish(accel_brake_map_status); return; } @@ -1096,9 +1115,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::CALIBRATION_REQUIRED; } stat.summary(level, msg); + calibration_status_pub_->publish(accel_brake_map_status); } // function for debug 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 new file mode 100755 index 0000000000000..1bbb5820bd24a --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/plot_csv_client_node.py @@ -0,0 +1,122 @@ +#! /usr/bin/env python3 + +# Copyright 2022 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import os + +from ament_index_python.packages import get_package_share_directory +import rclpy +from rclpy.node import Node +from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData + + +class CalibrationDataRelay(Node): + def __init__(self, args): + super().__init__("plot_client") + self.cli = self.create_client( + CalibData, "/api/external/get/accel_brake_map_calibrator/data" + ) + + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available, waiting again...") + + self.request = CalibData.Request() + + def send_request(self): + self.future = self.cli.call_async(self.request) + + +def main(args=None): + rclpy.init(args=None) + client = CalibrationDataRelay(args) + client.send_request() + while rclpy.ok(): + rclpy.spin_once(client) + if client.future.done(): + try: + response = client.future.result() + save_dir = "./test_data_save" + if not os.path.exists(save_dir): + os.makedirs(save_dir) + svg_name = save_dir + "/graph.svg" + + f_svg = open(svg_name, "w") + svg_byte = bytes(response.graph_image) + text = svg_byte.decode() + f_svg.write(text) + + print("svg done") + + acc_map_name = save_dir + "/accel_map.csv" + f_acc = open(acc_map_name, "w") + f_acc.write(response.accel_map) + + print("accel map done") + + brk_map_name = save_dir + "/brake_map.csv" + f_brk = open(brk_map_name, "w") + f_brk.write(response.brake_map) + + print("brake map done") + + except Exception as e: + client.get_logger().info("Service call failed %r" % (e,)) + + break + + client.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + package_path = get_package_share_directory("accel_brake_map_calibrator") + parser = argparse.ArgumentParser() + parser.add_argument( + "-d", "--default-map-dir", default=None, type=str, help="directory of default map" + ) + parser.add_argument( + "-c", "--calibrated-map-dir", default=None, type=str, help="directory of calibrated map" + ) + parser.add_argument("-s", "--scatter-only", action="store_true", help="show only scatters") + parser.add_argument( + "-l", + "--log-file", + default=package_path + "/config/log.csv", + type=str, + help="path of log.csv", + ) + parser.add_argument( + "--min-vel-thr", default=0.1, type=float, help="valid min velocity threshold" + ) + parser.add_argument( + "--vel-diff-thr", default=0.556, type=float, help="valid velocity diff threshold" + ) + parser.add_argument( + "--pedal-diff-thr", default=0.03, type=float, help="valid pedal diff threshold" + ) + parser.add_argument( + "--max-steer-thr", default=0.2, type=float, help="valid max steer threshold" + ) + parser.add_argument( + "--max-pitch-thr", default=0.02, type=float, help="valid max pitch threshold" + ) + parser.add_argument("--max-jerk-thr", default=0.7, type=float, help="valid max jerk threshold") + parser.add_argument( + "--max-pedal-vel-thr", default=0.7, type=float, help="valid max pedal velocity threshold" + ) + + args = parser.parse_args() + main(args) diff --git a/vehicle/accel_brake_map_calibrator/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..9e57813bd4010 --- /dev/null +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -0,0 +1,62 @@ +#! /usr/bin/env python3 + +# Copyright 2022 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from autoware_auto_vehicle_msgs.msg import VelocityReport +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from tier4_vehicle_msgs.msg import ActuationStatusStamped + + +class ActuationStatusPublisher(Node): + def __init__(self): + super().__init__("actuation_status_publisher") + + qos_profile = QoSProfile(depth=1) + qos_profile.reliability = QoSReliabilityPolicy.RELIABLE + qos_profile.history = QoSHistoryPolicy.KEEP_LAST + qos_profile.durability = QoSDurabilityPolicy.VOLATILE + + self.pub = self.create_publisher( + ActuationStatusStamped, "/vehicle/status/actuation_status", qos_profile + ) + self.sub = self.create_subscription( + VelocityReport, "/vehicle/status/velocity_status", self.callback, qos_profile + ) + + def callback(self, msg): + data = ActuationStatusStamped() + data.header = msg.header + data.status.accel_status = msg.longitudinal_velocity * 0.1 + data.status.brake_status = 0.1 + data.status.steer_status = 0.1 + self.pub.publish(data) + + +def main(args=None): + rclpy.init(args=args) + node = ActuationStatusPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()