From d6c83744f86e7df4d5c5e5d00b8aae377ad059d2 Mon Sep 17 00:00:00 2001 From: Tiankui Xian <1041084556@qq.com> Date: Wed, 10 Jul 2024 18:44:38 +0900 Subject: [PATCH] feat(perception reproducer): some improvements in perception reproducer. (#72) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(perception reproducer): improve perception reproducer (#69) * temp save new parameters. Signed-off-by: temkei.kem <1041084556@qq.com> * update parameter Signed-off-by: temkei.kem <1041084556@qq.com> * update commit Signed-off-by: temkei.kem <1041084556@qq.com> * feat(metrics): make it possible to hide graph for each metrics (#64) Signed-off-by: satoshi-ota Signed-off-by: temkei.kem <1041084556@qq.com> * feat(planning_debug_tools): add cpu_usage_checker.py (#66) * feat(planning_debug_tools): add cpu_usage_checker.py Signed-off-by: Takayuki Murooka * update README Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka Signed-off-by: temkei.kem <1041084556@qq.com> * feat(route_client): add route_client (#67) Signed-off-by: Fumiya Watanabe Signed-off-by: temkei.kem <1041084556@qq.com> * fix: autoware_msgs (#68) Signed-off-by: kosuke55 Signed-off-by: temkei.kem <1041084556@qq.com> * feat(control_data_collecting_tool): add control_data_collecting_tool (#33) * Add control data collecting tool for learning-based control * Implement basic part * Disable data collection area selection during automatic driving * Remove unnecessary code * Add rviz_config for data collection * Update rviz_config for data collection * Changed to start driving by pressing LOCAL * Fix hard coding rviz config path when launching * Add a launch file in this package * Add missing changes * Claen up arguments to use rviz_config * Updated pre-commit-hooks-ros version from v0.8.0 to v0.9.0 (#24) Signed-off-by: Shintaro Sakoda * Add 8-figure target route * Avoid errors when data collecting area cannot be selected by clicking properly. * Change to launch another launch for data collection after launching the standard autoware_launch. * chore(tools): move system and evaluation tools to this repo from autoware.universe (#26) * chore(tools): move system tools Signed-off-by: satoshi-ota * chore(evaluator): move evaluators Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota * update * update trajectory publisher * update * feat(driving_environment_analyzer): add rviz plugin (#23) * feat(driving_environment_analyzer): add rviz plugin Signed-off-by: satoshi-ota * feat(driving_environment_analyzer): output csv file Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota * add naive pure pursuit * update * clean up naive pure pursuit * smoothing trajectory position * refactor * chore(rviz_plugin): move peripheral rviz plugin (#27) * feat: calibrator tier4 pkg name (#200) * feat: change names Signed-off-by: tanaka3 * feat: move pkg to common Signed-off-by: tanaka3 * feat: change pkg name Signed-off-by: tanaka3 * fix: library path (#225) Signed-off-by: taikitanaka3 Co-authored-by: taikitanaka3 * feat: add rviz plugin to publish and control the simulated clock (#349) * Add tier4_clock_rviz_plugin to publish&control the sim clock in rviz Signed-off-by: Maxime CLEMENT * Add step control Signed-off-by: Maxime CLEMENT * Fix precommit Signed-off-by: Maxime CLEMENT * Update documentation Signed-off-by: Maxime CLEMENT * Fix spellcheck Signed-off-by: Maxime CLEMENT * Update plugin description and icon Signed-off-by: Maxime CLEMENT * Rename package Signed-off-by: Maxime CLEMENT * Fix bug with long duration jumps (high speed + low rate) Signed-off-by: Maxime CLEMENT * ci: check include guard (#438) * ci: check include guard * apply pre-commit * Update .pre-commit-config.yaml Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * fix: pre-commit Signed-off-by: Takagi, Isamu Co-authored-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * chore: sync files (#629) * chore: sync files Signed-off-by: GitHub * ci(pre-commit): autofix Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * style: fix format of package.xml (#844) Signed-off-by: Kenji Miyake * refactor: use autoware cmake (#849) * remove autoware_auto_cmake Signed-off-by: Kenji Miyake * add build_depend of autoware_cmake Signed-off-by: Kenji Miyake * use autoware_cmake in CMakeLists.txt Signed-off-by: Kenji Miyake * fix bugs Signed-off-by: Kenji Miyake * fix cmake lint errors Signed-off-by: Kenji Miyake * chore: upgrade cmake_minimum_required to 3.14 (#856) Signed-off-by: Kenji Miyake * fix(accel_brake_map_calibrator): rviz panel type (#895) * fixed panel type Signed-off-by: Mamoru Sobue * modified instruction for rosbag replay case Signed-off-by: Mamoru Sobue * modified update_map_dir service name Signed-off-by: Mamoru Sobue * feat: add manual controller (#989) * feat: add manual controller Signed-off-by: tanaka3 * ci(pre-commit): autofix Signed-off-by: Shumpei Wakabayashi * fix : typo Signed-off-by: Shumpei Wakabayashi * chore: minor update Signed-off-by: tanaka3 Signed-off-by: Shumpei Wakabayashi * chore : add minor update Signed-off-by: tanaka3 Signed-off-by: Shumpei Wakabayashi * docs: add image for readme Signed-off-by: Shumpei Wakabayashi * docs: update docs Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix * Update common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp Signed-off-by: Shumpei Wakabayashi Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * Update common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> * feat(manual_controller): support backward driving (#1119) * feat(manual_controller): support backward driving Signed-off-by: tanaka3 * chore: unite variable * feat: add screen capture package (#1177) * feat: add screen capture package Signed-off-by: tanaka3 * feat: add state to current process * style: refactor codes * fix: time * feat: add mp4 writer and finalize folders * feat: add trigger by service Signed-off-by: tanaka3 * feat: update recorder Signed-off-by: tanaka3 * fix: change codec of mp4 to h264 (#1262) Signed-off-by: tomoya.kimura * feat: optimize screen capture panel (#1470) * feat: optimize screen capture panel Signed-off-by: tomoya.kimura * apply pre-commit Signed-off-by: tomoya.kimura * fix: remove unused check of rviz plugin version (#1474) Signed-off-by: Takagi, Isamu * fix(rviz_plugin): fix screen capture plugin (#1492) Signed-off-by: tanaka3 * refactor(tier4_calibration_rviz_plugin): apply clang-tidy (#1596) * refactor(tier4_control_rviz_plugin): apply clang-tidy (#1597) * refactor(tier4_control_rviz_plugin): apply clang-tidy * revert: readability-identifier-naming Signed-off-by: h-ohta Signed-off-by: h-ohta * revert(tier4_calibration_rviz_plugin): readability-identifier-naming (#1618) * fix(tier4_screen_capture_rviz_plugin): fix release process to handle video writer correctly (#1622) * refactor(tier4_screen_capture_rviz_plugin):apply clang-tidy (#1649) Signed-off-by: h-ohta Signed-off-by: h-ohta * fix(tier4_screen_capture_rviz_plugin): fix spell check (#1790) * fix(tier4_screen_capture_rviz_plugin): fix spell check Signed-off-by: h-ohta * ci(pre-commit): autofix Signed-off-by: h-ohta Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat: add rtc manager rviz plugin (#1936) * feat: add rtc manager rviz plugin Signed-off-by: tanaka3 * chore: cosmetic change Signed-off-by: tanaka3 * chore: remove indent Signed-off-by: tanaka3 * feat: add rtc safe unsafe color Signed-off-by: tanaka3 * fix: typo Signed-off-by: tanaka3 * chore: simplify layout Signed-off-by: tanaka3 * feat: update rtc panel Signed-off-by: tanaka3 Signed-off-by: tanaka3 * feat(rtc_manager_panel): add rtc all approval (#2009) * feat(rtc_manager_panel): add rtc all approval Signed-off-by: taikitanaka3 * chore: cosmetic change Signed-off-by: taikitanaka3 * fix: static cast Signed-off-by: tanaka3 * chore: update text Co-authored-by: Fumiya Watanabe * chore: update text Co-authored-by: Fumiya Watanabe * doc: update documents Signed-off-by: tanaka3 * doc: update Signed-off-by: tanaka3 * doc: small update Signed-off-by: tanaka3 Signed-off-by: taikitanaka3 Signed-off-by: tanaka3 Co-authored-by: Fumiya Watanabe * feat(rtc_manager_rviz_plugin): add_indivisual_exe (#2021) Signed-off-by: tanaka3 Signed-off-by: tanaka3 * feat(tier4_screen_capture_rviz_plugin): add prefix to video name (#2038) feat: add prefix * feat(tier4_debug_rviz_plugin): pie chart with float32 multi array stamped (#2055) * feat(tier4_debug_rviz_plugin): add ros2 pie chart with Float32MultiArrayStamped Signed-off-by: Takayuki Murooka * update README.md Signed-off-by: Takayuki Murooka * fix typo Signed-off-by: Takayuki Murooka * fixed license Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * removed unnecessary include Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka * fix(tier4_control_rviz_plugin): add time stamp for control command (#2154) Signed-off-by: tanaka3 Signed-off-by: tanaka3 * fix(rtc_manager_rviz_plugin): size check (#2163) * feat(behavior_path_planner): external request lane change (#2442) * feature(behavior_path_planner): add external request lane change module Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): fix for RTC Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): fix decision logic Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): fix behavior_path_planner_tree.xml Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): fix for rebase Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): output multiple candidate paths Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): get path candidate in behavior tree manager Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): fix for multiple candidate path Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): separate external request lane change module Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): add create publisher method Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): move publishers to node Signed-off-by: Fumiya Watanabe feature(behavior_path_planner): remove unnecessary publisher Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): move reset path candidate function to behavior tree manager Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): add external request lane change path candidate publisher Signed-off-by: Fumiya Watanabe feat(behavior_path_planner): apply abort lane change Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): remove unnecessary change Signed-off-by: Fumiya Watanabe * feat(behavior_path_planner): fix getLaneChangePaths() Signed-off-by: Fumiya Watanabe * feat(behavior_path_planner): disable external request lane change in default tree Signed-off-by: Fumiya Watanabe * Update rtc_auto_mode_manager.param.yaml * fix(route_handler): remove redundant code * fix(behavior_path_planner): fix for turn signal Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe * chore(rtc_manager_rviz_plugin): add code owner (#2792) Signed-off-by: tomoya.kimura * feat(rtc_manager_rviz_plugin): add the number of rtc status (#2791) * feat(rtc_manager_rviz_plugin): add the number of rtc status Signed-off-by: taikitanaka3 * chore: simplify layout Signed-off-by: taikitanaka3 --------- Signed-off-by: taikitanaka3 Co-authored-by: Tomoya Kimura * feat(automatic_goal): add automatic goal rviz plugin (#3031) * add first version automatic_goal plugin Signed-off-by: Dawid Moszyński * feat(automatic_goal): extract automatic_goal_sender, add logging achieved goals Signed-off-by: Dawid Moszyński * doc(automatic_goal): append README Signed-off-by: Dawid Moszyński * ref(automatic_goal): apply pre-commity, fix depend Signed-off-by: Dawid Moszyński * fix(automatic_goal): fix warnings - treated as errors Signed-off-by: Dawid Moszyński * ref(automatic_goal): add author, apply clang-tidy hints Signed-off-by: Dawid Moszyński * ref(automatic_goal): add maintainer, change year Signed-off-by: Dawid Moszyński * ref(automatic_goal): fix package.xml order Signed-off-by: Dawid Moszyński * ref(automatic_goal): names, initializations, main except Signed-off-by: Dawid Moszyński * fix(automatic_goal): change path home->tmp Signed-off-by: Dawid Moszyński * fix(automatic_goal): fix bad string init, expand readme Signed-off-by: Dawid Moszyński * fix(automatic_goal): fix name Signed-off-by: Dawid Moszyński --------- Signed-off-by: Dawid Moszyński * feat(rtc_manager_rviz_plugin): add avoidance by lc (#3118) Signed-off-by: satoshi-ota * fix(tier4_screen_capture_rviz_plugin): fix extra/missing naming components (#3207) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> * chore: sync files (#3227) * chore: sync files Signed-off-by: GitHub * style(pre-commit): autofix --------- Signed-off-by: GitHub Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat: change external lane change rtc name (#3259) * feat: change external lane change rtc name Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka * feat(behavior_velocity_planner::intersection): add occlusion detection feature (#3458) * migrated Signed-off-by: Mamoru Sobue * WIP Signed-off-by: Mamoru Sobue * IntersectionModuleManager own one rtc_interface for intersection_occlusion Signed-off-by: Mamoru Sobue * divide occlusion safety and activated Signed-off-by: Mamoru Sobue * fixed to update occlusion cooperate status at construction Signed-off-by: Mamoru Sobue * fixed getOcclusionFirstStopSafety() Signed-off-by: Mamoru Sobue * fixed not to show both intersection and intersection_occlusion after passed 1st stop line Signed-off-by: Mamoru Sobue * fixed the intersection_occlusion/inersection stop position afte r CLEARED Signed-off-by: Mamoru Sobue * if occlusion cleared when eog is before 1st stop line, set stop line to 1st stop line and clear prev_occlusion_stop_line_pose_ Signed-off-by: Mamoru Sobue * (misc) fix viz, sync param Signed-off-by: Mamoru Sobue * fixed vehicle footprint offset calculation Signed-off-by: Mamoru Sobue * add occcupancy_grid_map method/param var to launcher Signed-off-by: Mamoru Sobue * migrated latest Signed-off-by: Mamoru Sobue * use static pass judge line Signed-off-by: Mamoru Sobue * removed some params Signed-off-by: Mamoru Sobue * organized param Signed-off-by: Mamoru Sobue * add occlusion enable flag Signed-off-by: Mamoru Sobue * revert occupancy grid settings in this PR Signed-off-by: Mamoru Sobue * remove comment Signed-off-by: Mamoru Sobue * fixed pass judge line collision detection to original Signed-off-by: Mamoru Sobue * style(pre-commit): autofix * use vehicle_length for static pass judge line Signed-off-by: Mamoru Sobue * fixed virtual wall marker Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue Signed-off-by: Mamoru Sobue Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(rtc_manager_rviz_plugin): update panel visualization properly (#3517) Signed-off-by: tomoya.kimura * refactor(behavior_path_planner): rename pull_over to goal_planner (#3501) Signed-off-by: kosuke55 * build: mark autoware_cmake as (#3616) * build: mark autoware_cmake as with , autoware_cmake is automatically exported with ament_target_dependencies() (unecessary) Signed-off-by: Vincent Richard * style(pre-commit): autofix * chore: fix pre-commit errors Signed-off-by: Kenji Miyake --------- Signed-off-by: Vincent Richard Signed-off-by: Kenji Miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake * build(iron): remove rmw_qos_profile_t (#3809) Signed-off-by: wep21 * refactor(start_planner): rename pull out to start planner (#3908) Signed-off-by: kosuke55 * fix(accel_brake_map_calibrator_button_panel): fix calibration service name (#4539) * fix(accel_brake_map_calibrator_button_panel): fix calibration service name Signed-off-by: tomoya.kimura * misc Signed-off-by: tomoya.kimura --------- Signed-off-by: tomoya.kimura * feat(rviz_plugin): add target object type display (#4855) * add common/tier4_target_object_type_rviz_plugin Signed-off-by: Takamasa Horibe * fix format Signed-off-by: Takamasa Horibe * update color Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * add readme Signed-off-by: Takamasa Horibe * Update common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * remove unused depend Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix(rtc_manager_panel): fix panel chattering (#4988) Signed-off-by: satoshi-ota * build(tier4_target_object_type_rviz_plugin): add missing cv_bridge dependency (#5000) Signed-off-by: Esteve Fernandez * feat(logging_level_configure): add rviz plugin to change logging level (#5112) * feat(logging_level_configure): add rviz plugin to change logging level Signed-off-by: Takamasa Horibe * change file names Signed-off-by: Takamasa Horibe * move initialization code from constructor to onInitialize Signed-off-by: Takamasa Horibe * add maintainer Signed-off-by: Takamasa Horibe * add maintainer Signed-off-by: Takamasa Horibe * fix include Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe * feat(logger_level_reconfigure_plugin): use node interface and some cosmetic updates (#5204) * use node service Signed-off-by: Takamasa Horibe * enable yaml configuration Signed-off-by: Takamasa Horibe * update yaml loading Signed-off-by: Takamasa Horibe * make it scrollable Signed-off-by: Takamasa Horibe * change function order Signed-off-by: Takamasa Horibe * change color for level Signed-off-by: Takamasa Horibe * fix depend Signed-off-by: Takamasa Horibe * Update common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp Co-authored-by: Kosuke Takeuchi * Update common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp --------- Signed-off-by: Takamasa Horibe Co-authored-by: Kosuke Takeuchi * refactor(lane_change): add debug log (#5308) Signed-off-by: Takamasa Horibe * docs(logger_level_reconfigure): update readme (#5471) Signed-off-by: Takamasa Horibe * feat(localization): enable logging_level_configure (#5487) * feat(localization): enable logging_level_configure * style(pre-commit): autofix * update logger config Signed-off-by: kminoda * fix pre-commit Signed-off-by: kminoda * add tier4_autoware_utils in dependency Signed-off-by: kminoda * add tier4_autoware_utils in dependency Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * Logger level update (#5494) * address ordering Signed-off-by: Takamasa Horibe * add grouping Signed-off-by: Takamasa Horibe * remove unused comment Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe * feat(logging_level_configure_rviz_plugin): add autoware_util logger button (#5666) * feat(logging_level_configure_rviz_plugin): add autoware_util logger button Signed-off-by: Takamasa Horibe * add for control Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe * refactor(lane_change): standardizing lane change logger name (#5899) Signed-off-by: Zulfaqar Azmi * feat(tier4_logging_level_configure_rviz_plugin): add goal/start planner (#5978) Signed-off-by: kosuke55 * chore: set log level of debug printing in rviz plugin to DEBUG (#5996) Signed-off-by: Takayuki Murooka * feat(rviz_plugin): add string visualization plugin (#6100) Signed-off-by: satoshi-ota * feat(tier4_automatic_goal_rviz_plugin): make it possible to register checkpoints (#6153) Signed-off-by: satoshi-ota * chore(object_type_panel): update rosparam name config (#6347) Signed-off-by: Takamasa Horibe * style(update): autoware tools icons (#6351) Signed-off-by: KhalilSelyan * fix(readme): add acknowledgement for material icons in tool plugins (#6354) Signed-off-by: KhalilSelyan * feat(mission_planner_rviz_plugin): create mission planner tool (#6362) * feat(mission_planner_rviz_plugin): create package Signed-off-by: Takagi, Isamu * fix copyright Signed-off-by: Takagi, Isamu * add interrupted state Signed-off-by: Takagi, Isamu * use full license text instead of spdx Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu * fix(manual_controller): set PARK gear when going from reverse to drive (#6230) Signed-off-by: Maxime CLEMENT * feat(logger_level_configure): make it possible to change level of container logger (#6823) * feat(launch): add logging_demo::LoggerConfig into container Signed-off-by: satoshi-ota * fix(logger_level_reconfigure_plugin): fix yaml Signed-off-by: satoshi-ota * feat(logging_level_configure): add composable node Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota * revert: "feat(logger_level_configure): make it possible to change level of container logger (#6823)" (#6842) This reverts commit 9d045335d8e3763984bce8dea92f63de3423ebde. * docs(tier4_logging_level_configure_rviz_plugin): update document (#6720) * docs(tier4_logging_level_configure_rviz_plugin): update document Signed-off-by: Zulfaqar Azmi * fix spell check Signed-off-by: Zulfaqar Azmi * fix Warning Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi * refactor(bpp, avoidance): remove unnecessary verbose flag (#6822) * refactor(avoidance): logger small change Signed-off-by: satoshi-ota * refactor(bpp): remove verbose flag Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota * feat(tier4_screen_capture_panel): add new service to capture screen shot (#6867) * feat(tier4_screen_capture_panel): add new service to capture screen shot Signed-off-by: satoshi-ota * docs(tier4_screen_capture_rviz_plugin): update readme Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota * refactor(lane_change): fix logger (#6873) Signed-off-by: Zulfaqar Azmi * fix(route_handler): add logger (#6888) * fix(route_handler): add logger Signed-off-by: Muhammad Zulfaqar Azmi * fix indent Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi * docs(tier4_simulated_clock_rviz_plugin): update how to use (#6914) * docs(tier4_simulated_clock_rviz_plugin): update how to use Signed-off-by: Muhammad Zulfaqar Azmi * fixed tabbed warning Signed-off-by: Muhammad Zulfaqar Azmi * fix warning not working Signed-off-by: Muhammad Zulfaqar Azmi * Fix bullet list Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi * refactor(bpp): path shifter clang tidy and logging level configuration (#6917) Signed-off-by: Muhammad Zulfaqar Azmi * fix(accel_brake_calibrator): fix to set service name and exception failure (#6973) * add service * fix exception * fix style * refactor(motion_utils): supress log message with rclcpp logging (#6955) * refactor(motion_utils): supress log message with rclcpp logging Signed-off-by: Muhammad Zulfaqar Azmi * remove std string Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: tanaka3 Signed-off-by: Maxime CLEMENT Signed-off-by: Kenji Miyake Signed-off-by: Mamoru Sobue Signed-off-by: tomoya.kimura Signed-off-by: Takagi, Isamu Signed-off-by: h-ohta Signed-off-by: taikitanaka3 Signed-off-by: Takayuki Murooka Signed-off-by: Fumiya Watanabe Signed-off-by: Dawid Moszyński Signed-off-by: satoshi-ota Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: GitHub Signed-off-by: Mamoru Sobue Signed-off-by: kosuke55 Signed-off-by: Vincent Richard Signed-off-by: wep21 Signed-off-by: Takamasa Horibe Signed-off-by: Esteve Fernandez Signed-off-by: kminoda Signed-off-by: Zulfaqar Azmi Signed-off-by: KhalilSelyan Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka3 Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: awf-autoware-bot[bot] <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Tomoya Kimura Co-authored-by: Hiroki OTA Co-authored-by: Fumiya Watanabe Co-authored-by: Takayuki Murooka Co-authored-by: dmoszynski <121798334+dmoszynski@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Co-authored-by: Kosuke Takeuchi Co-authored-by: Vincent Richard Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> * update smoothing of start/end-point * Add simplified lateral acc limit * update debug plot * add gif * Update README.md * Update README.md * use ros param * ros param * update * update image * Update README.md * Add velocity noise * Update acc and steer noises * update rviz * update resource * Update README.md * Update README.md * Update control_data_collecting_tool/README.md Co-authored-by: Kosuke Takeuchi * update readme * Update bag2lanelet/scripts/bag2map.py Co-authored-by: Kosuke Takeuchi * modify sentence * chore(rtc_manager_panel): update module name (#29) * chore(rtc_manager_panel): update module name Signed-off-by: satoshi-ota * chore(gitignore): remove py cache Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota * improve pure pursuit * Update control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py Co-authored-by: Kosuke Takeuchi * remove deprecated linearized pure pursuit * clean up traj pub * Reduce speed for large lateral error * Improve how to obtain the trajectory closest point * update readme * rename variables * compute trajectory only when receive data collecting area info * smoothing target yaw * add yaw error threshold * remove rviz config * renew png and gif * add arrow on png * feat(driving_environment_analyzer): remove dependency to autoware_auto_tf2 (#31) Signed-off-by: Ryohsuke Mitsudome --------- Signed-off-by: Shintaro Sakoda Signed-off-by: satoshi-ota Signed-off-by: tanaka3 Signed-off-by: Maxime CLEMENT Signed-off-by: Kenji Miyake Signed-off-by: Mamoru Sobue Signed-off-by: tomoya.kimura Signed-off-by: Takagi, Isamu Signed-off-by: h-ohta Signed-off-by: taikitanaka3 Signed-off-by: Takayuki Murooka Signed-off-by: Fumiya Watanabe Signed-off-by: Dawid Moszyński Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: GitHub Signed-off-by: Mamoru Sobue Signed-off-by: kosuke55 Signed-off-by: Vincent Richard Signed-off-by: wep21 Signed-off-by: Takamasa Horibe Signed-off-by: Esteve Fernandez Signed-off-by: kminoda Signed-off-by: Zulfaqar Azmi Signed-off-by: KhalilSelyan Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Ryohsuke Mitsudome Co-authored-by: SakodaShintaro Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Mamoru Sobue Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka3 Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: awf-autoware-bot[bot] <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Tomoya Kimura Co-authored-by: Hiroki OTA Co-authored-by: Fumiya Watanabe Co-authored-by: Takayuki Murooka Co-authored-by: dmoszynski <121798334+dmoszynski@users.noreply.github.com> Co-authored-by: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Co-authored-by: Kosuke Takeuchi Co-authored-by: Vincent Richard Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * fix markdownlint in README Signed-off-by: Toru Hishinuma * add maitainers Signed-off-by: Toru Hishinuma * update msgs Signed-off-by: kosuke55 * add linearized pure pursuit control Signed-off-by: Toru Hishinuma * change default params Signed-off-by: Toru Hishinuma * add stop automatic driving Signed-off-by: Toru Hishinuma * update control cmd limit Signed-off-by: Toru Hishinuma * [bug fix] recompute trajectory when changing rosparam online Signed-off-by: Toru Hishinuma * fix dead link Signed-off-by: Toru Hishinuma * run pre-commit Signed-off-by: kosuke55 * fix spell Signed-off-by: kosuke55 * Update bag2lanelet/scripts/bag2map.py Co-authored-by: Kosuke Takeuchi --------- Signed-off-by: Shintaro Sakoda Signed-off-by: satoshi-ota Signed-off-by: tanaka3 Signed-off-by: Maxime CLEMENT Signed-off-by: Kenji Miyake Signed-off-by: Mamoru Sobue Signed-off-by: tomoya.kimura Signed-off-by: Takagi, Isamu Signed-off-by: h-ohta Signed-off-by: taikitanaka3 Signed-off-by: Takayuki Murooka Signed-off-by: Fumiya Watanabe Signed-off-by: Dawid Moszyński Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: GitHub Signed-off-by: Mamoru Sobue Signed-off-by: kosuke55 Signed-off-by: Vincent Richard Signed-off-by: wep21 Signed-off-by: Takamasa Horibe Signed-off-by: Esteve Fernandez Signed-off-by: kminoda Signed-off-by: Zulfaqar Azmi Signed-off-by: KhalilSelyan Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Ryohsuke Mitsudome Signed-off-by: Toru Hishinuma Co-authored-by: SakodaShintaro Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Mamoru Sobue Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka3 Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: awf-autoware-bot[bot] <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Tomoya Kimura Co-authored-by: Hiroki OTA Co-authored-by: Fumiya Watanabe Co-authored-by: Takayuki Murooka Co-authored-by: dmoszynski <121798334+dmoszynski@users.noreply.github.com> Co-authored-by: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Co-authored-by: Kosuke Takeuchi Co-authored-by: Vincent Richard Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: masayukiaino <101699734+masayukiaino@users.noreply.github.com> Signed-off-by: temkei.kem <1041084556@qq.com> * refresh cool down for setting initial pose in psim. Signed-off-by: temkei.kem <1041084556@qq.com> * style(pre-commit): autofix * parameterize publishing_speed_factor Signed-off-by: temkei.kem <1041084556@qq.com> --------- Signed-off-by: temkei.kem <1041084556@qq.com> Signed-off-by: satoshi-ota Signed-off-by: Takayuki Murooka Signed-off-by: Fumiya Watanabe Signed-off-by: kosuke55 Signed-off-by: Shintaro Sakoda Signed-off-by: tanaka3 Signed-off-by: Maxime CLEMENT Signed-off-by: Kenji Miyake Signed-off-by: Mamoru Sobue Signed-off-by: tomoya.kimura Signed-off-by: Takagi, Isamu Signed-off-by: h-ohta Signed-off-by: taikitanaka3 Signed-off-by: Dawid Moszyński Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: GitHub Signed-off-by: Mamoru Sobue Signed-off-by: Vincent Richard Signed-off-by: wep21 Signed-off-by: Takamasa Horibe Signed-off-by: Esteve Fernandez Signed-off-by: kminoda Signed-off-by: Zulfaqar Azmi Signed-off-by: KhalilSelyan Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Ryohsuke Mitsudome Signed-off-by: Toru Hishinuma Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: Fumiya Watanabe Co-authored-by: Kosuke Takeuchi Co-authored-by: Toru Hishinuma <92719065+th1991-01@users.noreply.github.com> Co-authored-by: SakodaShintaro Co-authored-by: Mamoru Sobue Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka3 Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: awf-autoware-bot[bot] <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Tomoya Kimura Co-authored-by: Hiroki OTA Co-authored-by: dmoszynski <121798334+dmoszynski@users.noreply.github.com> Co-authored-by: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Co-authored-by: Vincent Richard Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: masayukiaino <101699734+masayukiaino@users.noreply.github.com> Signed-off-by: temkei.kem <1041084556@qq.com> * fix a bug in converting old traffic liht message. (#70) Signed-off-by: temkei.kem <1041084556@qq.com> * refactored perception_replayer/reproducer. Signed-off-by: temkei.kem <1041084556@qq.com> * refactor perception_reproducer in order to speed up publishing speed. delete some unnessesary parameters. Signed-off-by: temkei.kem <1041084556@qq.com> * add noise to perception objects when repeat publishing. Signed-off-by: temkei.kem <1041084556@qq.com> * update speed constraint Signed-off-by: temkei.kem <1041084556@qq.com> * fix spell err Signed-off-by: temkei.kem <1041084556@qq.com> * style(pre-commit): autofix * fix spell err Signed-off-by: temkei.kem <1041084556@qq.com> * style(pre-commit): autofix --------- Signed-off-by: temkei.kem <1041084556@qq.com> Signed-off-by: satoshi-ota Signed-off-by: Takayuki Murooka Signed-off-by: Fumiya Watanabe Signed-off-by: kosuke55 Signed-off-by: Shintaro Sakoda Signed-off-by: tanaka3 Signed-off-by: Maxime CLEMENT Signed-off-by: Kenji Miyake Signed-off-by: Mamoru Sobue Signed-off-by: tomoya.kimura Signed-off-by: Takagi, Isamu Signed-off-by: h-ohta Signed-off-by: taikitanaka3 Signed-off-by: Dawid Moszyński Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> Signed-off-by: GitHub Signed-off-by: Mamoru Sobue Signed-off-by: Vincent Richard Signed-off-by: wep21 Signed-off-by: Takamasa Horibe Signed-off-by: Esteve Fernandez Signed-off-by: kminoda Signed-off-by: Zulfaqar Azmi Signed-off-by: KhalilSelyan Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Ryohsuke Mitsudome Signed-off-by: Toru Hishinuma Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: Fumiya Watanabe Co-authored-by: Kosuke Takeuchi Co-authored-by: Toru Hishinuma <92719065+th1991-01@users.noreply.github.com> Co-authored-by: SakodaShintaro Co-authored-by: Mamoru Sobue Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka3 Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: awf-autoware-bot[bot] <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Tomoya Kimura Co-authored-by: Hiroki OTA Co-authored-by: dmoszynski <121798334+dmoszynski@users.noreply.github.com> Co-authored-by: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Co-authored-by: Vincent Richard Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: masayukiaino <101699734+masayukiaino@users.noreply.github.com> --- .../perception_replayer.py | 6 +- .../perception_replayer_common.py | 4 +- .../perception_reproducer.py | 225 ++++++++++-------- 3 files changed, 132 insertions(+), 103 deletions(-) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index 597c58a8..0e3a78f6 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -84,8 +84,8 @@ def on_timer(self): if objects_msg: objects_msg.header.stamp = timestamp if self.args.detected_object: - if not self.ego_pose: - print("No ego pose found.") + if not self.ego_odom: + print("No ego odom found.") return ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) @@ -93,7 +93,7 @@ def on_timer(self): return log_ego_pose = ego_odom.pose.pose - translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) + translate_objects_coordinate(self.ego_odom.pose.pose, log_ego_pose, objects_msg) self.objects_pub.publish(objects_msg) # traffic signals diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index ce0d058f..10f413ba 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -49,7 +49,7 @@ def __init__(self, args, name): super().__init__(name) self.args = args - self.ego_pose = None + self.ego_odom = None self.rosbag_objects_data = [] self.rosbag_ego_odom_data = [] self.rosbag_traffic_signals_data = [] @@ -110,7 +110,7 @@ def __init__(self, args, name): time.sleep(1.0) def on_odom(self, odom): - self.ego_pose = odom.pose.pose + self.ego_odom = odom def load_rosbag(self, rosbag2_path: str): reader = open_reader(str(rosbag2_path)) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 9f733ee2..b096aa34 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -26,33 +26,15 @@ from utils import create_empty_pointcloud from utils import translate_objects_coordinate -dist_eps = 1e-3 # (meters) - class PerceptionReproducer(PerceptionReplayerCommon): def __init__(self, args): - self.rosbag_ego_odom_search_radius = ( - args.search_radius - ) # (m) the range of the ego odom to search, - self.ego_odom_search_radius = ( - self.rosbag_ego_odom_search_radius - ) # it may be set by an individual parameter. - - self.reproduce_cool_down = ( - args.reproduce_cool_down if args.search_radius != 0.0 else 0.0 - ) # (sec) the cool down time for republishing published data, please make sure that it's greater than the ego's stopping time. + self.rosbag_ego_odom_search_radius = args.search_radius + self.ego_odom_search_radius = self.rosbag_ego_odom_search_radius + self.reproduce_cool_down = args.reproduce_cool_down if args.search_radius != 0.0 else 0.0 super().__init__(args, "perception_reproducer") - self.reproduce_sequence_indices = deque() # contains ego_odom_idx - self.cool_down_indices = deque() # contains ego_odom_idx - self.ego_odom_id2last_published_timestamp = {} # for checking last published timestamp. - - self.prev_ego_pos = None - self.prev_ego_odom_msg = None - self.perv_objects_msg = None - self.prev_traffic_signals_msg = None - self.stopwatch = StopWatch(self.args.verbose) # for debug # refresh cool down for setting initial pose in psim. @@ -63,17 +45,27 @@ def __init__(self, args): # to make some data to accelerate computation self.preprocess_data() + self.reproduce_sequence_indices = deque() # contains ego_odom_idx + self.cool_down_indices = deque() # contains ego_odom_idx + self.ego_odom_id2last_published_timestamp = {} # for checking last published timestamp. + self.last_sequenced_ego_pose = None + + pose_timestamp, self.prev_ego_odom_msg = self.rosbag_ego_odom_data[0] + self.perv_objects_msg, self.prev_traffic_signals_msg = self.find_topics_by_timestamp( + pose_timestamp + ) + self.memorized_original_objects_msg = ( + self.memorized_noised_objects_msg + ) = self.perv_objects_msg + # start main timer callback - time_diffs = [] - prev_stamp = self.rosbag_ego_odom_data[0][0] - for stamp, _ in self.rosbag_ego_odom_data[1:]: - time_diff = (stamp - prev_stamp) / 1e9 - time_diffs.append(time_diff) - prev_stamp = stamp - - average_ego_odom_interval = sum(time_diffs) / len(time_diffs) - # slow down the publication speed. - average_ego_odom_interval *= args.publishing_speed_factor + + average_ego_odom_interval = np.mean( + [ + (self.rosbag_ego_odom_data[i][0] - self.rosbag_ego_odom_data[i - 1][0]) / 1e9 + for i in range(1, len(self.rosbag_ego_odom_data)) + ] + ) self.timer = self.create_timer(average_ego_odom_interval, self.on_timer) # kill perception process to avoid a conflict of the perception topics @@ -105,30 +97,34 @@ def on_timer(self): pointcloud_msg = create_empty_pointcloud(timestamp_msg) self.pointcloud_pub.publish(pointcloud_msg) - if not self.ego_pose: - print("No ego pose found.") + if not self.ego_odom: + print("No ego odom found.") return - # Update reproduce list if ego_pos is moved. - if ( - self.ego_pose is None - or self.prev_ego_pos is None - or ( - (self.ego_pose.position.x - self.prev_ego_pos.position.x) ** 2 - + (self.ego_pose.position.y - self.prev_ego_pos.position.y) ** 2 + ego_pose = self.ego_odom.pose.pose + dist_moved = ( + np.sqrt( + (ego_pose.position.x - self.last_sequenced_ego_pose.position.x) ** 2 + + (ego_pose.position.y - self.last_sequenced_ego_pose.position.y) ** 2 ) - > dist_eps**2 - ): + if self.last_sequenced_ego_pose + else 999 + ) + + # Update the reproduce sequence if the distance moved is greater than the search radius. + if dist_moved > self.ego_odom_search_radius: + self.last_sequenced_ego_pose = ego_pose + # find the nearest ego odom by simulation observation self.stopwatch.tic("find_nearest_ego_odom_by_observation") nearby_ego_odom_indies = self.find_nearby_ego_odom_indies( - [self.ego_pose], self.ego_odom_search_radius + [ego_pose], self.ego_odom_search_radius ) nearby_ego_odom_indies = [ self.rosbag_ego_odom_data[idx][1].pose.pose for idx in nearby_ego_odom_indies ] if not nearby_ego_odom_indies: - nearest_ego_odom_ind = self.find_nearest_ego_odom_index(self.ego_pose) + nearest_ego_odom_ind = self.find_nearest_ego_odom_index(ego_pose) nearby_ego_odom_indies += [ self.rosbag_ego_odom_data[nearest_ego_odom_ind][1].pose.pose ] @@ -157,78 +153,80 @@ def on_timer(self): ] ego_odom_indices = sorted(ego_odom_indices) self.reproduce_sequence_indices = deque(ego_odom_indices) - self.stopwatch.toc("update reproduce_sequence") - self.prev_ego_pos = self.ego_pose + if self.args.verbose: + print("reproduce_sequence_indices: ", list(self.reproduce_sequence_indices)[:20]) + + # Get messages + repeat_flag = len(self.reproduce_sequence_indices) == 0 - # get data to publish - repeat_trigger = len(self.reproduce_sequence_indices) == 0 - if not repeat_trigger: # pop data from reproduce_sequence if sequence is not empty. + # Add an additional constraint to avoid publishing too fast when there is a speed gap between the ego and the rosbag's ego when ego is departing/stopping while rosbag's ego is moving. + if not repeat_flag: + ego_speed = np.sqrt( + self.ego_odom.twist.twist.linear.x**2 + self.ego_odom.twist.twist.linear.y**2 + ) + ego_odom_idx = self.reproduce_sequence_indices[0] + _, ego_odom_msg = self.rosbag_ego_odom_data[ego_odom_idx] + ego_rosbag_speed = np.sqrt( + ego_odom_msg.twist.twist.linear.x**2 + ego_odom_msg.twist.twist.linear.y**2 + ) + + ego_rosbag_dist = np.sqrt( + (ego_pose.position.x - ego_odom_msg.pose.pose.position.x) ** 2 + + (ego_pose.position.y - ego_odom_msg.pose.pose.position.y) ** 2 + ) + repeat_flag = ego_rosbag_speed > ego_speed * 5 and ego_rosbag_dist > 1.0 + # set the speed threshold to many (5) times then ego_speed because this constraint is mainly for departing/stopping (ego speed is close to 0). + + if not repeat_flag: + self.stopwatch.tic("find_topics_by_timestamp") ego_odom_idx = self.reproduce_sequence_indices.popleft() + # extract messages by the nearest ego odom timestamp pose_timestamp, ego_odom_msg = self.rosbag_ego_odom_data[ego_odom_idx] - # extract message by the nearest ego odom timestamp - self.stopwatch.tic("find_topics_by_timestamp") objects_msg, traffic_signals_msg = self.find_topics_by_timestamp(pose_timestamp) self.stopwatch.toc("find_topics_by_timestamp") - # if self.args.verbose: - # print("reproduce_sequence_indices: ", list(self.reproduce_sequence_indices)[:20]) - - else: # get perv data to publish if reproduce_sequence is empty. + # update cool down info. + self.ego_odom_id2last_published_timestamp[ego_odom_idx] = timestamp + self.cool_down_indices.append(ego_odom_idx) + else: ego_odom_msg = self.prev_ego_odom_msg objects_msg = self.perv_objects_msg traffic_signals_msg = self.prev_traffic_signals_msg - # transform and publish current data. + # Transform and publish messages. self.stopwatch.tic("transform and publish") - # ego odom - self.recorded_ego_pub.publish(ego_odom_msg) - + if ego_odom_msg: + self.prev_ego_odom_msg = ego_odom_msg + self.recorded_ego_pub.publish(ego_odom_msg) # objects + objects_msg = objects_msg if objects_msg else self.perv_objects_msg if objects_msg: + self.perv_objects_msg = objects_msg objects_msg.header.stamp = timestamp_msg + + # add noise to repeat published objects + if repeat_flag and self.args.noise: + objects_msg = self.add_perception_noise(objects_msg) + if self.args.detected_object: - # copy the messages - self.stopwatch.tic("message deepcopy") - objects_msg_copied = pickle.loads( - pickle.dumps(objects_msg) - ) # this is x5 faster than deepcopy - self.stopwatch.toc("message deepcopy") - - log_ego_pose = ego_odom_msg.pose.pose - translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg_copied) - else: - objects_msg_copied = objects_msg - self.objects_pub.publish(objects_msg_copied) + objects_msg = self.copy_message(objects_msg) + translate_objects_coordinate(ego_pose, ego_odom_msg.pose.pose, objects_msg) + + self.objects_pub.publish(objects_msg) # traffic signals + traffic_signals_msg = ( + traffic_signals_msg if traffic_signals_msg else self.prev_traffic_signals_msg + ) if traffic_signals_msg: traffic_signals_msg.stamp = timestamp_msg - self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg - elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.stamp = timestamp_msg + self.traffic_signals_pub.publish(traffic_signals_msg) - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) self.stopwatch.toc("transform and publish") - if not repeat_trigger: - # save data for repeat publication. - self.prev_ego_odom_msg = ego_odom_msg - self.perv_objects_msg = ( - objects_msg if objects_msg is not None else self.perv_objects_msg - ) - self.prev_traffic_signals_msg = ( - traffic_signals_msg - if traffic_signals_msg is not None - else self.prev_traffic_signals_msg - ) - - # update cool down info. - self.ego_odom_id2last_published_timestamp[ego_odom_idx] = timestamp - self.cool_down_indices.append(ego_odom_idx) - self.stopwatch.toc("total on_timer") def find_nearest_ego_odom_index(self, ego_pose): @@ -247,10 +245,47 @@ def find_nearby_ego_odom_indies(self, ego_poses: list, search_radius: float): return nearby_indices + def copy_message(self, msg): + self.stopwatch.tic("message deepcopy") + objects_msg_copied = pickle.loads(pickle.dumps(msg)) # this is x5 faster than deepcopy + self.stopwatch.toc("message deepcopy") + return objects_msg_copied + + def add_perception_noise( + self, objects_msg, update_rate=0.03, x_noise_std=0.1, y_noise_std=0.05 + ): + if self.memorized_original_objects_msg != objects_msg: + self.memorized_noised_objects_msg = self.memorized_original_objects_msg = objects_msg + + if np.random.rand() < update_rate: + self.stopwatch.tic("add noise") + self.memorized_noised_objects_msg = self.copy_message( + self.memorized_original_objects_msg + ) + for obj in self.memorized_noised_objects_msg.objects: + noise_x = np.random.normal(0, x_noise_std) + noise_y = np.random.normal(0, y_noise_std) + if self.args.detected_object or self.args.tracked_object: + obj.kinematics.pose_with_covariance.pose.position.x += noise_x + obj.kinematics.pose_with_covariance.pose.position.y += noise_y + else: + obj.kinematics.initial_pose_with_covariance.pose.position.x += noise_x + obj.kinematics.initial_pose_with_covariance.pose.position.y += noise_y + self.stopwatch.toc("add noise") + + return self.memorized_noised_objects_msg + if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("-b", "--bag", help="rosbag", default=None) + parser.add_argument( + "-n", + "--noise", + help="apply perception noise to the objects when publishing repeated messages", + action="store_true", + default=True, + ) parser.add_argument( "-d", "--detected-object", help="publish detected object", action="store_true" ) @@ -273,16 +308,10 @@ def find_nearby_ego_odom_indies(self, ego_poses: list, search_radius: float): parser.add_argument( "-c", "--reproduce-cool-down", - help="The cool down time for republishing published messages (default is 80.0 seconds)", + help="The cool down time for republishing published messages (default is 80.0 seconds), please make sure that it's greater than the ego's stopping time.", type=float, default=80.0, ) - parser.add_argument( - "--publishing-speed-factor", - type=float, - default=1.2, - help="A factor to slow down the publication speed.", - ) args = parser.parse_args()