{ "cells": [ { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Defaulting to user installation because normal site-packages is not writeable\n", "Requirement already satisfied: libclang in /home/max/.local/lib/python3.10/site-packages (14.0.1)\n", "Note: you may need to restart the kernel to use updated packages.\n" ] } ], "source": [ "%pip install libclang" ] }, { "cell_type": "code", "execution_count": 2, "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } }, "outputs": [], "source": [ "import glob\n", "import json\n", "import os\n", "import pathlib\n", "import re\n", "from typing import Union, Iterator, List\n", "\n", "import clang.cindex as ci\n", "from clang.cindex import TokenKind as tk\n", "from clang.cindex import CursorKind as ck\n", "\n", "import ast_utils\n", "\n", "jj = os.path.join\n", "\n", "FS_ROOT = \"/\"\n", "ROOT_DIR = jj(FS_ROOT, \"/home/max/projects/autoware/\")\n", "\n", "def pext(t: Union[ci.Token, ci.Cursor]):\n", " e = t.extent\n", " return f\"{e.start.line}:{e.start.column}-{e.end.line}:{e.end.column}\"\n", "\n", "def pt(tokens, join_str=''):\n", " if tokens is None:\n", " return \"None\"\n", " return join_str.join(map(lambda t: t.spelling, tokens))\n", "\n", "if not os.path.exists(jj(ROOT_DIR, \"build/compile_commands.json\")):\n", " print(\"Run\")\n", " print(\" colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON\")\n", " print(\"to generate the files necessary to run this script.\")" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } }, "outputs": [], "source": [ "_arg_pattern = re.compile(r\"(?<=\\s)(?:-D\\s*\\S+|-I\\s*\\S+|-isystem\\s*\\S+|--?std\\s*\\S+)(?=\\s)\")\n", "\n", "def _extract_args(compile_command):\n", " segments = _arg_pattern.findall(compile_command)\n", " segments = [part.strip() for seg in segments for part in seg.split(\" \")]\n", " return segments\n", "\n", "with open(jj(ROOT_DIR, \"build/compile_commands.json\"), \"r\") as f:\n", " compile_commands = json.load(f)\n", "\n", "compile_commands = {cmd['file']: cmd['command'] for cmd in compile_commands}\n", "compile_commands = {k: _extract_args(v) for k, v in compile_commands.items()}" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } }, "outputs": [], "source": [] }, { "cell_type": "code", "execution_count": 4, "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "['{ transform_listener_ = std :: make_shared < tier4_autoware_utils :: TransformListener > ( this ) ; // get parameter update_hz_ = this -> declare_parameter < double > ( \"update_hz\" , 10.0 ) ; covariance_ = this -> declare_parameter < double > ( \"initial_covariance\" , 0.05 ) ; velocity_min_threshold_ = this -> declare_parameter < double > ( \"velocity_min_threshold\" , 0.1 ) ; velocity_diff_threshold_ = this -> declare_parameter < double > ( \"velocity_diff_threshold\" , 0.556 ) ; pedal_diff_threshold_ = this -> declare_parameter < double > ( \"pedal_diff_threshold\" , 0.03 ) ; max_steer_threshold_ = this -> declare_parameter < double > ( \"max_steer_threshold\" , 0.2 ) ; max_pitch_threshold_ = this -> declare_parameter < double > ( \"max_pitch_threshold\" , 0.02 ) ; max_jerk_threshold_ = this -> declare_parameter < double > ( \"max_jerk_threshold\" , 0.7 ) ; pedal_velocity_thresh_ = this -> declare_parameter < double > ( \"pedal_velocity_thresh\" , 0.15 ) ; map_update_gain_ = this -> declare_parameter < double > ( \"map_update_gain\" , 0.02 ) ; max_accel_ = this -> declare_parameter < double > ( \"max_accel\" , 5.0 ) ; min_accel_ = this -> declare_parameter < double > ( \"min_accel\" , - 5.0 ) ; pedal_to_accel_delay_ = this -> declare_parameter < double > ( \"pedal_to_accel_delay\" , 0.3 ) ; max_data_count_ = this -> declare_parameter < int > ( \"max_data_count\" , 200 ) ; pedal_accel_graph_output_ = this -> declare_parameter < bool > ( \"pedal_accel_graph_output\" , false ) ; progress_file_output_ = this -> declare_parameter < bool > ( \"progress_file_output\" , false ) ; const auto get_pitch_method_str = this -> declare_parameter < std :: string > ( \"get_pitch_method\" , std :: string ( \"tf\" ) ) ; if ( get_pitch_method_str == std :: string ( \"tf\" ) ) { get_pitch_method_ = GET_PITCH_METHOD :: TF ; } else if ( get_pitch_method_str == std :: string ( \"none\" ) ) { get_pitch_method_ = GET_PITCH_METHOD :: NONE ; } else { RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"update_method_\" << \" is wrong. (available method: tf, file, none\" ) ; return ; } update_suggest_thresh_ = this -> declare_parameter < double > ( \"update_suggest_thresh\" , 0.7 ) ; csv_calibrated_map_dir_ = this -> declare_parameter < std :: string > ( \"csv_calibrated_map_dir\" , std :: string ( \"\" ) ) ; output_accel_file_ = csv_calibrated_map_dir_ + \"/accel_map.csv\" ; output_brake_file_ = csv_calibrated_map_dir_ + \"/brake_map.csv\" ; const std :: string update_method_str = this -> declare_parameter < std :: string > ( \"update_method\" , std :: string ( \"update_offset_each_cell\" ) ) ; if ( update_method_str == std :: string ( \"update_offset_each_cell\" ) ) { update_method_ = UPDATE_METHOD :: UPDATE_OFFSET_EACH_CELL ; } else if ( update_method_str == std :: string ( \"update_offset_total\" ) ) { update_method_ = UPDATE_METHOD :: UPDATE_OFFSET_TOTAL ; } else { RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"update_method\" << \" is wrong. (available method: update_offset_each_cell, update_offset_total\" ) ; return ; } // initializer // QoS setup static constexpr std :: size_t queue_size = 1 ; rclcpp :: QoS durable_qos ( queue_size ) ; // Publisher for checkUpdateSuggest calibration_status_pub_ = create_publisher < tier4_external_api_msgs :: msg :: CalibrationStatus > ( \"/accel_brake_map_calibrator/output/calibration_status\" , durable_qos ) ; /* Diagnostic Updater */ updater_ptr_ = std :: make_shared < diagnostic_updater :: Updater > ( this , 1.0 / update_hz_ ) ; updater_ptr_ -> setHardwareID ( \"accel_brake_map_calibrator\" ) ; updater_ptr_ -> add ( \"accel_brake_map_calibrator\" , this , & AccelBrakeMapCalibrator :: checkUpdateSuggest ) ; { csv_default_map_dir_ = this -> declare_parameter < std :: string > ( \"csv_default_map_dir\" , std :: string ( \"\" ) ) ; std :: string csv_path_accel_map = csv_default_map_dir_ + \"/accel_map.csv\" ; std :: string csv_path_brake_map = csv_default_map_dir_ + \"/brake_map.csv\" ; if ( ! accel_map_ . readAccelMapFromCSV ( csv_path_accel_map ) || ! new_accel_map_ . readAccelMapFromCSV ( csv_path_accel_map ) ) { is_default_map_ = false ; RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"Cannot read accelmap. csv path = \" << csv_path_accel_map . c_str ( ) << \". stop calculation.\" ) ; return ; } if ( ! brake_map_ . readBrakeMapFromCSV ( csv_path_brake_map ) || ! new_brake_map_ . readBrakeMapFromCSV ( csv_path_brake_map ) ) { is_default_map_ = false ; RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"Cannot read brakemap. csv path = \" << csv_path_brake_map . c_str ( ) << \". stop calculation.\" ) ; return ; } } std :: string output_log_file = this -> declare_parameter < std :: string > ( \"output_log_file\" , std :: string ( \"\" ) ) ; output_log_ . open ( output_log_file ) ; addIndexToCSV ( & output_log_ ) ; debug_values_ . data . resize ( num_debug_values_ ) ; // input map info accel_map_value_ = accel_map_ . getAccelMap ( ) ; brake_map_value_ = brake_map_ . getBrakeMap ( ) ; accel_vel_index_ = accel_map_ . getVelIdx ( ) ; brake_vel_index_ = brake_map_ . getVelIdx ( ) ; accel_pedal_index_ = accel_map_ . getThrottleIdx ( ) ; brake_pedal_index_ = brake_map_ . getBrakeIdx ( ) ; update_accel_map_value_ . resize ( ( accel_map_value_ . size ( ) ) ) ; update_brake_map_value_ . resize ( ( brake_map_value_ . size ( ) ) ) ; for ( auto & m : update_accel_map_value_ ) { m . resize ( accel_map_value_ . at ( 0 ) . size ( ) ) ; } for ( auto & m : update_brake_map_value_ ) { m . resize ( brake_map_value_ . at ( 0 ) . size ( ) ) ; } map_value_data_ . resize ( accel_map_value_ . size ( ) + brake_map_value_ . size ( ) - 1 ) ; for ( auto & m : map_value_data_ ) { m . resize ( accel_map_value_ . at ( 0 ) . size ( ) ) ; } std :: copy ( accel_map_value_ . begin ( ) , accel_map_value_ . end ( ) , update_accel_map_value_ . begin ( ) ) ; std :: copy ( brake_map_value_ . begin ( ) , brake_map_value_ . end ( ) , update_brake_map_value_ . begin ( ) ) ; // publisher update_suggest_pub_ = create_publisher < std_msgs :: msg :: Bool > ( \"~/output/update_suggest\" , durable_qos ) ; original_map_occ_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/original_occ_map\" , durable_qos ) ; update_map_occ_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/update_occ_map\" , durable_qos ) ; data_ave_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_average_occ_map\" , durable_qos ) ; data_std_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_std_dev_occ_map\" , durable_qos ) ; data_count_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_count_occ_map\" , durable_qos ) ; data_count_with_self_pose_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_count_self_pose_occ_map\" , durable_qos ) ; index_pub_ = create_publisher < visualization_msgs :: msg :: MarkerArray > ( \"/accel_brake_map_calibrator/debug/occ_index\" , durable_qos ) ; original_map_raw_pub_ = create_publisher < std_msgs :: msg :: Float32MultiArray > ( \"/accel_brake_map_calibrator/debug/original_raw_map\" , durable_qos ) ; update_map_raw_pub_ = create_publisher < std_msgs :: msg :: Float32MultiArray > ( \"/accel_brake_map_calibrator/output/update_raw_map\" , durable_qos ) ; debug_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32MultiArrayStamped > ( \"/accel_brake_map_calibrator/output/debug_values\" , durable_qos ) ; current_map_error_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/current_map_error\" , durable_qos ) ; updated_map_error_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/updated_map_error\" , durable_qos ) ; map_error_ratio_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/map_error_ratio\" , durable_qos ) ; // subscriber using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; velocity_sub_ = create_subscription < autoware_auto_vehicle_msgs :: msg :: VelocityReport > ( \"~/input/velocity\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackVelocity , this , _1 ) ) ; steer_sub_ = create_subscription < autoware_auto_vehicle_msgs :: msg :: SteeringReport > ( \"~/input/steer\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackSteer , this , _1 ) ) ; actuation_status_sub_ = create_subscription < tier4_vehicle_msgs :: msg :: ActuationStatusStamped > ( \"~/input/actuation_status\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackActuationStatus , this , _1 ) ) ; // Service update_map_dir_server_ = create_service < tier4_vehicle_msgs :: srv :: UpdateAccelBrakeMap > ( \"~/input/update_map_dir\" , std :: bind ( & AccelBrakeMapCalibrator :: callbackUpdateMapService , this , _1 , _2 , _3 ) ) ; // timer initTimer ( 1.0 / update_hz_ ) ; initOutputCSVTimer ( 30.0 ) ; }', '{ transform_listener_ = std :: make_shared < tier4_autoware_utils :: TransformListener > ( this ) ; // get parameter update_hz_ = this -> declare_parameter < double > ( \"update_hz\" , 10.0 ) ; covariance_ = this -> declare_parameter < double > ( \"initial_covariance\" , 0.05 ) ; velocity_min_threshold_ = this -> declare_parameter < double > ( \"velocity_min_threshold\" , 0.1 ) ; velocity_diff_threshold_ = this -> declare_parameter < double > ( \"velocity_diff_threshold\" , 0.556 ) ; pedal_diff_threshold_ = this -> declare_parameter < double > ( \"pedal_diff_threshold\" , 0.03 ) ; max_steer_threshold_ = this -> declare_parameter < double > ( \"max_steer_threshold\" , 0.2 ) ; max_pitch_threshold_ = this -> declare_parameter < double > ( \"max_pitch_threshold\" , 0.02 ) ; max_jerk_threshold_ = this -> declare_parameter < double > ( \"max_jerk_threshold\" , 0.7 ) ; pedal_velocity_thresh_ = this -> declare_parameter < double > ( \"pedal_velocity_thresh\" , 0.15 ) ; map_update_gain_ = this -> declare_parameter < double > ( \"map_update_gain\" , 0.02 ) ; max_accel_ = this -> declare_parameter < double > ( \"max_accel\" , 5.0 ) ; min_accel_ = this -> declare_parameter < double > ( \"min_accel\" , - 5.0 ) ; pedal_to_accel_delay_ = this -> declare_parameter < double > ( \"pedal_to_accel_delay\" , 0.3 ) ; max_data_count_ = this -> declare_parameter < int > ( \"max_data_count\" , 200 ) ; pedal_accel_graph_output_ = this -> declare_parameter < bool > ( \"pedal_accel_graph_output\" , false ) ; progress_file_output_ = this -> declare_parameter < bool > ( \"progress_file_output\" , false ) ; const auto get_pitch_method_str = this -> declare_parameter < std :: string > ( \"get_pitch_method\" , std :: string ( \"tf\" ) ) ; if ( get_pitch_method_str == std :: string ( \"tf\" ) ) { get_pitch_method_ = GET_PITCH_METHOD :: TF ; } else if ( get_pitch_method_str == std :: string ( \"none\" ) ) { get_pitch_method_ = GET_PITCH_METHOD :: NONE ; } else { RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"update_method_\" << \" is wrong. (available method: tf, file, none\" ) ; return ; } update_suggest_thresh_ = this -> declare_parameter < double > ( \"update_suggest_thresh\" , 0.7 ) ; csv_calibrated_map_dir_ = this -> declare_parameter < std :: string > ( \"csv_calibrated_map_dir\" , std :: string ( \"\" ) ) ; output_accel_file_ = csv_calibrated_map_dir_ + \"/accel_map.csv\" ; output_brake_file_ = csv_calibrated_map_dir_ + \"/brake_map.csv\" ; const std :: string update_method_str = this -> declare_parameter < std :: string > ( \"update_method\" , std :: string ( \"update_offset_each_cell\" ) ) ; if ( update_method_str == std :: string ( \"update_offset_each_cell\" ) ) { update_method_ = UPDATE_METHOD :: UPDATE_OFFSET_EACH_CELL ; } else if ( update_method_str == std :: string ( \"update_offset_total\" ) ) { update_method_ = UPDATE_METHOD :: UPDATE_OFFSET_TOTAL ; } else { RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"update_method\" << \" is wrong. (available method: update_offset_each_cell, update_offset_total\" ) ; return ; } // initializer // QoS setup static constexpr std :: size_t queue_size = 1 ; rclcpp :: QoS durable_qos ( queue_size ) ; // Publisher for checkUpdateSuggest calibration_status_pub_ = create_publisher < tier4_external_api_msgs :: msg :: CalibrationStatus > ( \"/accel_brake_map_calibrator/output/calibration_status\" , durable_qos ) ; /* Diagnostic Updater */ updater_ptr_ = std :: make_shared < diagnostic_updater :: Updater > ( this , 1.0 / update_hz_ ) ; updater_ptr_ -> setHardwareID ( \"accel_brake_map_calibrator\" ) ; updater_ptr_ -> add ( \"accel_brake_map_calibrator\" , this , & AccelBrakeMapCalibrator :: checkUpdateSuggest ) ; { csv_default_map_dir_ = this -> declare_parameter < std :: string > ( \"csv_default_map_dir\" , std :: string ( \"\" ) ) ; std :: string csv_path_accel_map = csv_default_map_dir_ + \"/accel_map.csv\" ; std :: string csv_path_brake_map = csv_default_map_dir_ + \"/brake_map.csv\" ; if ( ! accel_map_ . readAccelMapFromCSV ( csv_path_accel_map ) || ! new_accel_map_ . readAccelMapFromCSV ( csv_path_accel_map ) ) { is_default_map_ = false ; RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"Cannot read accelmap. csv path = \" << csv_path_accel_map . c_str ( ) << \". stop calculation.\" ) ; return ; } if ( ! brake_map_ . readBrakeMapFromCSV ( csv_path_brake_map ) || ! new_brake_map_ . readBrakeMapFromCSV ( csv_path_brake_map ) ) { is_default_map_ = false ; RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"Cannot read brakemap. csv path = \" << csv_path_brake_map . c_str ( ) << \". stop calculation.\" ) ; return ; } } std :: string output_log_file = this -> declare_parameter < std :: string > ( \"output_log_file\" , std :: string ( \"\" ) ) ; output_log_ . open ( output_log_file ) ; addIndexToCSV ( & output_log_ ) ; debug_values_ . data . resize ( num_debug_values_ ) ; // input map info accel_map_value_ = accel_map_ . getAccelMap ( ) ; brake_map_value_ = brake_map_ . getBrakeMap ( ) ; accel_vel_index_ = accel_map_ . getVelIdx ( ) ; brake_vel_index_ = brake_map_ . getVelIdx ( ) ; accel_pedal_index_ = accel_map_ . getThrottleIdx ( ) ; brake_pedal_index_ = brake_map_ . getBrakeIdx ( ) ; update_accel_map_value_ . resize ( ( accel_map_value_ . size ( ) ) ) ; update_brake_map_value_ . resize ( ( brake_map_value_ . size ( ) ) ) ; for ( auto & m : update_accel_map_value_ ) { m . resize ( accel_map_value_ . at ( 0 ) . size ( ) ) ; } for ( auto & m : update_brake_map_value_ ) { m . resize ( brake_map_value_ . at ( 0 ) . size ( ) ) ; } map_value_data_ . resize ( accel_map_value_ . size ( ) + brake_map_value_ . size ( ) - 1 ) ; for ( auto & m : map_value_data_ ) { m . resize ( accel_map_value_ . at ( 0 ) . size ( ) ) ; } std :: copy ( accel_map_value_ . begin ( ) , accel_map_value_ . end ( ) , update_accel_map_value_ . begin ( ) ) ; std :: copy ( brake_map_value_ . begin ( ) , brake_map_value_ . end ( ) , update_brake_map_value_ . begin ( ) ) ; // publisher update_suggest_pub_ = create_publisher < std_msgs :: msg :: Bool > ( \"~/output/update_suggest\" , durable_qos ) ; original_map_occ_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/original_occ_map\" , durable_qos ) ; update_map_occ_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/update_occ_map\" , durable_qos ) ; data_ave_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_average_occ_map\" , durable_qos ) ; data_std_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_std_dev_occ_map\" , durable_qos ) ; data_count_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_count_occ_map\" , durable_qos ) ; data_count_with_self_pose_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_count_self_pose_occ_map\" , durable_qos ) ; index_pub_ = create_publisher < visualization_msgs :: msg :: MarkerArray > ( \"/accel_brake_map_calibrator/debug/occ_index\" , durable_qos ) ; original_map_raw_pub_ = create_publisher < std_msgs :: msg :: Float32MultiArray > ( \"/accel_brake_map_calibrator/debug/original_raw_map\" , durable_qos ) ; update_map_raw_pub_ = create_publisher < std_msgs :: msg :: Float32MultiArray > ( \"/accel_brake_map_calibrator/output/update_raw_map\" , durable_qos ) ; debug_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32MultiArrayStamped > ( \"/accel_brake_map_calibrator/output/debug_values\" , durable_qos ) ; current_map_error_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/current_map_error\" , durable_qos ) ; updated_map_error_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/updated_map_error\" , durable_qos ) ; map_error_ratio_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/map_error_ratio\" , durable_qos ) ; // subscriber using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; velocity_sub_ = create_subscription < autoware_auto_vehicle_msgs :: msg :: VelocityReport > ( \"~/input/velocity\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackVelocity , this , _1 ) ) ; steer_sub_ = create_subscription < autoware_auto_vehicle_msgs :: msg :: SteeringReport > ( \"~/input/steer\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackSteer , this , _1 ) ) ; actuation_status_sub_ = create_subscription < tier4_vehicle_msgs :: msg :: ActuationStatusStamped > ( \"~/input/actuation_status\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackActuationStatus , this , _1 ) ) ; // Service update_map_dir_server_ = create_service < tier4_vehicle_msgs :: srv :: UpdateAccelBrakeMap > ( \"~/input/update_map_dir\" , std :: bind ( & AccelBrakeMapCalibrator :: callbackUpdateMapService , this , _1 , _2 , _3 ) ) ; // timer initTimer ( 1.0 / update_hz_ ) ; initOutputCSVTimer ( 30.0 ) ; }', '{ transform_listener_ = std :: make_shared < tier4_autoware_utils :: TransformListener > ( this ) ; // get parameter update_hz_ = this -> declare_parameter < double > ( \"update_hz\" , 10.0 ) ; covariance_ = this -> declare_parameter < double > ( \"initial_covariance\" , 0.05 ) ; velocity_min_threshold_ = this -> declare_parameter < double > ( \"velocity_min_threshold\" , 0.1 ) ; velocity_diff_threshold_ = this -> declare_parameter < double > ( \"velocity_diff_threshold\" , 0.556 ) ; pedal_diff_threshold_ = this -> declare_parameter < double > ( \"pedal_diff_threshold\" , 0.03 ) ; max_steer_threshold_ = this -> declare_parameter < double > ( \"max_steer_threshold\" , 0.2 ) ; max_pitch_threshold_ = this -> declare_parameter < double > ( \"max_pitch_threshold\" , 0.02 ) ; max_jerk_threshold_ = this -> declare_parameter < double > ( \"max_jerk_threshold\" , 0.7 ) ; pedal_velocity_thresh_ = this -> declare_parameter < double > ( \"pedal_velocity_thresh\" , 0.15 ) ; map_update_gain_ = this -> declare_parameter < double > ( \"map_update_gain\" , 0.02 ) ; max_accel_ = this -> declare_parameter < double > ( \"max_accel\" , 5.0 ) ; min_accel_ = this -> declare_parameter < double > ( \"min_accel\" , - 5.0 ) ; pedal_to_accel_delay_ = this -> declare_parameter < double > ( \"pedal_to_accel_delay\" , 0.3 ) ; max_data_count_ = this -> declare_parameter < int > ( \"max_data_count\" , 200 ) ; pedal_accel_graph_output_ = this -> declare_parameter < bool > ( \"pedal_accel_graph_output\" , false ) ; progress_file_output_ = this -> declare_parameter < bool > ( \"progress_file_output\" , false ) ; const auto get_pitch_method_str = this -> declare_parameter < std :: string > ( \"get_pitch_method\" , std :: string ( \"tf\" ) ) ; if ( get_pitch_method_str == std :: string ( \"tf\" ) ) { get_pitch_method_ = GET_PITCH_METHOD :: TF ; } else if ( get_pitch_method_str == std :: string ( \"none\" ) ) { get_pitch_method_ = GET_PITCH_METHOD :: NONE ; } else { RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"update_method_\" << \" is wrong. (available method: tf, file, none\" ) ; return ; } update_suggest_thresh_ = this -> declare_parameter < double > ( \"update_suggest_thresh\" , 0.7 ) ; csv_calibrated_map_dir_ = this -> declare_parameter < std :: string > ( \"csv_calibrated_map_dir\" , std :: string ( \"\" ) ) ; output_accel_file_ = csv_calibrated_map_dir_ + \"/accel_map.csv\" ; output_brake_file_ = csv_calibrated_map_dir_ + \"/brake_map.csv\" ; const std :: string update_method_str = this -> declare_parameter < std :: string > ( \"update_method\" , std :: string ( \"update_offset_each_cell\" ) ) ; if ( update_method_str == std :: string ( \"update_offset_each_cell\" ) ) { update_method_ = UPDATE_METHOD :: UPDATE_OFFSET_EACH_CELL ; } else if ( update_method_str == std :: string ( \"update_offset_total\" ) ) { update_method_ = UPDATE_METHOD :: UPDATE_OFFSET_TOTAL ; } else { RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"update_method\" << \" is wrong. (available method: update_offset_each_cell, update_offset_total\" ) ; return ; } // initializer // QoS setup static constexpr std :: size_t queue_size = 1 ; rclcpp :: QoS durable_qos ( queue_size ) ; // Publisher for checkUpdateSuggest calibration_status_pub_ = create_publisher < tier4_external_api_msgs :: msg :: CalibrationStatus > ( \"/accel_brake_map_calibrator/output/calibration_status\" , durable_qos ) ; /* Diagnostic Updater */ updater_ptr_ = std :: make_shared < diagnostic_updater :: Updater > ( this , 1.0 / update_hz_ ) ; updater_ptr_ -> setHardwareID ( \"accel_brake_map_calibrator\" ) ; updater_ptr_ -> add ( \"accel_brake_map_calibrator\" , this , & AccelBrakeMapCalibrator :: checkUpdateSuggest ) ; { csv_default_map_dir_ = this -> declare_parameter < std :: string > ( \"csv_default_map_dir\" , std :: string ( \"\" ) ) ; std :: string csv_path_accel_map = csv_default_map_dir_ + \"/accel_map.csv\" ; std :: string csv_path_brake_map = csv_default_map_dir_ + \"/brake_map.csv\" ; if ( ! accel_map_ . readAccelMapFromCSV ( csv_path_accel_map ) || ! new_accel_map_ . readAccelMapFromCSV ( csv_path_accel_map ) ) { is_default_map_ = false ; RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"Cannot read accelmap. csv path = \" << csv_path_accel_map . c_str ( ) << \". stop calculation.\" ) ; return ; } if ( ! brake_map_ . readBrakeMapFromCSV ( csv_path_brake_map ) || ! new_brake_map_ . readBrakeMapFromCSV ( csv_path_brake_map ) ) { is_default_map_ = false ; RCLCPP_ERROR_STREAM ( rclcpp :: get_logger ( \"accel_brake_map_calibrator\" ) , \"Cannot read brakemap. csv path = \" << csv_path_brake_map . c_str ( ) << \". stop calculation.\" ) ; return ; } } std :: string output_log_file = this -> declare_parameter < std :: string > ( \"output_log_file\" , std :: string ( \"\" ) ) ; output_log_ . open ( output_log_file ) ; addIndexToCSV ( & output_log_ ) ; debug_values_ . data . resize ( num_debug_values_ ) ; // input map info accel_map_value_ = accel_map_ . getAccelMap ( ) ; brake_map_value_ = brake_map_ . getBrakeMap ( ) ; accel_vel_index_ = accel_map_ . getVelIdx ( ) ; brake_vel_index_ = brake_map_ . getVelIdx ( ) ; accel_pedal_index_ = accel_map_ . getThrottleIdx ( ) ; brake_pedal_index_ = brake_map_ . getBrakeIdx ( ) ; update_accel_map_value_ . resize ( ( accel_map_value_ . size ( ) ) ) ; update_brake_map_value_ . resize ( ( brake_map_value_ . size ( ) ) ) ; for ( auto & m : update_accel_map_value_ ) { m . resize ( accel_map_value_ . at ( 0 ) . size ( ) ) ; } for ( auto & m : update_brake_map_value_ ) { m . resize ( brake_map_value_ . at ( 0 ) . size ( ) ) ; } map_value_data_ . resize ( accel_map_value_ . size ( ) + brake_map_value_ . size ( ) - 1 ) ; for ( auto & m : map_value_data_ ) { m . resize ( accel_map_value_ . at ( 0 ) . size ( ) ) ; } std :: copy ( accel_map_value_ . begin ( ) , accel_map_value_ . end ( ) , update_accel_map_value_ . begin ( ) ) ; std :: copy ( brake_map_value_ . begin ( ) , brake_map_value_ . end ( ) , update_brake_map_value_ . begin ( ) ) ; // publisher update_suggest_pub_ = create_publisher < std_msgs :: msg :: Bool > ( \"~/output/update_suggest\" , durable_qos ) ; original_map_occ_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/original_occ_map\" , durable_qos ) ; update_map_occ_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/update_occ_map\" , durable_qos ) ; data_ave_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_average_occ_map\" , durable_qos ) ; data_std_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_std_dev_occ_map\" , durable_qos ) ; data_count_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_count_occ_map\" , durable_qos ) ; data_count_with_self_pose_pub_ = create_publisher < nav_msgs :: msg :: OccupancyGrid > ( \"/accel_brake_map_calibrator/debug/data_count_self_pose_occ_map\" , durable_qos ) ; index_pub_ = create_publisher < visualization_msgs :: msg :: MarkerArray > ( \"/accel_brake_map_calibrator/debug/occ_index\" , durable_qos ) ; original_map_raw_pub_ = create_publisher < std_msgs :: msg :: Float32MultiArray > ( \"/accel_brake_map_calibrator/debug/original_raw_map\" , durable_qos ) ; update_map_raw_pub_ = create_publisher < std_msgs :: msg :: Float32MultiArray > ( \"/accel_brake_map_calibrator/output/update_raw_map\" , durable_qos ) ; debug_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32MultiArrayStamped > ( \"/accel_brake_map_calibrator/output/debug_values\" , durable_qos ) ; current_map_error_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/current_map_error\" , durable_qos ) ; updated_map_error_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/updated_map_error\" , durable_qos ) ; map_error_ratio_pub_ = create_publisher < tier4_debug_msgs :: msg :: Float32Stamped > ( \"/accel_brake_map_calibrator/output/map_error_ratio\" , durable_qos ) ; // subscriber using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; velocity_sub_ = create_subscription < autoware_auto_vehicle_msgs :: msg :: VelocityReport > ( \"~/input/velocity\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackVelocity , this , _1 ) ) ; steer_sub_ = create_subscription < autoware_auto_vehicle_msgs :: msg :: SteeringReport > ( \"~/input/steer\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackSteer , this , _1 ) ) ; actuation_status_sub_ = create_subscription < tier4_vehicle_msgs :: msg :: ActuationStatusStamped > ( \"~/input/actuation_status\" , queue_size , std :: bind ( & AccelBrakeMapCalibrator :: callbackActuationStatus , this , _1 ) ) ; // Service update_map_dir_server_ = create_service < tier4_vehicle_msgs :: srv :: UpdateAccelBrakeMap > ( \"~/input/update_map_dir\" , std :: bind ( & AccelBrakeMapCalibrator :: callbackUpdateMapService , this , _1 , _2 , _3 ) ) ; // timer initTimer ( 1.0 / update_hz_ ) ; initOutputCSVTimer ( 30.0 ) ; }']\n", "[]\n", "[]\n", "['{ using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; // Parameter update_rate_ = this -> declare_parameter ( \"update_rate\" , 10.0 ) ; disengage_on_route_ = this -> declare_parameter ( \"disengage_on_route\" , true ) ; disengage_on_goal_ = this -> declare_parameter ( \"disengage_on_goal\" , true ) ; // Parameter for StateMachine state_param_ . th_arrived_distance_m = this -> declare_parameter ( \"th_arrived_distance_m\" , 1.0 ) ; const auto th_arrived_angle_deg = this -> declare_parameter ( \"th_arrived_angle_deg\" , 45.0 ) ; state_param_ . th_arrived_angle = tier4_autoware_utils :: deg2rad ( th_arrived_angle_deg ) ; state_param_ . th_stopped_time_sec = this -> declare_parameter ( \"th_stopped_time_sec\" , 1.0 ) ; state_param_ . th_stopped_velocity_mps = this -> declare_parameter ( \"th_stopped_velocity_mps\" , 0.01 ) ; // State Machine state_machine_ = std :: make_shared < StateMachine > ( state_param_ ) ; // Config topic_configs_ = getConfigs < TopicConfig > ( this -> get_node_parameters_interface ( ) , \"topic_configs\" ) ; tf_configs_ = getConfigs < TfConfig > ( this -> get_node_parameters_interface ( ) , \"tf_configs\" ) ; // Callback Groups callback_group_subscribers_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; callback_group_services_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; auto subscriber_option = rclcpp :: SubscriptionOptions ( ) ; subscriber_option . callback_group = callback_group_subscribers_ ; // Topic Callback for ( const auto & topic_config : topic_configs_ ) { registerTopicCallback ( topic_config . name , topic_config . type , topic_config . transient_local , topic_config . best_effort ) ; } // Subscriber sub_autoware_engage_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"input/autoware_engage\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onAutowareEngage , this , _1 ) , subscriber_option ) ; sub_control_mode_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: ControlModeReport > ( \"input/control_mode\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onVehicleControlMode , this , _1 ) , subscriber_option ) ; sub_map_ = create_subscription < autoware_auto_mapping_msgs :: msg :: HADMapBin > ( \"input/vector_map\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onMap , this , _1 ) , subscriber_option ) ; sub_route_ = this -> create_subscription < autoware_auto_planning_msgs :: msg :: HADMapRoute > ( \"input/route\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onRoute , this , _1 ) , subscriber_option ) ; sub_odom_ = this -> create_subscription < nav_msgs :: msg :: Odometry > ( \"input/odometry\" , 100 , std :: bind ( & AutowareStateMonitorNode :: onOdometry , this , _1 ) , subscriber_option ) ; // Service srv_shutdown_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/shutdown\" , std :: bind ( & AutowareStateMonitorNode :: onShutdownService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; srv_reset_route_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/reset_route\" , std :: bind ( & AutowareStateMonitorNode :: onResetRouteService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; // Publisher pub_autoware_state_ = this -> create_publisher < autoware_auto_system_msgs :: msg :: AutowareState > ( \"output/autoware_state\" , 1 ) ; pub_autoware_engage_ = this -> create_publisher < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"output/autoware_engage\" , 1 ) ; // Diagnostic Updater setupDiagnosticUpdater ( ) ; // Wait for first topics std :: this_thread :: sleep_for ( std :: chrono :: milliseconds ( 1000 ) ) ; // Timer const auto period_ns = rclcpp :: Rate ( update_rate_ ) . period ( ) ; timer_ = rclcpp :: create_timer ( this , get_clock ( ) , period_ns , std :: bind ( & AutowareStateMonitorNode :: onTimer , this ) , callback_group_subscribers_ ) ; }', '{ using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; // Parameter update_rate_ = this -> declare_parameter ( \"update_rate\" , 10.0 ) ; disengage_on_route_ = this -> declare_parameter ( \"disengage_on_route\" , true ) ; disengage_on_goal_ = this -> declare_parameter ( \"disengage_on_goal\" , true ) ; // Parameter for StateMachine state_param_ . th_arrived_distance_m = this -> declare_parameter ( \"th_arrived_distance_m\" , 1.0 ) ; const auto th_arrived_angle_deg = this -> declare_parameter ( \"th_arrived_angle_deg\" , 45.0 ) ; state_param_ . th_arrived_angle = tier4_autoware_utils :: deg2rad ( th_arrived_angle_deg ) ; state_param_ . th_stopped_time_sec = this -> declare_parameter ( \"th_stopped_time_sec\" , 1.0 ) ; state_param_ . th_stopped_velocity_mps = this -> declare_parameter ( \"th_stopped_velocity_mps\" , 0.01 ) ; // State Machine state_machine_ = std :: make_shared < StateMachine > ( state_param_ ) ; // Config topic_configs_ = getConfigs < TopicConfig > ( this -> get_node_parameters_interface ( ) , \"topic_configs\" ) ; tf_configs_ = getConfigs < TfConfig > ( this -> get_node_parameters_interface ( ) , \"tf_configs\" ) ; // Callback Groups callback_group_subscribers_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; callback_group_services_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; auto subscriber_option = rclcpp :: SubscriptionOptions ( ) ; subscriber_option . callback_group = callback_group_subscribers_ ; // Topic Callback for ( const auto & topic_config : topic_configs_ ) { registerTopicCallback ( topic_config . name , topic_config . type , topic_config . transient_local , topic_config . best_effort ) ; } // Subscriber sub_autoware_engage_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"input/autoware_engage\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onAutowareEngage , this , _1 ) , subscriber_option ) ; sub_control_mode_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: ControlModeReport > ( \"input/control_mode\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onVehicleControlMode , this , _1 ) , subscriber_option ) ; sub_map_ = create_subscription < autoware_auto_mapping_msgs :: msg :: HADMapBin > ( \"input/vector_map\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onMap , this , _1 ) , subscriber_option ) ; sub_route_ = this -> create_subscription < autoware_auto_planning_msgs :: msg :: HADMapRoute > ( \"input/route\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onRoute , this , _1 ) , subscriber_option ) ; sub_odom_ = this -> create_subscription < nav_msgs :: msg :: Odometry > ( \"input/odometry\" , 100 , std :: bind ( & AutowareStateMonitorNode :: onOdometry , this , _1 ) , subscriber_option ) ; // Service srv_shutdown_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/shutdown\" , std :: bind ( & AutowareStateMonitorNode :: onShutdownService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; srv_reset_route_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/reset_route\" , std :: bind ( & AutowareStateMonitorNode :: onResetRouteService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; // Publisher pub_autoware_state_ = this -> create_publisher < autoware_auto_system_msgs :: msg :: AutowareState > ( \"output/autoware_state\" , 1 ) ; pub_autoware_engage_ = this -> create_publisher < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"output/autoware_engage\" , 1 ) ; // Diagnostic Updater setupDiagnosticUpdater ( ) ; // Wait for first topics std :: this_thread :: sleep_for ( std :: chrono :: milliseconds ( 1000 ) ) ; // Timer const auto period_ns = rclcpp :: Rate ( update_rate_ ) . period ( ) ; timer_ = rclcpp :: create_timer ( this , get_clock ( ) , period_ns , std :: bind ( & AutowareStateMonitorNode :: onTimer , this ) , callback_group_subscribers_ ) ; }', '{ using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; // Parameter update_rate_ = this -> declare_parameter ( \"update_rate\" , 10.0 ) ; disengage_on_route_ = this -> declare_parameter ( \"disengage_on_route\" , true ) ; disengage_on_goal_ = this -> declare_parameter ( \"disengage_on_goal\" , true ) ; // Parameter for StateMachine state_param_ . th_arrived_distance_m = this -> declare_parameter ( \"th_arrived_distance_m\" , 1.0 ) ; const auto th_arrived_angle_deg = this -> declare_parameter ( \"th_arrived_angle_deg\" , 45.0 ) ; state_param_ . th_arrived_angle = tier4_autoware_utils :: deg2rad ( th_arrived_angle_deg ) ; state_param_ . th_stopped_time_sec = this -> declare_parameter ( \"th_stopped_time_sec\" , 1.0 ) ; state_param_ . th_stopped_velocity_mps = this -> declare_parameter ( \"th_stopped_velocity_mps\" , 0.01 ) ; // State Machine state_machine_ = std :: make_shared < StateMachine > ( state_param_ ) ; // Config topic_configs_ = getConfigs < TopicConfig > ( this -> get_node_parameters_interface ( ) , \"topic_configs\" ) ; tf_configs_ = getConfigs < TfConfig > ( this -> get_node_parameters_interface ( ) , \"tf_configs\" ) ; // Callback Groups callback_group_subscribers_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; callback_group_services_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; auto subscriber_option = rclcpp :: SubscriptionOptions ( ) ; subscriber_option . callback_group = callback_group_subscribers_ ; // Topic Callback for ( const auto & topic_config : topic_configs_ ) { registerTopicCallback ( topic_config . name , topic_config . type , topic_config . transient_local , topic_config . best_effort ) ; } // Subscriber sub_autoware_engage_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"input/autoware_engage\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onAutowareEngage , this , _1 ) , subscriber_option ) ; sub_control_mode_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: ControlModeReport > ( \"input/control_mode\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onVehicleControlMode , this , _1 ) , subscriber_option ) ; sub_map_ = create_subscription < autoware_auto_mapping_msgs :: msg :: HADMapBin > ( \"input/vector_map\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onMap , this , _1 ) , subscriber_option ) ; sub_route_ = this -> create_subscription < autoware_auto_planning_msgs :: msg :: HADMapRoute > ( \"input/route\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onRoute , this , _1 ) , subscriber_option ) ; sub_odom_ = this -> create_subscription < nav_msgs :: msg :: Odometry > ( \"input/odometry\" , 100 , std :: bind ( & AutowareStateMonitorNode :: onOdometry , this , _1 ) , subscriber_option ) ; // Service srv_shutdown_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/shutdown\" , std :: bind ( & AutowareStateMonitorNode :: onShutdownService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; srv_reset_route_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/reset_route\" , std :: bind ( & AutowareStateMonitorNode :: onResetRouteService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; // Publisher pub_autoware_state_ = this -> create_publisher < autoware_auto_system_msgs :: msg :: AutowareState > ( \"output/autoware_state\" , 1 ) ; pub_autoware_engage_ = this -> create_publisher < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"output/autoware_engage\" , 1 ) ; // Diagnostic Updater setupDiagnosticUpdater ( ) ; // Wait for first topics std :: this_thread :: sleep_for ( std :: chrono :: milliseconds ( 1000 ) ) ; // Timer const auto period_ns = rclcpp :: Rate ( update_rate_ ) . period ( ) ; timer_ = rclcpp :: create_timer ( this , get_clock ( ) , period_ns , std :: bind ( & AutowareStateMonitorNode :: onTimer , this ) , callback_group_subscribers_ ) ; }', '{ using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; // Parameter update_rate_ = this -> declare_parameter ( \"update_rate\" , 10.0 ) ; disengage_on_route_ = this -> declare_parameter ( \"disengage_on_route\" , true ) ; disengage_on_goal_ = this -> declare_parameter ( \"disengage_on_goal\" , true ) ; // Parameter for StateMachine state_param_ . th_arrived_distance_m = this -> declare_parameter ( \"th_arrived_distance_m\" , 1.0 ) ; const auto th_arrived_angle_deg = this -> declare_parameter ( \"th_arrived_angle_deg\" , 45.0 ) ; state_param_ . th_arrived_angle = tier4_autoware_utils :: deg2rad ( th_arrived_angle_deg ) ; state_param_ . th_stopped_time_sec = this -> declare_parameter ( \"th_stopped_time_sec\" , 1.0 ) ; state_param_ . th_stopped_velocity_mps = this -> declare_parameter ( \"th_stopped_velocity_mps\" , 0.01 ) ; // State Machine state_machine_ = std :: make_shared < StateMachine > ( state_param_ ) ; // Config topic_configs_ = getConfigs < TopicConfig > ( this -> get_node_parameters_interface ( ) , \"topic_configs\" ) ; tf_configs_ = getConfigs < TfConfig > ( this -> get_node_parameters_interface ( ) , \"tf_configs\" ) ; // Callback Groups callback_group_subscribers_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; callback_group_services_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; auto subscriber_option = rclcpp :: SubscriptionOptions ( ) ; subscriber_option . callback_group = callback_group_subscribers_ ; // Topic Callback for ( const auto & topic_config : topic_configs_ ) { registerTopicCallback ( topic_config . name , topic_config . type , topic_config . transient_local , topic_config . best_effort ) ; } // Subscriber sub_autoware_engage_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"input/autoware_engage\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onAutowareEngage , this , _1 ) , subscriber_option ) ; sub_control_mode_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: ControlModeReport > ( \"input/control_mode\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onVehicleControlMode , this , _1 ) , subscriber_option ) ; sub_map_ = create_subscription < autoware_auto_mapping_msgs :: msg :: HADMapBin > ( \"input/vector_map\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onMap , this , _1 ) , subscriber_option ) ; sub_route_ = this -> create_subscription < autoware_auto_planning_msgs :: msg :: HADMapRoute > ( \"input/route\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onRoute , this , _1 ) , subscriber_option ) ; sub_odom_ = this -> create_subscription < nav_msgs :: msg :: Odometry > ( \"input/odometry\" , 100 , std :: bind ( & AutowareStateMonitorNode :: onOdometry , this , _1 ) , subscriber_option ) ; // Service srv_shutdown_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/shutdown\" , std :: bind ( & AutowareStateMonitorNode :: onShutdownService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; srv_reset_route_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/reset_route\" , std :: bind ( & AutowareStateMonitorNode :: onResetRouteService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; // Publisher pub_autoware_state_ = this -> create_publisher < autoware_auto_system_msgs :: msg :: AutowareState > ( \"output/autoware_state\" , 1 ) ; pub_autoware_engage_ = this -> create_publisher < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"output/autoware_engage\" , 1 ) ; // Diagnostic Updater setupDiagnosticUpdater ( ) ; // Wait for first topics std :: this_thread :: sleep_for ( std :: chrono :: milliseconds ( 1000 ) ) ; // Timer const auto period_ns = rclcpp :: Rate ( update_rate_ ) . period ( ) ; timer_ = rclcpp :: create_timer ( this , get_clock ( ) , period_ns , std :: bind ( & AutowareStateMonitorNode :: onTimer , this ) , callback_group_subscribers_ ) ; }', '{ using std :: placeholders :: _1 ; using std :: placeholders :: _2 ; using std :: placeholders :: _3 ; // Parameter update_rate_ = this -> declare_parameter ( \"update_rate\" , 10.0 ) ; disengage_on_route_ = this -> declare_parameter ( \"disengage_on_route\" , true ) ; disengage_on_goal_ = this -> declare_parameter ( \"disengage_on_goal\" , true ) ; // Parameter for StateMachine state_param_ . th_arrived_distance_m = this -> declare_parameter ( \"th_arrived_distance_m\" , 1.0 ) ; const auto th_arrived_angle_deg = this -> declare_parameter ( \"th_arrived_angle_deg\" , 45.0 ) ; state_param_ . th_arrived_angle = tier4_autoware_utils :: deg2rad ( th_arrived_angle_deg ) ; state_param_ . th_stopped_time_sec = this -> declare_parameter ( \"th_stopped_time_sec\" , 1.0 ) ; state_param_ . th_stopped_velocity_mps = this -> declare_parameter ( \"th_stopped_velocity_mps\" , 0.01 ) ; // State Machine state_machine_ = std :: make_shared < StateMachine > ( state_param_ ) ; // Config topic_configs_ = getConfigs < TopicConfig > ( this -> get_node_parameters_interface ( ) , \"topic_configs\" ) ; tf_configs_ = getConfigs < TfConfig > ( this -> get_node_parameters_interface ( ) , \"tf_configs\" ) ; // Callback Groups callback_group_subscribers_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; callback_group_services_ = this -> create_callback_group ( rclcpp :: CallbackGroupType :: MutuallyExclusive ) ; auto subscriber_option = rclcpp :: SubscriptionOptions ( ) ; subscriber_option . callback_group = callback_group_subscribers_ ; // Topic Callback for ( const auto & topic_config : topic_configs_ ) { registerTopicCallback ( topic_config . name , topic_config . type , topic_config . transient_local , topic_config . best_effort ) ; } // Subscriber sub_autoware_engage_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"input/autoware_engage\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onAutowareEngage , this , _1 ) , subscriber_option ) ; sub_control_mode_ = this -> create_subscription < autoware_auto_vehicle_msgs :: msg :: ControlModeReport > ( \"input/control_mode\" , 1 , std :: bind ( & AutowareStateMonitorNode :: onVehicleControlMode , this , _1 ) , subscriber_option ) ; sub_map_ = create_subscription < autoware_auto_mapping_msgs :: msg :: HADMapBin > ( \"input/vector_map\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onMap , this , _1 ) , subscriber_option ) ; sub_route_ = this -> create_subscription < autoware_auto_planning_msgs :: msg :: HADMapRoute > ( \"input/route\" , rclcpp :: QoS { 1 } . transient_local ( ) , std :: bind ( & AutowareStateMonitorNode :: onRoute , this , _1 ) , subscriber_option ) ; sub_odom_ = this -> create_subscription < nav_msgs :: msg :: Odometry > ( \"input/odometry\" , 100 , std :: bind ( & AutowareStateMonitorNode :: onOdometry , this , _1 ) , subscriber_option ) ; // Service srv_shutdown_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/shutdown\" , std :: bind ( & AutowareStateMonitorNode :: onShutdownService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; srv_reset_route_ = this -> create_service < std_srvs :: srv :: Trigger > ( \"service/reset_route\" , std :: bind ( & AutowareStateMonitorNode :: onResetRouteService , this , _1 , _2 , _3 ) , rmw_qos_profile_services_default , callback_group_services_ ) ; // Publisher pub_autoware_state_ = this -> create_publisher < autoware_auto_system_msgs :: msg :: AutowareState > ( \"output/autoware_state\" , 1 ) ; pub_autoware_engage_ = this -> create_publisher < autoware_auto_vehicle_msgs :: msg :: Engage > ( \"output/autoware_engage\" , 1 ) ; // Diagnostic Updater setupDiagnosticUpdater ( ) ; // Wait for first topics std :: this_thread :: sleep_for ( std :: chrono :: milliseconds ( 1000 ) ) ; // Timer const auto period_ns = rclcpp :: Rate ( update_rate_ ) . period ( ) ; timer_ = rclcpp :: create_timer ( this , get_clock ( ) , period_ns , std :: bind ( & AutowareStateMonitorNode :: onTimer , this ) , callback_group_subscribers_ ) ; }']\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n", "[]\n" ] }, { "ename": "KeyboardInterrupt", "evalue": "", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", "\u001b[1;32m/mnt/c/Users/maxim/Projects/ma-autoware-dependency-digger/clang-digger.ipynb Cell 5'\u001b[0m in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[39mfor\u001b[39;00m src_file, args \u001b[39min\u001b[39;00m compile_commands\u001b[39m.\u001b[39mitems():\n\u001b[1;32m 5\u001b[0m src_file: \u001b[39mstr\u001b[39m\n\u001b[0;32m----> 6\u001b[0m tu \u001b[39m=\u001b[39m idx\u001b[39m.\u001b[39;49mparse(src_file, args\u001b[39m=\u001b[39;49margs) \u001b[39m# 0x200: KeepGoing (after fatal errors)\u001b[39;00m\n\u001b[1;32m 7\u001b[0m h \u001b[39m=\u001b[39m ast_utils\u001b[39m.\u001b[39mTUHandler(tu)\n\u001b[1;32m 8\u001b[0m h\u001b[39m.\u001b[39mget_subscription_callback_handlers()\n", "File \u001b[0;32m~/.local/lib/python3.10/site-packages/clang/cindex.py:2815\u001b[0m, in \u001b[0;36mIndex.parse\u001b[0;34m(self, path, args, unsaved_files, options)\u001b[0m\n\u001b[1;32m 2802\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mparse\u001b[39m(\u001b[39mself\u001b[39m, path, args\u001b[39m=\u001b[39m\u001b[39mNone\u001b[39;00m, unsaved_files\u001b[39m=\u001b[39m\u001b[39mNone\u001b[39;00m, options \u001b[39m=\u001b[39m \u001b[39m0\u001b[39m):\n\u001b[1;32m 2803\u001b[0m \u001b[39m\"\"\"Load the translation unit from the given source code file by running\u001b[39;00m\n\u001b[1;32m 2804\u001b[0m \u001b[39m clang and generating the AST before loading. Additional command line\u001b[39;00m\n\u001b[1;32m 2805\u001b[0m \u001b[39m parameters can be passed to clang via the args parameter.\u001b[39;00m\n\u001b[0;32m (...)\u001b[0m\n\u001b[1;32m 2813\u001b[0m \u001b[39m will be raised.\u001b[39;00m\n\u001b[1;32m 2814\u001b[0m \u001b[39m \"\"\"\u001b[39;00m\n\u001b[0;32m-> 2815\u001b[0m \u001b[39mreturn\u001b[39;00m TranslationUnit\u001b[39m.\u001b[39;49mfrom_source(path, args, unsaved_files, options,\n\u001b[1;32m 2816\u001b[0m \u001b[39mself\u001b[39;49m)\n", "File \u001b[0;32m~/.local/lib/python3.10/site-packages/clang/cindex.py:2923\u001b[0m, in \u001b[0;36mTranslationUnit.from_source\u001b[0;34m(cls, filename, args, unsaved_files, options, index)\u001b[0m\n\u001b[1;32m 2920\u001b[0m unsaved_array[i]\u001b[39m.\u001b[39mcontents \u001b[39m=\u001b[39m contents\n\u001b[1;32m 2921\u001b[0m unsaved_array[i]\u001b[39m.\u001b[39mlength \u001b[39m=\u001b[39m \u001b[39mlen\u001b[39m(contents)\n\u001b[0;32m-> 2923\u001b[0m ptr \u001b[39m=\u001b[39m conf\u001b[39m.\u001b[39;49mlib\u001b[39m.\u001b[39;49mclang_parseTranslationUnit(index,\n\u001b[1;32m 2924\u001b[0m fspath(filename) \u001b[39mif\u001b[39;49;00m filename \u001b[39mis\u001b[39;49;00m \u001b[39mnot\u001b[39;49;00m \u001b[39mNone\u001b[39;49;00m \u001b[39melse\u001b[39;49;00m \u001b[39mNone\u001b[39;49;00m,\n\u001b[1;32m 2925\u001b[0m args_array,\n\u001b[1;32m 2926\u001b[0m \u001b[39mlen\u001b[39;49m(args), unsaved_array,\n\u001b[1;32m 2927\u001b[0m \u001b[39mlen\u001b[39;49m(unsaved_files), options)\n\u001b[1;32m 2929\u001b[0m \u001b[39mif\u001b[39;00m \u001b[39mnot\u001b[39;00m ptr:\n\u001b[1;32m 2930\u001b[0m \u001b[39mraise\u001b[39;00m TranslationUnitLoadError(\u001b[39m\"\u001b[39m\u001b[39mError parsing translation unit.\u001b[39m\u001b[39m\"\u001b[39m)\n", "\u001b[0;31mKeyboardInterrupt\u001b[0m: " ] } ], "source": [ "all_mappings = {}\n", "idx = ci.Index.create()\n", "\n", "for src_file, args in compile_commands.items():\n", " src_file: str\n", " tu = idx.parse(src_file, args=args) # 0x200: KeepGoing (after fatal errors)\n", " h = ast_utils.TUHandler(tu)\n", " h.get_subscription_callback_handlers()\n", "\n", "with open(\"cb_identifiers.json\", \"w\") as f:\n", " json.dump(all_mappings, f)" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } }, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3.10.5 64-bit", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.10.5" }, "orig_nbformat": 4, "vscode": { "interpreter": { "hash": "8a94588eda9d64d9e9a351ab8144e55b1fabf5113b54e67dd26a8c27df0381b3" } } }, "nbformat": 4, "nbformat_minor": 2 }