From f3859241b588377ffed5c915f4a4c4f5c9b23a7e Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 6 Dec 2021 19:38:37 +0900 Subject: [PATCH] feat: add simple planning simulator package (#5) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * add sample ros2 packages Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Fix simple planning simulator (#26) * simple planning simulator: fix params & launch file Signed-off-by: Takamasa Horibe * remove unused file Signed-off-by: Takamasa Horibe * fix timercallback Signed-off-by: Takamasa Horibe * [simple_planning_simulator] add rostopic relay in launch file (#117) * [simple_planning_simulator] add rostopic relay in launch file Signed-off-by: mitsudome-r * add topic_tools as exec_depend Signed-off-by: mitsudome-r * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * reduce terminal ouput for better error message visibility (#200) * reduce terminal ouput for better error message visibility Signed-off-by: mitsudome-r * [costmap_generator] fix waiting for first transform Signed-off-by: mitsudome-r * fix tests Signed-off-by: mitsudome-r * fix test Signed-off-by: mitsudome-r * Use trajectory for z position source (#243) Signed-off-by: wep21 * Ros2 v0.8.0 engage (#342) * [autoware_vehicle_msgs]: Add engage message Signed-off-by: wep21 * [as]: Update message Signed-off-by: wep21 * [awapi_awiv_adapter]: Update message Signed-off-by: wep21 * [web_controller]: Update message Signed-off-by: wep21 * [vehicle_cmd_gate]: Update message Signed-off-by: wep21 * [autoware_state_monitor]: Update message Signed-off-by: wep21 * [autoware_control_msgs]: Remove EngageMode message Signed-off-by: wep21 * [simple_planning_simulator]: Update message Signed-off-by: wep21 * Ros2 v0.8.0 fix packages (#351) * add subscription to QoS * add vihicle_param _file to simple_planning_sim * update cmake/packages.xml * comment out unused parameter * apply lint * add vehicle_info_util to lane_change_planner * add vehicle_info_util to vehicle_cmd_gate * fix cmake of simple planning simulator * update cmake/packages.xml of vehicle cmd gate * apply lint * apply lint * add latch option to autoware_state_monitor * delete unused comment * Rename ROS-related .yaml to .param.yaml (#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake * Fix overlooked parameters Signed-off-by: Kenji Miyake * Fix typo in simulator module (#439) * add use_sim-time option (#454) * Format launch files (#1219) Signed-off-by: Kenji Miyake * Fix rolling build errors (#1225) * Add missing include files Signed-off-by: Kenji Miyake * Replace rclcpp::Duration Signed-off-by: Kenji Miyake * Use reference for exceptions Signed-off-by: Kenji Miyake * Use from_seconds Signed-off-by: Kenji Miyake * Sync public repo (#1228) * [simple_planning_simulator] add readme (#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (#460) * Fix pose2twist (#462) Signed-off-by: Takagi, Isamu * Ros2 vehicle info param server (#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix issues in hdd_reader (#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (#463) * Rviz overlay render fix (#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * back to RTD as superclass Signed-off-by: Adam Dabrowski * Rviz overlay render in update (#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski * restored RTD superclass Signed-off-by: Adam Dabrowski Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Refactor vehicle info util (#1305) * Update license Signed-off-by: Kenji Miyake * Refactor vehicle_info_util Signed-off-by: Kenji Miyake * Rename and split files Signed-off-by: Kenji Miyake * Fix interfaces Signed-off-by: Kenji Miyake * Fix bug and add error handling Signed-off-by: Kenji Miyake * Add "// namespace" Signed-off-by: Kenji Miyake * Add missing include Signed-off-by: Kenji Miyake * Fix lint errors (#1378) * Fix lint errors Signed-off-by: Kenji Miyake * Fix variable names Signed-off-by: Kenji Miyake * Add pre-commit (#1560) * add pre-commit * add pre-commit-config * add additional settings for private repository * use default pre-commit-config * update pre-commit setting * Ignore whitespace for line breaks in markdown * Update .github/workflows/pre-commit.yml Co-authored-by: Kazuki Miyahara * exclude svg * remove pretty-format-json * add double-quote-string-fixer * consider COLCON_IGNORE file when seaching modified package * format file * pre-commit fixes * Update pre-commit.yml * Update .pre-commit-config.yaml Co-authored-by: Kazuki Miyahara Co-authored-by: pre-commit Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Add markdownlint and prettier (#1661) * Add markdownlint and prettier Signed-off-by: Kenji Miyake * Ignore .param.yaml Signed-off-by: Kenji Miyake * Apply format Signed-off-by: Kenji Miyake * add cov pub in psim (#1732) * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * fix some typos (#1941) * fix some typos * fix typo * Fix typo Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake * Add autoware api (#1979) * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Feature/add ideal accel model interface (#1894) * Add IDEAL_ACCEL model interface for simple planning simulator Signed-off-by: Makoto Kurihara * Add IDEAL_ACCEL model descriptions Signed-off-by: Makoto Kurihara * Fix format Signed-off-by: Makoto Kurihara * Change vehicle model type description at config file Signed-off-by: Makoto Kurihara * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * Back port .auto control packages (#571) * Implement Lateral and Longitudinal Control Muxer * [#570] Porting wf_simulator * [#1189] Deactivate flaky test in 'trajectory_follower_nodes' * [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer' * [#1057] Add osqp_interface package * [#1057] Add library code for MPC-based lateral control * [#1271] Use std::abs instead of abs * [#1057] Implement Lateral Controller for Cargo ODD * [#1246] Resolve "Test case names currently use snake_case but should be CamelCase" * [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes' * [#1058] Add library code of longitudinal controller * Fix build error for trajectory follower Signed-off-by: wep21 * Fix build error for trajectory follower nodes Signed-off-by: wep21 * [#1272] Add AckermannControlCommand support to simple_planning_simulator * [#1058] Add Longitudinal Controller node * [#1058] Rename velocity_controller -> longitudinal_controller * [#1058] Update CMakeLists.txt for the longitudinal_controller_node * [#1058] Add smoke test python launch file * [#1058] Use LowPassFilter1d from trajectory_follower * [#1058] Use autoware_auto_msgs * [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener) * [#1058] Remove unused parameters * [#1058] Fix ros test * [#1058] Rm default params from declare_parameters + use autoware types * [#1058] Use default param file to setup NodeOptions in the ros test * [#1058] Fix docstring * [#1058] Replace receiving a Twist with a VehicleKinematicState * [#1058] Change class variables format to m_ prefix * [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt * [#1058] Fix copyright dates * [#1058] Reorder includes * [#1058] Add some tests (~89% coverage without disabling flaky tests) * [#1058] Add more tests (90+% coverage without disabling flaky tests) * [#1058] Use Float32MultiArrayDiagnostic message for debug and slope * [#1058] Calculate wheel_base value from vehicle parameters * [#1058] Cleanup redundant logger setting in tests * [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures * [#1058] Remove TF listener and use published vehicle state instead * [#1058] Change smoke tests to use autoware_testing * [#1058] Add plotjuggler cfg for both lateral and longitudinal control * [#1058] Improve design documents * [#1058] Disable flaky test * [#1058] Properly transform vehicle state in longitudinal node * [#1058] Fix TF buffer of lateral controller * [#1058] Tuning of lateral controller for LGSVL * [#1058] Fix formating * [#1058] Fix /tf_static sub to be transient_local * [#1058] Fix yaw recalculation of reverse trajs in the lateral controller * modify trajectory_follower for galactic build Signed-off-by: Takamasa Horibe * [#1379] Update trajectory_follower * [#1379] Update simple_planning_simulator * [#1379] Update trajectory_follower_nodes * apply trajectory msg modification in control Signed-off-by: Takamasa Horibe * move directory Signed-off-by: Takamasa Horibe * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe * remove .iv trajectory follower Signed-off-by: Takamasa Horibe * use .auto trajectory_follower Signed-off-by: Takamasa Horibe * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe * apply .auto message split Signed-off-by: Takamasa Horibe * fix build depend Signed-off-by: Takamasa Horibe * fix packages using osqp * fix autoware_auto_geometry * ignore lint of some packages * ignore ament_lint of some packages * ignore lint/pre-commit of trajectory_follower_nodes * disable unit tests of some packages Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura * [simple planning simulator]change type of msg (#590) * remove kinematic_state * remove vehicle_state_command/report * get z-position from trajectory * set topic name of trajectory * twist -> velocity report * change default param * Update simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * Update simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix typo Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * [autoware_vehicle_rviz_plugin/route_handler/simple_planning_simulator]fix some packages (#606) * fix console meter * fix velocity_history * fix route handler * change topic name * update to support velocity report header (#655) * update to support velocity report header Signed-off-by: Takamasa Horibe * Update simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp Co-authored-by: tkimura4 * use maybe_unused Signed-off-by: Takamasa Horibe * fix precommit Signed-off-by: Takamasa Horibe Co-authored-by: tkimura4 * adapt to actuation cmd/status as control msg (#646) * adapt to actuation cmd/status as control msg * fix readme * fix topics * fix remaing topics * as to pacmod interface * fix vehicle status * add header to twist * revert gyro_odometer_change * revert twist topic change * revert unchanged package * FIx vehicle status topic name/type (#658) * shift -> gear_status * twist -> velocity_status * fix topic name (#674) * fix topic name Signed-off-by: Takamasa Horibe * fix gear message name Signed-off-by: Takamasa Horibe * Fix psim param path (#696) * Fix/psim topics emergency handler awapi (#702) * fix emergency handler * fix awapi * remove unused topic * remove duplecated vehicle cmd * Auto/add turn indicators and hazards (#717) * add turn indicators * add hazard light * omit name space * remap topic name * delete unnecessary blank line * [simple_planning_simulator]fix bug (#727) * input z-axis of trajectory to pose(tf/odometry) * output 0 velocity when invalid gear is input * fix gear process in sim (#728) * Fix for integration test (#732) * Add backward compatibility of autoware state * Add simulator initial pose service * Fix pre-commit * Fix pre-commit * Simple planning simulator update for latest develop (#735) * Refactor vehicle info util (#1305) Signed-off-by: Takamasa Horibe * add cov pub in psim (#1732) Signed-off-by: Takamasa Horibe * remove pose_with_covariance publisher and add covariance information in Odometry Signed-off-by: Takamasa Horibe * Fix acceleration for reverse (#737) * Fix acceleration for reverse * Fix acceleration in set_input * remove unused using * Fix code * ci(pre-commit): autofix * remove tests Co-authored-by: mitsudome-r Co-authored-by: Takamasa Horibe Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Nikolai Morin Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit Co-authored-by: Kosuke Murakami Co-authored-by: Hiroki OTA Co-authored-by: Kenji Miyake Co-authored-by: Makoto Kurihara Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autoware_api_utils/rclcpp/client.hpp | 6 +- .../autoware_api_utils/rclcpp/service.hpp | 6 +- .../simple_planning_simulator/CMakeLists.txt | 44 ++ .../simple_planning_simulator-design.md | 122 ++++ .../simple_planning_simulator_core.hpp | 337 +++++++++++ .../vehicle_model/sim_model.hpp | 26 + .../sim_model_delay_steer_acc.hpp | 145 +++++ .../sim_model_delay_steer_acc_geared.hpp | 152 +++++ .../sim_model_ideal_steer_acc.hpp | 113 ++++ .../sim_model_ideal_steer_acc_geared.hpp | 121 ++++ .../sim_model_ideal_steer_vel.hpp | 113 ++++ .../vehicle_model/sim_model_interface.hpp | 163 ++++++ .../visibility_control.hpp | 40 ++ .../simple_planning_simulator.launch.py | 81 +++ .../simple_planning_simulator/package.xml | 38 ++ ...mple_planning_simulator_default.param.yaml | 21 + .../param/vehicle_characteristics.param.yaml | 12 + .../simple_planning_simulator_core.cpp | 547 ++++++++++++++++++ .../sim_model_delay_steer_acc.cpp | 99 ++++ .../sim_model_delay_steer_acc_geared.cpp | 139 +++++ .../sim_model_ideal_steer_acc.cpp | 52 ++ .../sim_model_ideal_steer_acc_geared.cpp | 90 +++ .../sim_model_ideal_steer_vel.cpp | 52 ++ .../vehicle_model/sim_model_interface.cpp | 41 ++ .../test/test_simple_planning_simulator.cpp | 386 ++++++++++++ 25 files changed, 2938 insertions(+), 8 deletions(-) create mode 100644 simulator/simple_planning_simulator/CMakeLists.txt create mode 100644 simulator/simple_planning_simulator/design/simple_planning_simulator-design.md create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp create mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py create mode 100644 simulator/simple_planning_simulator/package.xml create mode 100644 simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml create mode 100644 simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp create mode 100644 simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp diff --git a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp index ebe6204b38d6a..ed2716df5d31e 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp +++ b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp @@ -41,8 +41,7 @@ class Client const typename ServiceT::Request::SharedPtr & request, const std::chrono::nanoseconds & timeout = std::chrono::seconds(2)) { - RCLCPP_INFO( - logger_, "client request: \n%s", rosidl_generator_traits::to_yaml(*request).c_str()); + RCLCPP_INFO(logger_, "client request"); if (!client_->service_is_ready()) { RCLCPP_INFO(logger_, "client available"); @@ -55,8 +54,7 @@ class Client return {response_error("Internal service has timed out."), nullptr}; } - RCLCPP_INFO( - logger_, "client response: \n%s", rosidl_generator_traits::to_yaml(future.get()).c_str()); + RCLCPP_INFO(logger_, "client response"); return {response_success(), future.get()}; } diff --git a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp index 08095bc8751ea..39d11b19fda68 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp +++ b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp @@ -33,11 +33,9 @@ class Service auto wrapped_callback = [logger, callback]( typename ServiceT::Request::SharedPtr request, typename ServiceT::Response::SharedPtr response) { - RCLCPP_INFO( - logger, "service request: \n%s", rosidl_generator_traits::to_yaml(*request).c_str()); + RCLCPP_INFO(logger, "service request"); callback(request, response); - RCLCPP_INFO( - logger, "service response: \n%s", rosidl_generator_traits::to_yaml(*response).c_str()); + RCLCPP_INFO(logger, "service response"); }; return wrapped_callback; } diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt new file mode 100644 index 0000000000000..e217ca572e583 --- /dev/null +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.6) + +project(simple_planning_simulator) + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +# Component +ament_auto_add_library(${PROJECT_NAME} SHARED + include/simple_planning_simulator/simple_planning_simulator_core.hpp + include/simple_planning_simulator/visibility_control.hpp + src/simple_planning_simulator/simple_planning_simulator_core.cpp + src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp + src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp + src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp + src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp + src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp + src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +) +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) +autoware_set_compile_options(${PROJECT_NAME}) + +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. + + +# Node executable +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" + EXECUTABLE ${PROJECT_NAME}_exe +) + + +### Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + + + + +ament_auto_package(INSTALL_TO_SHARE param launch) diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md new file mode 100644 index 0000000000000..a2e34cf6c7cc6 --- /dev/null +++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md @@ -0,0 +1,122 @@ +simple_planning_simulator {#simple_planning_simulator-package-design} +=========== + + +# Purpose / Use cases + +This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model. + + + +# Design + + +The purpose of this simulator is for the integration test of planning and control modules. This does not simulate sensing or perception, but is implemented in pure c++ only and works without GPU. + +## Assumptions / Known limits + + - It simulates only in 2D motion. + - It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics. + + +## Inputs / Outputs / API + + +**input** + - input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle. + - input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle. + - input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear). + - /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose + +**output** + - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) + - /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM) + - /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.) + + +## Inner-workings / Algorithms + +### Common Parameters + +|Name|Type|Description|Default value| +|:---|:---|:---|:---| +|simulated_frame_id | string | set to the child_frame_id in output tf |"base_link"| +|origin_frame_id | string | set to the frame_id in output tf |"odom"| +|initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" | +|add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results.| true| +|pos_noise_stddev | double | Standard deviation for position noise | 0.01| +|rpy_noise_stddev | double | Standard deviation for Euler angle noise| 0.0001| +|vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0| +|angvel_noise_stddev | double | Standard deviation for angular velocity noise| 0.0| +|steer_noise_stddev | double | Standard deviation for steering angle noise| 0.0001| + + + +### Vehicle Model Parameters + + +**vehicle_model_type options** + + - `IDEAL_STEER_VEL` + - `IDEAL_STEER_ACC` + - `IDEAL_STEER_ACC_GEARED` + - `DELAY_STEER_ACC` + - `DELAY_STEER_ACC_GEARED` + +The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. + +The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). + + +|Name|Type|Description|I_ST_V|I_ST_A|I_ST_A_G|D_ST_A|D_ST_A_G|Default value| unit | +|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---| +|acc_time_delay | double | dead time for the acceleration input | x | x | x | o | o | 0.1 | [s] | +|steer_time_delay | double | dead time for the steering input | x | x | x | o | o | 0.24| [s] | +|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | o | 0.1 | [s] | +|steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | 0.27| [s] | +|vel_lim | double | limit of velocity | x | x | x | o | o | 50.0| [m/s] | +|vel_rate_lim | double | limit of acceleration | x | x | x | o | o | 7.0 | [m/ss] | +|steer_lim | double | limit of steering angle | x | x | x | o | o | 1.0 | [rad] | +|steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | 5.0 | [rad/s] | + + + +*Note*: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a *delay* model. The definition of the *time constant* is the time it takes for the step response to rise up to 63% of its final value. The *deadtime* is a delay in the response to a control input. + + +### Default TF configuration + +Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. +In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. + + +## Error detection and handling + +The only validation on inputs being done is testing for a valid vehicle model type. + + +# Security considerations + + + + +# References / External links +This is originally developed in the Autoware.AI. See the link below. + +https://github.com/Autoware-AI/simulation/tree/master/wf_simulator + +# Future extensions / Unimplemented parts + + - Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior) + - Cooperation with modules that output pseudo pointcloud or pseudo perception results + + +# Related issues + + - #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp new file mode 100644 index 0000000000000..5cfa6371eee66 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -0,0 +1,337 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ + + +#include +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "simple_planning_simulator/visibility_control.hpp" + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_auto_geometry_msgs/msg/complex32.hpp" +#include "common/types.hpp" + +#include "autoware_api_utils/autoware_api_utils.hpp" +#include "autoware_external_api_msgs/srv/initialize_pose.hpp" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + + +namespace simulation +{ +namespace simple_planning_simulator +{ +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; + +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_geometry_msgs::msg::Complex32; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_vehicle_msgs::msg::ControlModeReport; +using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_auto_vehicle_msgs::msg::GearReport; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; +using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_auto_vehicle_msgs::msg::HazardLightsReport; +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_external_api_msgs::srv::InitializePose; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::PoseWithCovarianceStamped; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; + +class DeltaTime +{ +public: + DeltaTime() + : prev_updated_time_ptr_(nullptr) {} + float64_t get_dt(const rclcpp::Time & now) + { + if (prev_updated_time_ptr_ == nullptr) { + prev_updated_time_ptr_ = std::make_shared(now); + return 0.0; + } + const float64_t dt = (now - *prev_updated_time_ptr_).seconds(); + *prev_updated_time_ptr_ = now; + return dt; + } + +private: + std::shared_ptr prev_updated_time_ptr_; +}; + +class MeasurementNoiseGenerator +{ +public: + MeasurementNoiseGenerator() {} + + std::shared_ptr rand_engine_; + std::shared_ptr> pos_dist_; + std::shared_ptr> vel_dist_; + std::shared_ptr> rpy_dist_; + std::shared_ptr> steer_dist_; +}; + +class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node +{ +public: + explicit SimplePlanningSimulator(const rclcpp::NodeOptions & options); + +private: + /* ros system */ + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_steer_; + rclcpp::Publisher::SharedPtr pub_control_mode_report_; + rclcpp::Publisher::SharedPtr pub_gear_report_; + rclcpp::Publisher::SharedPtr pub_turn_indicators_report_; + rclcpp::Publisher::SharedPtr pub_hazard_lights_report_; + rclcpp::Publisher::SharedPtr pub_tf_; + rclcpp::Publisher::SharedPtr pub_current_pose_; + + rclcpp::Subscription::SharedPtr sub_gear_cmd_; + rclcpp::Subscription::SharedPtr sub_turn_indicators_cmd_; + rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_; + rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; + rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_init_pose_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + rclcpp::CallbackGroup::SharedPtr group_api_service_; + autoware_api_utils::Service::SharedPtr srv_set_pose_; + + uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time + rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation + + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult on_parameter( + const std::vector & parameters); + + /* tf */ + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + /* received & published topics */ + VelocityReport current_velocity_; + Odometry current_odometry_; + SteeringReport current_steer_; + VehicleControlCommand::ConstSharedPtr current_vehicle_cmd_ptr_; + AckermannControlCommand::ConstSharedPtr current_ackermann_cmd_ptr_; + GearCommand::ConstSharedPtr current_gear_cmd_ptr_; + TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_; + HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_; + Trajectory::ConstSharedPtr current_trajectory_ptr_; + + /* frame_id */ + std::string simulated_frame_id_; //!< @brief simulated vehicle frame id + std::string origin_frame_id_; //!< @brief map frame_id + + /* flags */ + bool8_t is_initialized_; //!< @brief flag to check the initial position is set + bool8_t add_measurement_noise_; //!< @brief flag to add measurement noise + + DeltaTime delta_time_; //!< @brief to calculate delta time + + MeasurementNoiseGenerator measurement_noise_; //!< @brief for measurement noise + + float64_t x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate + float64_t y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate + + /* vehicle model */ + enum class VehicleModelType + { + IDEAL_STEER_ACC = 0, + IDEAL_STEER_ACC_GEARED = 1, + DELAY_STEER_ACC = 2, + DELAY_STEER_ACC_GEARED = 3, + IDEAL_STEER_VEL = 4, + } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics + std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer + + /** + * @brief set current_vehicle_cmd_ptr_ with received message + */ + void on_vehicle_cmd(const VehicleControlCommand::ConstSharedPtr msg); + + /** + * @brief set current_ackermann_cmd_ptr_ with received message + */ + void on_ackermann_cmd(const AckermannControlCommand::ConstSharedPtr msg); + + /** + * @brief set input steering, velocity, and acceleration of the vehicle model + */ + void set_input(const float steer, const float vel, const float accel); + + /** + * @brief set current_vehicle_state_ with received message + */ + void on_gear_cmd(const GearCommand::ConstSharedPtr msg); + + /** + * @brief set current_vehicle_state_ with received message + */ + void on_turn_indicators_cmd(const TurnIndicatorsCommand::ConstSharedPtr msg); + + /** + * @brief set current_vehicle_state_ with received message + */ + void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg); + + /** + * @brief set initial pose for simulation with received message + */ + void on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg); + + /** + * @brief set initial pose for simulation with received request + */ + void on_set_pose( + const InitializePose::Request::ConstSharedPtr request, + const InitializePose::Response::SharedPtr response); + + /** + * @brief subscribe trajectory for deciding self z position. + */ + void on_trajectory(const Trajectory::ConstSharedPtr msg); + + /** + * @brief get z-position from trajectory + * @param [in] x current x-position + * @param [in] y current y-position + * @return get z-position from trajectory + */ + double get_z_pose_from_trajectory(const double x, const double y); + + /** + * @brief get transform from two frame_ids + * @param [in] parent_frame parent frame id + * @param [in] child_frame child frame id + * @return transform from parent frame to child frame + */ + TransformStamped get_transform_msg(const std::string parent_frame, const std::string child_frame); + + /** + * @brief timer callback for simulation with loop_rate + */ + void on_timer(); + + /** + * @brief initialize vehicle_model_ptr + */ + void initialize_vehicle_model(); + + /** + * @brief add measurement noise + * @param [in] odometry odometry to add noise + * @param [in] vel velocity report to add noise + * @param [in] steer steering to add noise + */ + void add_measurement_noise(Odometry & odom, VelocityReport & vel, SteeringReport & steer) const; + + /** + * @brief set initial state of simulated vehicle + * @param [in] pose initial position and orientation + * @param [in] twist initial velocity and angular velocity + */ + void set_initial_state(const Pose & pose, const Twist & twist); + + /** + * @brief set initial state of simulated vehicle with pose transformation based on frame_id + * @param [in] pose initial position and orientation with header + * @param [in] twist initial velocity and angular velocity + */ + void set_initial_state_with_transform(const PoseStamped & pose, const Twist & twist); + + /** + * @brief publish velocity + * @param [in] velocity The velocity report to publish + */ + void publish_velocity(const VelocityReport & velocity); + + /** + * @brief publish pose and twist + * @param [in] odometry The odometry to publish + */ + void publish_odometry(const Odometry & odometry); + + /** + * @brief publish steering + * @param [in] steer The steering to publish + */ + void publish_steering(const SteeringReport & steer); + + /** + * @brief publish control_mode report + */ + void publish_control_mode_report(); + + /** + * @brief publish gear report + */ + void publish_gear_report(); + + /** + * @brief publish turn indicators report + */ + void publish_turn_indicators_report(); + + /** + * @brief publish hazard lights report + */ + void publish_hazard_lights_report(); + + /** + * @brief publish tf + * @param [in] state The kinematic state to publish as a TF + */ + void publish_tf(const Odometry & odometry); +}; +} // namespace simple_planning_simulator +} // namespace simulation + +#endif // SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp new file mode 100644 index 0000000000000..ef72b9e1f2c2c --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -0,0 +1,26 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" + + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp new file mode 100644 index 0000000000000..f0a610c54eb0f --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -0,0 +1,145 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/LU" +#include "eigen3/Eigen/Core" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +class SimModelDelaySteerAcc : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] acc_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + */ + SimModelDelaySteerAcc( + float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, + float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant); + + /** + * @brief default destructor + */ + ~SimModelDelaySteerAcc() = default; + +private: + const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX + { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U + { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + const float64_t vx_lim_; //!< @brief velocity limit [m/s] + const float64_t vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const float64_t steer_lim_; //!< @brief steering limit [rad] + const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const float64_t acc_delay_; //!< @brief time delay for accel command [s] + const float64_t acc_time_constant_; //!< @brief time constant for accel dynamics + const float64_t steer_delay_; //!< @brief time delay for steering command [s] + const float64_t steer_time_constant_; //!< @brief time constant for steering dynamics + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const float64_t & dt); + + /** + * @brief get vehicle position x + */ + float64_t getX() override; + + /** + * @brief get vehicle position y + */ + float64_t getY() override; + + /** + * @brief get vehicle angle yaw + */ + float64_t getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + float64_t getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal acceleration + */ + float64_t getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + float64_t getWz() override; + + /** + * @brief get vehicle steering angle + */ + float64_t getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const float64_t & dt) override; + + /** + * @brief calculate derivative of states with time delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp new file mode 100644 index 0000000000000..c9bcf1bb32e6f --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -0,0 +1,152 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/LU" +#include "eigen3/Eigen/Core" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +class SimModelDelaySteerAccGeared : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] acc_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + */ + SimModelDelaySteerAccGeared( + float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, + float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant); + + /** + * @brief default destructor + */ + ~SimModelDelaySteerAccGeared() = default; + +private: + const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX + { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U + { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + const float64_t vx_lim_; //!< @brief velocity limit [m/s] + const float64_t vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const float64_t steer_lim_; //!< @brief steering limit [rad] + const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const float64_t acc_delay_; //!< @brief time delay for accel command [s] + const float64_t acc_time_constant_; //!< @brief time constant for accel dynamics + const float64_t steer_delay_; //!< @brief time delay for steering command [s] + const float64_t steer_time_constant_; //!< @brief time constant for steering dynamics + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const float64_t & dt); + + /** + * @brief get vehicle position x + */ + float64_t getX() override; + + /** + * @brief get vehicle position y + */ + float64_t getY() override; + + /** + * @brief get vehicle angle yaw + */ + float64_t getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + float64_t getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal acceleration + */ + float64_t getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + float64_t getWz() override; + + /** + * @brief get vehicle steering angle + */ + float64_t getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const float64_t & dt) override; + + /** + * @brief calculate derivative of states with time delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; + + /** + * @brief calculate velocity with considering current velocity and gear + * @param [in] state current state + * @param [in] gear current gear (defined in autoware_auto_msgs/VehicleStateCommand) + */ + float64_t calcVelocityWithGear(const Eigen::VectorXd & state, const uint8_t gear) const; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp new file mode 100644 index 0000000000000..c4aca56e5bd51 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp @@ -0,0 +1,113 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +/** + * @class SimModelIdealSteerAcc + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteerAcc : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + explicit SimModelIdealSteerAcc(float64_t wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteerAcc() = default; + +private: + enum IDX + { + X = 0, + Y, + YAW, + VX + }; + enum IDX_U + { + AX_DES = 0, + STEER_DES, + }; + + const float64_t wheelbase_; //!< @brief vehicle wheelbase length + + /** + * @brief get vehicle position x + */ + float64_t getX() override; + + /** + * @brief get vehicle position y + */ + float64_t getY() override; + + /** + * @brief get vehicle angle yaw + */ + float64_t getYaw() override; + + /** + * @brief get vehicle longitudinal velocity + */ + float64_t getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal acceleration + */ + float64_t getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + float64_t getWz() override; + + /** + * @brief get vehicle steering angle + */ + float64_t getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const float64_t & dt) override; + + /** + * @brief calculate derivative of states with ideal steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp new file mode 100644 index 0000000000000..8b74c5f1baf19 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -0,0 +1,121 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +/** + * @class SimModelIdealSteerAccGeared + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteerAccGeared : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + explicit SimModelIdealSteerAccGeared(float64_t wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteerAccGeared() = default; + +private: + enum IDX + { + X = 0, + Y, + YAW, + VX + }; + enum IDX_U + { + AX_DES = 0, + STEER_DES, + }; + + const float64_t wheelbase_; //!< @brief vehicle wheelbase length + float64_t current_acc_; //!< @brief current_acc with gear consideration + + /** + * @brief get vehicle position x + */ + float64_t getX() override; + + /** + * @brief get vehicle position y + */ + float64_t getY() override; + + /** + * @brief get vehicle angle yaw + */ + float64_t getYaw() override; + + /** + * @brief get vehicle longitudinal velocity + */ + float64_t getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal acceleration + */ + float64_t getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + float64_t getWz() override; + + /** + * @brief get vehicle steering angle + */ + float64_t getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const float64_t & dt) override; + + /** + * @brief calculate derivative of states with ideal steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; + + /** + * @brief calculate velocity with considering current velocity and gear + * @param [in] state current state + * @param [in] gear current gear (defined in autoware_auto_msgs/VehicleStateCommand) + */ + float64_t calcVelocityWithGear(const Eigen::VectorXd & state, const uint8_t gear) const; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp new file mode 100644 index 0000000000000..dae5f0fab633c --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp @@ -0,0 +1,113 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +/** + * @class SimModelIdealSteerVel + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteerVel : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + explicit SimModelIdealSteerVel(float64_t wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteerVel() = default; + +private: + enum IDX + { + X = 0, + Y, + YAW + }; + enum IDX_U + { + VX_DES = 0, + STEER_DES, + }; + + const float64_t wheelbase_; //!< @brief vehicle wheelbase length + float64_t prev_vx_ = 0.0; + float64_t current_ax_ = 0.0; + + /** + * @brief get vehicle position x + */ + float64_t getX() override; + + /** + * @brief get vehicle position y + */ + float64_t getY() override; + + /** + * @brief get vehicle angle yaw + */ + float64_t getYaw() override; + + /** + * @brief get vehicle longitudinal velocity + */ + float64_t getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal acceleration + */ + float64_t getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + float64_t getWz() override; + + /** + * @brief get vehicle steering angle + */ + float64_t getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const float64_t & dt) override; + + /** + * @brief calculate derivative of states with ideal steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp new file mode 100644 index 0000000000000..bdcafdda14de5 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -0,0 +1,163 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ + +#include "eigen3/Eigen/Core" +#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" +#include "common/types.hpp" + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; + +/** + * @class SimModelInterface + * @brief simple_planning_simulator vehicle model class to calculate vehicle dynamics + */ +class SimModelInterface +{ +protected: + const int dim_x_; //!< @brief dimension of state x + const int dim_u_; //!< @brief dimension of input u + Eigen::VectorXd state_; //!< @brief vehicle state vector + Eigen::VectorXd input_; //!< @brief vehicle input vector + + //!< @brief gear command defined in autoware_auto_msgs/VehicleStateCommand + uint8_t gear_ = autoware_auto_vehicle_msgs::msg::VehicleStateCommand::GEAR_DRIVE; + +public: + /** + * @brief constructor + * @param [in] dim_x dimension of state x + * @param [in] dim_u dimension of input u + */ + SimModelInterface(int dim_x, int dim_u); + + /** + * @brief destructor + */ + ~SimModelInterface() = default; + + /** + * @brief get state vector of model + * @param [out] state state vector + */ + void getState(Eigen::VectorXd & state); + + /** + * @brief get input vector of model + * @param [out] input input vector + */ + void getInput(Eigen::VectorXd & input); + + /** + * @brief set state vector of model + * @param [in] state state vector + */ + void setState(const Eigen::VectorXd & state); + + /** + * @brief set input vector of model + * @param [in] input input vector + */ + void setInput(const Eigen::VectorXd & input); + + /** + * @brief set gear + * @param [in] gear gear command defined in autoware_auto_msgs/VehicleStateCommand + */ + void setGear(const uint8_t gear); + + /** + * @brief update vehicle states with Runge-Kutta methods + * @param [in] dt delta time [s] + * @param [in] input vehicle input + */ + void updateRungeKutta(const float64_t & dt, const Eigen::VectorXd & input); + + /** + * @brief update vehicle states with Euler methods + * @param [in] dt delta time [s] + * @param [in] input vehicle input + */ + void updateEuler(const float64_t & dt, const Eigen::VectorXd & input); + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + virtual void update(const float64_t & dt) = 0; + + /** + * @brief get vehicle position x + */ + virtual float64_t getX() = 0; + + /** + * @brief get vehicle position y + */ + virtual float64_t getY() = 0; + + /** + * @brief get vehicle angle yaw + */ + virtual float64_t getYaw() = 0; + + /** + * @brief get vehicle velocity vx + */ + virtual float64_t getVx() = 0; + + /** + * @brief get vehicle lateral velocity + */ + virtual float64_t getVy() = 0; + + /** + * @brief get vehicle longiudinal acceleration + */ + virtual float64_t getAx() = 0; + + /** + * @brief get vehicle angular-velocity wz + */ + virtual float64_t getWz() = 0; + + /** + * @brief get vehicle steering angle + */ + virtual float64_t getSteer() = 0; + + /** + * @brief get state vector dimension + */ + inline int getDimX() {return dim_x_;} + + /** + * @brief get input vector demension + */ + inline int getDimU() {return dim_u_;} + + /** + * @brief calculate derivative of states with vehicle model + * @param [in] state current model state + * @param [in] input input vector to model + */ + virtual Eigen::VectorXd calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) = 0; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp new file mode 100644 index 0000000000000..d1179bbcc833d --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || \ + defined(PLANNING_SIMULATOR_EXPORTS) +#define PLANNING_SIMULATOR_PUBLIC __declspec(dllexport) +#define PLANNING_SIMULATOR_LOCAL +#else // defined(PLANNING_SIMULATOR_BUILDING_DLL) || + // defined(PLANNING_SIMULATOR_EXPORTS) +#define PLANNING_SIMULATOR_PUBLIC __declspec(dllimport) +#define PLANNING_SIMULATOR_LOCAL +#endif // defined(PLANNING_SIMULATOR_BUILDING_DLL) || + // defined(PLANNING_SIMULATOR_EXPORTS) +#elif defined(__linux__) +#define PLANNING_SIMULATOR_PUBLIC __attribute__((visibility("default"))) +#define PLANNING_SIMULATOR_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define PLANNING_SIMULATOR_PUBLIC __attribute__((visibility("default"))) +#define PLANNING_SIMULATOR_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) +#error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py new file mode 100644 index 0000000000000..72ce051a55d83 --- /dev/null +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -0,0 +1,81 @@ +# Copyright 2021 The Autoware Foundation. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +#    http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ament_index_python +import launch +import launch_ros.actions + +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python import get_package_share_directory + +import os + + +def generate_launch_description(): + + default_vehicle_characteristics_param = os.path.join( + get_package_share_directory('simple_planning_simulator'), + 'param/vehicle_characteristics.param.yaml') + + vehicle_characteristics_param = DeclareLaunchArgument( + 'vehicle_characteristics_param_file', + default_value=default_vehicle_characteristics_param, + description='Path to config file for vehicle characteristics' + ) + + simple_planning_simulator_node = launch_ros.actions.Node( + package='simple_planning_simulator', + executable='simple_planning_simulator_exe', + name='simple_planning_simulator', + namespace='simulation', + output='screen', + parameters=[ + "{}/param/simple_planning_simulator_default.param.yaml".format( + ament_index_python.get_package_share_directory( + "simple_planning_simulator" + ) + ), + LaunchConfiguration('vehicle_characteristics_param_file'), + ], + remappings=[ + ('input/ackermann_control_command', '/control/command/control_cmd'), + ('input/gear_command', '/control/command/gear_cmd'), + ('input/turn_indicators_command', '/control/command/turn_indicators_cmd'), + ('input/hazard_lights_command', '/control/command/hazard_lights_cmd'), + ('input/trajectory', '/planning/scenario_planning/trajectory'), + ('output/twist', '/vehicle/status/velocity_status'), + ('output/odometry', '/localization/kinematic_state'), + ('output/steering', '/vehicle/status/steering_status'), + ('output/gear_report', '/vehicle/status/gear_status'), + ('output/turn_indicators_report', '/vehicle/status/turn_indicators_status'), + ('output/hazard_lights_report', '/vehicle/status/hazard_lights_status'), + ('output/control_mode_report', '/vehicle/status/control_mode'), + ('/initialpose', '/initialpose'), + ] + ) + + map_to_odom_tf_publisher = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_map_to_odom_tf_publisher', + output='screen', + arguments=['0.0', '0.0', '0.0', '0', '0', '0', 'map', 'odom']) + + ld = launch.LaunchDescription([ + vehicle_characteristics_param, + simple_planning_simulator_node, + map_to_odom_tf_publisher + ]) + return ld diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml new file mode 100644 index 0000000000000..032ad95f4bf0f --- /dev/null +++ b/simulator/simple_planning_simulator/package.xml @@ -0,0 +1,38 @@ + + + + simple_planning_simulator + 1.0.0 + simple_planning_simulator as a ROS 2 node + Takamasa HORIBE + Apache License 2.0 + + ament_cmake_auto + autoware_auto_cmake + + autoware_api_utils + autoware_auto_planning_msgs + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_auto_tf2 + autoware_auto_common + autoware_external_api_msgs + geometry_msgs + nav_msgs + vehicle_info_util + autoware_utils + + motion_common + + rclcpp + tf2 + tf2_ros + + launch_ros + + ament_cmake_gtest + + + + ament_cmake + diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml new file mode 100644 index 0000000000000..52f2d67f5bc50 --- /dev/null +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -0,0 +1,21 @@ +simulation: + simple_planning_simulator: + ros__parameters: + simulated_frame_id: "base_link" + origin_frame_id: "map" + vehicle_model_type: "DELAY_STEER_ACC_GEARED" + initialize_source: "INITIAL_POSE_TOPIC" + timer_sampling_time_ms: 25 + add_measurement_noise: False + vel_lim: 30.0 + vel_rate_lim: 30.0 + steer_lim: 0.6 + steer_rate_lim: 6.28 + acc_time_delay: 0.1 + acc_time_constant: 0.1 + steer_time_delay: 0.1 + steer_time_constant: 0.1 + x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate + y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate + +# Note: vehicle characteristics parameters (e.g. wheelbase) are defined in a separate file. diff --git a/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml new file mode 100644 index 0000000000000..12b6efa79d1cd --- /dev/null +++ b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + vehicle: + cg_to_front_m: 1.228 + cg_to_rear_m: 1.5618 + front_corner_stiffness: 17000.0 + rear_corner_stiffness: 20000.0 + mass_kg: 1460.0 + yaw_inertia_kgm2: 2170.0 + width_m: 2.0 + front_overhang_m: 0.5 + rear_overhang_m: 0.5 diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp new file mode 100644 index 0000000000000..bd204a33fc2ad --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -0,0 +1,547 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" + +#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "common/types.hpp" +#include "motion_common/motion_common.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace +{ + +autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( + const std::shared_ptr vehicle_model_ptr) +{ + autoware_auto_vehicle_msgs::msg::VelocityReport velocity; + velocity.longitudinal_velocity = static_cast(vehicle_model_ptr->getVx()); + velocity.lateral_velocity = 0.0F; + velocity.heading_rate = static_cast(vehicle_model_ptr->getWz()); + return velocity; +} + +nav_msgs::msg::Odometry to_odometry(const std::shared_ptr vehicle_model_ptr) +{ + nav_msgs::msg::Odometry odometry; + odometry.pose.pose.position.x = vehicle_model_ptr->getX(); + odometry.pose.pose.position.y = vehicle_model_ptr->getY(); + odometry.pose.pose.orientation = motion::motion_common::from_angle(vehicle_model_ptr->getYaw()); + odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); + odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); + + return odometry; +} + +autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( + const std::shared_ptr vehicle_model_ptr) +{ + autoware_auto_vehicle_msgs::msg::SteeringReport steer; + steer.steering_tire_angle = static_cast(vehicle_model_ptr->getSteer()); + return steer; +} + +} // namespace + +namespace simulation +{ +namespace simple_planning_simulator +{ + +SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options) +: Node("simple_planning_simulator", options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_, std::shared_ptr(this, [](auto) {}), false) +{ + simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link"); + origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); + add_measurement_noise_ = declare_parameter("add_measurement_noise", false); + + using rclcpp::QoS; + using std::placeholders::_1; + using std::placeholders::_2; + + sub_init_pose_ = create_subscription( + "/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); + sub_ackermann_cmd_ = create_subscription( + "input/ackermann_control_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); + sub_gear_cmd_ = create_subscription( + "input/gear_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_gear_cmd, this, _1)); + sub_turn_indicators_cmd_ = create_subscription( + "input/turn_indicators_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_turn_indicators_cmd, this, _1)); + sub_hazard_lights_cmd_ = create_subscription( + "input/hazard_lights_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_hazard_lights_cmd, this, _1)); + sub_trajectory_ = create_subscription( + "input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1)); + + pub_control_mode_report_ = + create_publisher("output/control_mode_report", QoS{1}); + pub_gear_report_ = create_publisher("output/gear_report", QoS{1}); + pub_turn_indicators_report_ = + create_publisher("output/turn_indicators_report", QoS{1}); + pub_hazard_lights_report_ = + create_publisher("output/hazard_lights_report", QoS{1}); + pub_current_pose_ = create_publisher("/current_pose", QoS{1}); + pub_velocity_ = create_publisher("output/twist", QoS{1}); + pub_odom_ = create_publisher("output/odometry", QoS{1}); + pub_steer_ = create_publisher("output/steering", QoS{1}); + pub_tf_ = create_publisher("/tf", QoS{1}); + + /* set param callback */ + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); + + timer_sampling_time_ms_ = static_cast(declare_parameter("timer_sampling_time_ms", 25)); + on_timer_ = create_wall_timer( + std::chrono::milliseconds(timer_sampling_time_ms_), + std::bind(&SimplePlanningSimulator::on_timer, this)); + + autoware_api_utils::ServiceProxyNodeInterface proxy(this); + group_api_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_set_pose_ = proxy.create_service( + "/api/simulator/set/pose", std::bind(&SimplePlanningSimulator::on_set_pose, this, _1, _2), + rmw_qos_profile_services_default, group_api_service_); + + // set vehicle model type + initialize_vehicle_model(); + + // set initialize source + const auto initialize_source = declare_parameter("initialize_source", "INITIAL_POSE_TOPIC"); + RCLCPP_INFO(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); + if (initialize_source == "ORIGIN") { + Pose p; + p.orientation.w = 1.0; // yaw = 0 + set_initial_state(p, Twist{}); // initialize with 0 for all variables + } else if (initialize_source == "INITIAL_POSE_TOPIC") { + // initialpose sub already exists. Do nothing. + } + + // measurement noise + { + std::random_device seed; + auto & m = measurement_noise_; + m.rand_engine_ = std::make_shared(seed()); + float64_t pos_noise_stddev = declare_parameter("pos_noise_stddev", 1e-2); + float64_t vel_noise_stddev = declare_parameter("vel_noise_stddev", 1e-2); + float64_t rpy_noise_stddev = declare_parameter("rpy_noise_stddev", 1e-4); + float64_t steer_noise_stddev = declare_parameter("steer_noise_stddev", 1e-4); + m.pos_dist_ = std::make_shared>(0.0, pos_noise_stddev); + m.vel_dist_ = std::make_shared>(0.0, vel_noise_stddev); + m.rpy_dist_ = std::make_shared>(0.0, rpy_noise_stddev); + m.steer_dist_ = std::make_shared>(0.0, steer_noise_stddev); + + x_stddev_ = declare_parameter("x_stddev", 0.0001); + y_stddev_ = declare_parameter("y_stddev", 0.0001); + } +} + +void SimplePlanningSimulator::initialize_vehicle_model() +{ + const auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER_VEL"); + + RCLCPP_INFO(this->get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); + + const float64_t vel_lim = declare_parameter("vel_lim", 50.0); + const float64_t vel_rate_lim = declare_parameter("vel_rate_lim", 7.0); + const float64_t steer_lim = declare_parameter("steer_lim", 1.0); + const float64_t steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); + const float64_t acc_time_delay = declare_parameter("acc_time_delay", 0.1); + const float64_t acc_time_constant = declare_parameter("acc_time_constant", 0.1); + const float64_t steer_time_delay = declare_parameter("steer_time_delay", 0.24); + const float64_t steer_time_constant = declare_parameter("steer_time_constant", 0.27); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const float64_t wheelbase = vehicle_info.wheel_base_m; + + if (vehicle_model_type_str == "IDEAL_STEER_VEL") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; + vehicle_model_ptr_ = std::make_shared(wheelbase); + } else if (vehicle_model_type_str == "IDEAL_STEER_ACC") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER_ACC; + vehicle_model_ptr_ = std::make_shared(wheelbase); + } else if (vehicle_model_type_str == "IDEAL_STEER_ACC_GEARED") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER_ACC_GEARED; + vehicle_model_ptr_ = std::make_shared(wheelbase); + } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); + } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); + } else { + throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); + } +} + +rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + try { + autoware_utils::updateParam(parameters, "x_stddev", x_stddev_); + autoware_utils::updateParam(parameters, "y_stddev", y_stddev_); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + + return result; +} + +void SimplePlanningSimulator::on_timer() +{ + if (!is_initialized_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting initialization..."); + return; + } + + // update vehicle dynamics + { + const float64_t dt = delta_time_.get_dt(get_clock()->now()); + vehicle_model_ptr_->update(dt); + } + + // set current state + current_odometry_ = to_odometry(vehicle_model_ptr_); + current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( + current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); + + current_velocity_ = to_velocity_report(vehicle_model_ptr_); + current_steer_ = to_steering_report(vehicle_model_ptr_); + + if (add_measurement_noise_) { + add_measurement_noise(current_odometry_, current_velocity_, current_steer_); + } + + // add estimate covariance + { + current_odometry_.pose.covariance[0 * 6 + 0] = x_stddev_; + current_odometry_.pose.covariance[1 * 6 + 1] = y_stddev_; + } + + // publish vehicle state + publish_odometry(current_odometry_); + publish_velocity(current_velocity_); + publish_steering(current_steer_); + + publish_control_mode_report(); + publish_gear_report(); + publish_turn_indicators_report(); + publish_hazard_lights_report(); + publish_tf(current_odometry_); +} + +void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + // save initial pose + Twist initial_twist; + PoseStamped initial_pose; + initial_pose.header = msg->header; + initial_pose.pose = msg->pose.pose; + set_initial_state_with_transform(initial_pose, initial_twist); +} + +void SimplePlanningSimulator::on_set_pose( + const InitializePose::Request::ConstSharedPtr request, + const InitializePose::Response::SharedPtr response) +{ + // save initial pose + Twist initial_twist; + PoseStamped initial_pose; + initial_pose.header = request->pose.header; + initial_pose.pose = request->pose.pose.pose; + set_initial_state_with_transform(initial_pose, initial_twist); + response->status = autoware_api_utils::response_success(); +} + +void SimplePlanningSimulator::on_ackermann_cmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + current_ackermann_cmd_ptr_ = msg; + set_input( + msg->lateral.steering_tire_angle, msg->longitudinal.speed, msg->longitudinal.acceleration); +} + +void SimplePlanningSimulator::set_input(const float steer, const float vel, const float accel) +{ + using autoware_auto_vehicle_msgs::msg::GearCommand; + Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); + + // TODO (Watanabe): The definition of the sign of acceleration in REVERSE mode is different + // between .auto and proposal.iv, and will be discussed later. + float acc = accel; + if (!current_gear_cmd_ptr_) { + acc = 0.0; + } else if ( + current_gear_cmd_ptr_->command == GearCommand::REVERSE || + current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) + { + acc = -accel; + } + + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL) { + input << vel, steer; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) + { + input << acc, steer; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { + input << acc, steer; + } + vehicle_model_ptr_->setInput(input); +} + +void SimplePlanningSimulator::on_gear_cmd(const GearCommand::ConstSharedPtr msg) +{ + current_gear_cmd_ptr_ = msg; + + if ( + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { + vehicle_model_ptr_->setGear(current_gear_cmd_ptr_->command); + } +} + +void SimplePlanningSimulator::on_turn_indicators_cmd( + const TurnIndicatorsCommand::ConstSharedPtr msg) +{ + current_turn_indicators_cmd_ptr_ = msg; +} + +void SimplePlanningSimulator::on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg) +{ + current_hazard_lights_cmd_ptr_ = msg; +} + +void SimplePlanningSimulator::on_trajectory(const Trajectory::ConstSharedPtr msg) +{ + current_trajectory_ptr_ = msg; +} + +void SimplePlanningSimulator::add_measurement_noise( + Odometry & odom, VelocityReport & vel, SteeringReport & steer) const +{ + auto & n = measurement_noise_; + odom.pose.pose.position.x += (*n.pos_dist_)(*n.rand_engine_); + odom.pose.pose.position.y += (*n.pos_dist_)(*n.rand_engine_); + const auto velocity_noise = (*n.vel_dist_)(*n.rand_engine_); + odom.twist.twist.linear.x = velocity_noise; + float32_t yaw = motion::motion_common::to_angle(odom.pose.pose.orientation); + yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); + odom.pose.pose.orientation = motion::motion_common::from_angle(yaw); + + vel.longitudinal_velocity += static_cast(velocity_noise); + + steer.steering_tire_angle += static_cast((*n.steer_dist_)(*n.rand_engine_)); +} + +void SimplePlanningSimulator::set_initial_state_with_transform( + const PoseStamped & pose_stamped, const Twist & twist) +{ + auto transform = get_transform_msg(origin_frame_id_, pose_stamped.header.frame_id); + Pose pose; + pose.position.x = pose_stamped.pose.position.x + transform.transform.translation.x; + pose.position.y = pose_stamped.pose.position.y + transform.transform.translation.y; + pose.position.z = pose_stamped.pose.position.z + transform.transform.translation.z; + pose.orientation = pose_stamped.pose.orientation; + set_initial_state(pose, twist); +} + +void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & twist) +{ + const float64_t x = pose.position.x; + const float64_t y = pose.position.y; + const float64_t yaw = ::motion::motion_common::to_angle(pose.orientation); + const float64_t vx = twist.linear.x; + const float64_t steer = 0.0; + const float64_t accx = 0.0; + + Eigen::VectorXd state(vehicle_model_ptr_->getDimX()); + + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL) { + state << x, y, yaw; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) + { + state << x, y, yaw, vx; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { + state << x, y, yaw, vx, steer, accx; + } + vehicle_model_ptr_->setState(state); + + is_initialized_ = true; +} + +double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const double y) +{ + // calculate closest point on trajectory + if (!current_trajectory_ptr_) { + return 0.0; + } + + const double max_sqrt_dist = std::numeric_limits::max(); + double min_sqrt_dist = max_sqrt_dist; + size_t index; + bool found = false; + for (size_t i = 0; i < current_trajectory_ptr_->points.size(); ++i) { + const double dist_x = (current_trajectory_ptr_->points.at(i).pose.position.x - x); + const double dist_y = (current_trajectory_ptr_->points.at(i).pose.position.y - y); + double sqrt_dist = dist_x * dist_x + dist_y * dist_y; + if (sqrt_dist < min_sqrt_dist) { + min_sqrt_dist = sqrt_dist; + index = i; + found = true; + } + } + if (found) { + return current_trajectory_ptr_->points.at(index).pose.position.z; + } + + return 0.0; +} + +TransformStamped SimplePlanningSimulator::get_transform_msg( + const std::string parent_frame, const std::string child_frame) +{ + TransformStamped transform; + while (true) { + try { + const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); + transform = tf_buffer_.lookupTransform( + parent_frame, child_frame, time_point, tf2::durationFromSec(0.0)); + break; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + rclcpp::sleep_for(std::chrono::milliseconds(500)); + } + } + return transform; +} + +void SimplePlanningSimulator::publish_velocity(const VelocityReport & velocity) +{ + VelocityReport msg = velocity; + msg.header.stamp = get_clock()->now(); + msg.header.frame_id = simulated_frame_id_; + pub_velocity_->publish(msg); +} + +void SimplePlanningSimulator::publish_odometry(const Odometry & odometry) +{ + Odometry msg = odometry; + msg.header.frame_id = origin_frame_id_; + msg.header.stamp = get_clock()->now(); + msg.child_frame_id = simulated_frame_id_; + pub_odom_->publish(msg); +} + +void SimplePlanningSimulator::publish_steering(const SteeringReport & steer) +{ + SteeringReport msg = steer; + msg.stamp = get_clock()->now(); + pub_steer_->publish(msg); +} + +void SimplePlanningSimulator::publish_control_mode_report() +{ + ControlModeReport msg; + msg.stamp = get_clock()->now(); + msg.mode = ControlModeReport::AUTONOMOUS; + pub_control_mode_report_->publish(msg); +} + +void SimplePlanningSimulator::publish_gear_report() +{ + if (!current_gear_cmd_ptr_) { + return; + } + GearReport msg; + msg.stamp = get_clock()->now(); + msg.report = current_gear_cmd_ptr_->command; + pub_gear_report_->publish(msg); +} + +void SimplePlanningSimulator::publish_turn_indicators_report() +{ + if (!current_turn_indicators_cmd_ptr_) { + return; + } + TurnIndicatorsReport msg; + msg.stamp = get_clock()->now(); + msg.report = current_turn_indicators_cmd_ptr_->command; + pub_turn_indicators_report_->publish(msg); +} + +void SimplePlanningSimulator::publish_hazard_lights_report() +{ + if (!current_hazard_lights_cmd_ptr_) { + return; + } + HazardLightsReport msg; + msg.stamp = get_clock()->now(); + msg.report = current_hazard_lights_cmd_ptr_->command; + pub_hazard_lights_report_->publish(msg); +} + +void SimplePlanningSimulator::publish_tf(const Odometry & odometry) +{ + TransformStamped tf; + tf.header.stamp = get_clock()->now(); + tf.header.frame_id = origin_frame_id_; + tf.child_frame_id = simulated_frame_id_; + tf.transform.translation.x = odometry.pose.pose.position.x; + tf.transform.translation.y = odometry.pose.pose.position.y; + tf.transform.translation.z = odometry.pose.pose.position.z; + tf.transform.rotation = odometry.pose.pose.orientation; + + tf2_msgs::msg::TFMessage tf_msg{}; + tf_msg.transforms.emplace_back(std::move(tf)); + pub_tf_->publish(tf_msg); +} +} // namespace simple_planning_simulator +} // namespace simulation + +RCLCPP_COMPONENTS_REGISTER_NODE(simulation::simple_planning_simulator::SimplePlanningSimulator) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp new file mode 100644 index 0000000000000..d2fff30b202ce --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -0,0 +1,99 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" + +#include + +SimModelDelaySteerAcc::SimModelDelaySteerAcc( + float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, + float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant) +: SimModelInterface(6 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + acc_delay_(acc_delay), + acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) +{ + initializeInputQueue(dt); +} + +float64_t SimModelDelaySteerAcc::getX() {return state_(IDX::X);} +float64_t SimModelDelaySteerAcc::getY() {return state_(IDX::Y);} +float64_t SimModelDelaySteerAcc::getYaw() {return state_(IDX::YAW);} +float64_t SimModelDelaySteerAcc::getVx() {return state_(IDX::VX);} +float64_t SimModelDelaySteerAcc::getVy() {return 0.0;} +float64_t SimModelDelaySteerAcc::getAx() {return state_(IDX::ACCX);} +float64_t SimModelDelaySteerAcc::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +float64_t SimModelDelaySteerAcc::getSteer() {return state_(IDX::STEER);} +void SimModelDelaySteerAcc::update(const float64_t & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); + acc_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); + + updateRungeKutta(dt, delayed_input); + + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); +} + +void SimModelDelaySteerAcc::initializeInputQueue(const float64_t & dt) +{ + size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); + acc_input_queue_.resize(acc_input_queue_size); + std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0); + + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + steer_input_queue_.resize(steer_input_queue_size); + std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); +} + +Eigen::VectorXd SimModelDelaySteerAcc::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; + + const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); + const float64_t yaw = state(IDX::YAW); + const float64_t steer = state(IDX::STEER); + const float64_t acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); + const float64_t steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + float64_t steer_rate = -(steer - steer_des) / steer_time_constant_; + steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; + + return d_state; +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp new file mode 100644 index 0000000000000..092c672e311fa --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -0,0 +1,139 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" + +SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( + float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, + float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant) +: SimModelInterface(6 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + acc_delay_(acc_delay), + acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) +{ + initializeInputQueue(dt); +} + +float64_t SimModelDelaySteerAccGeared::getX() {return state_(IDX::X);} +float64_t SimModelDelaySteerAccGeared::getY() {return state_(IDX::Y);} +float64_t SimModelDelaySteerAccGeared::getYaw() {return state_(IDX::YAW);} +float64_t SimModelDelaySteerAccGeared::getVx() {return state_(IDX::VX);} +float64_t SimModelDelaySteerAccGeared::getVy() {return 0.0;} +float64_t SimModelDelaySteerAccGeared::getAx() {return state_(IDX::ACCX);} +float64_t SimModelDelaySteerAccGeared::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +float64_t SimModelDelaySteerAccGeared::getSteer() {return state_(IDX::STEER);} +void SimModelDelaySteerAccGeared::update(const float64_t & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); + acc_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); + + const auto prev_vx = state_(IDX::VX); + + updateRungeKutta(dt, delayed_input); + + // take velocity limit explicitly + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); + + state_(IDX::VX) = calcVelocityWithGear(state_, gear_); + + // calc acc directly after gear consideration + state_(IDX::ACCX) = (state_(IDX::VX) - prev_vx) / std::max(dt, 1.0e-5); +} + +void SimModelDelaySteerAccGeared::initializeInputQueue(const float64_t & dt) +{ + size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); + acc_input_queue_.resize(acc_input_queue_size); + std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0); + + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + steer_input_queue_.resize(steer_input_queue_size); + std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); +} + +Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; + + const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); + const float64_t yaw = state(IDX::YAW); + const float64_t steer = state(IDX::STEER); + const float64_t acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); + const float64_t steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + float64_t steer_rate = -(steer - steer_des) / steer_time_constant_; + steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; + + return d_state; +} + + +float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( + const Eigen::VectorXd & state, const uint8_t gear) const +{ + using autoware_auto_vehicle_msgs::msg::GearCommand; + if ( + gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || + gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || + gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 || + gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 || + gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || + gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || + gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) + { + if (state(IDX::VX) < 0.0) { + return 0.0; + } + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + if (state(IDX::VX) > 0.0) { + return 0.0; + } + } else if (gear == GearCommand::PARK) { + return 0.0; + } else { + return 0.0; + } + + return state(IDX::VX); +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp new file mode 100644 index 0000000000000..ee74b645850cf --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" + + +SimModelIdealSteerAcc::SimModelIdealSteerAcc(float64_t wheelbase) +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} + +float64_t SimModelIdealSteerAcc::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerAcc::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerAcc::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerAcc::getVx() {return state_(IDX::VX);} +float64_t SimModelIdealSteerAcc::getVy() {return 0.0;} +float64_t SimModelIdealSteerAcc::getAx() {return input_(IDX_U::AX_DES);} +float64_t SimModelIdealSteerAcc::getWz() +{ + return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +} +float64_t SimModelIdealSteerAcc::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteerAcc::update(const float64_t & dt) +{ + updateRungeKutta(dt, input_); +} + +Eigen::VectorXd SimModelIdealSteerAcc::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const float64_t vx = state(IDX::VX); + const float64_t yaw = state(IDX::YAW); + const float64_t ax = input(IDX_U::AX_DES); + const float64_t steer = input(IDX_U::STEER_DES); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * std::cos(yaw); + d_state(IDX::Y) = vx * std::sin(yaw); + d_state(IDX::VX) = ax; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp new file mode 100644 index 0000000000000..9f4b6663d05f5 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -0,0 +1,90 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" + +SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(float64_t wheelbase) +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) {} + +float64_t SimModelIdealSteerAccGeared::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerAccGeared::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerAccGeared::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerAccGeared::getVx() {return state_(IDX::VX);} +float64_t SimModelIdealSteerAccGeared::getVy() {return 0.0;} +float64_t SimModelIdealSteerAccGeared::getAx() {return current_acc_;} +float64_t SimModelIdealSteerAccGeared::getWz() +{ + return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +} +float64_t SimModelIdealSteerAccGeared::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteerAccGeared::update(const float64_t & dt) +{ + const auto prev_vx = state_(IDX::VX); + + updateRungeKutta(dt, input_); + + state_(IDX::VX) = calcVelocityWithGear(state_, gear_); + + current_acc_ = (state_(IDX::VX) - prev_vx) / std::max(dt, 1.0e-5); +} + +Eigen::VectorXd SimModelIdealSteerAccGeared::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const float64_t vx = state(IDX::VX); + const float64_t yaw = state(IDX::YAW); + const float64_t ax = input(IDX_U::AX_DES); + const float64_t steer = input(IDX_U::STEER_DES); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * std::cos(yaw); + d_state(IDX::Y) = vx * std::sin(yaw); + d_state(IDX::VX) = ax; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +} + +float64_t SimModelIdealSteerAccGeared::calcVelocityWithGear( + const Eigen::VectorXd & state, const uint8_t gear) const +{ + using autoware_auto_vehicle_msgs::msg::GearCommand; + if ( + gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || + gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || + gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 || + gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 || + gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || + gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || + gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) + { + if (state(IDX::VX) < 0.0) { + return 0.0; + } + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + if (state(IDX::VX) > 0.0) { + return 0.0; + } + } else if (gear == GearCommand::PARK) { + return 0.0; + } else { + return 0.0; + } + + return state(IDX::VX); +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp new file mode 100644 index 0000000000000..ea40f647e2379 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" + + +SimModelIdealSteerVel::SimModelIdealSteerVel(float64_t wheelbase) +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} + +float64_t SimModelIdealSteerVel::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerVel::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerVel::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerVel::getVx() {return input_(IDX_U::VX_DES);} +float64_t SimModelIdealSteerVel::getVy() {return 0.0;} +float64_t SimModelIdealSteerVel::getAx() {return current_ax_;} +float64_t SimModelIdealSteerVel::getWz() +{ + return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +} +float64_t SimModelIdealSteerVel::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteerVel::update(const float64_t & dt) +{ + updateRungeKutta(dt, input_); + current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; + prev_vx_ = input_(IDX_U::VX_DES); +} + +Eigen::VectorXd SimModelIdealSteerVel::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const float64_t yaw = state(IDX::YAW); + const float64_t vx = input(IDX_U::VX_DES); + const float64_t steer = input(IDX_U::STEER_DES); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * std::cos(yaw); + d_state(IDX::Y) = vx * std::sin(yaw); + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp new file mode 100644 index 0000000000000..13bf38fc11f72 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 The Autoware Foundation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +SimModelInterface::SimModelInterface(int dim_x, int dim_u) +: dim_x_(dim_x), dim_u_(dim_u) +{ + state_ = Eigen::VectorXd::Zero(dim_x_); + input_ = Eigen::VectorXd::Zero(dim_u_); +} + +void SimModelInterface::updateRungeKutta(const float64_t & dt, const Eigen::VectorXd & input) +{ + Eigen::VectorXd k1 = calcModel(state_, input); + Eigen::VectorXd k2 = calcModel(state_ + k1 * 0.5 * dt, input); + Eigen::VectorXd k3 = calcModel(state_ + k2 * 0.5 * dt, input); + Eigen::VectorXd k4 = calcModel(state_ + k3 * dt, input); + + state_ += 1.0 / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) * dt; +} +void SimModelInterface::updateEuler(const float64_t & dt, const Eigen::VectorXd & input) +{ + state_ += calcModel(state_, input) * dt; +} +void SimModelInterface::getState(Eigen::VectorXd & state) {state = state_;} +void SimModelInterface::getInput(Eigen::VectorXd & input) {input = input_;} +void SimModelInterface::setState(const Eigen::VectorXd & state) {state_ = state;} +void SimModelInterface::setInput(const Eigen::VectorXd & input) {input_ = input;} +void SimModelInterface::setGear(const uint8_t gear) {gear_ = gear;} diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp new file mode 100644 index 0000000000000..e0de817247e2f --- /dev/null +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -0,0 +1,386 @@ +// Copyright 2021 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "gtest/gtest.h" + +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "motion_common/motion_common.hpp" + +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; +using autoware_auto_vehicle_msgs::msg::VehicleKinematicState; +using geometry_msgs::msg::PoseWithCovarianceStamped; + +using simulation::simple_planning_simulator::SimplePlanningSimulator; + +std::string toStrInfo(const VehicleKinematicState & state) +{ + const auto & s = state.state; + std::stringstream ss; + ss << "state x: " << s.pose.position.x << ", y: " << s.pose.position.y << ", yaw: " << + motion::motion_common::to_angle( + s.pose.orientation) << ", vx = " << s.longitudinal_velocity_mps << ", vy: " << + s.lateral_velocity_mps << + ", ax: " << s.acceleration_mps2 << ", steer: " << s.front_wheel_angle_rad; + return ss.str(); +} + +const std::vector vehicle_model_type_vec = { // NOLINT + "IDEAL_STEER_VEL", + "IDEAL_STEER_ACC", + "IDEAL_STEER_ACC_GEARED", + "DELAY_STEER_ACC", + "DELAY_STEER_ACC_GEARED", +}; + +static constexpr float64_t COM_TO_BASELINK = 1.5; +class PubSubNode : public rclcpp::Node +{ +public: + PubSubNode() + : Node{"test_simple_planning_simulator_pubsub"} + { + kinematic_state_sub_ = create_subscription( + "output/kinematic_state", rclcpp::QoS{1}, + [this](const VehicleKinematicState::SharedPtr msg) { + current_state_ = msg; + }); + pub_control_command_ = create_publisher( + "input/vehicle_control_command", + rclcpp::QoS{1}); + pub_ackermann_command_ = create_publisher( + "input/ackermann_control_command", + rclcpp::QoS{1}); + pub_initialpose_ = create_publisher( + "/initialpose", + rclcpp::QoS{1}); + pub_state_cmd_ = create_publisher( + "/input/vehicle_state_command", + rclcpp::QoS{1}); + } + + rclcpp::Publisher::SharedPtr pub_control_command_; + rclcpp::Publisher::SharedPtr pub_ackermann_command_; + rclcpp::Publisher::SharedPtr pub_state_cmd_; + rclcpp::Publisher::SharedPtr pub_initialpose_; + rclcpp::Subscription::SharedPtr kinematic_state_sub_; + + VehicleKinematicState::SharedPtr current_state_; +}; + +/** + * @brief Generate a VehicleControlCommand message + * @param [in] t timestamp + * @param [in] steer [rad] steering + * @param [in] vel [m/s] velocity + * @param [in] acc [m/s²] acceleration + */ +VehicleControlCommand cmdGen( + const builtin_interfaces::msg::Time & t, float32_t steer, float32_t vel, float32_t acc) +{ + VehicleControlCommand cmd; + cmd.stamp = t; + cmd.front_wheel_angle_rad = steer; + cmd.velocity_mps = vel; + cmd.long_accel_mps2 = acc; + return cmd; +} + +/** + * @brief Generate an AckermannControlCommand message + * @param [in] t timestamp + * @param [in] steer [rad] steering + * @param [in] vel [m/s] velocity + * @param [in] acc [m/s²] acceleration + */ +AckermannControlCommand ackermannCmdGen( + const builtin_interfaces::msg::Time & t, float32_t steer, float32_t vel, float32_t acc) +{ + AckermannControlCommand cmd; + cmd.stamp = t; + cmd.lateral.stamp = t; + cmd.lateral.steering_tire_angle = steer; + cmd.longitudinal.stamp = t; + cmd.longitudinal.speed = vel; + cmd.longitudinal.acceleration = acc; + return cmd; +} + +void resetInitialpose(rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) +{ + PoseWithCovarianceStamped p; + p.header.frame_id = "odom"; + p.header.stamp = sim_node->now(); + p.pose.pose.orientation.w = 1.0; // yaw = 0 + for (int i = 0; i < 10; ++i) { + pub_sub_node->pub_initialpose_->publish(p); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +void sendGear( + uint8_t gear, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) +{ + VehicleStateCommand cmd; + cmd.stamp = sim_node->now(); + cmd.gear = gear; + for (int i = 0; i < 10; ++i) { + pub_sub_node->pub_state_cmd_->publish(cmd); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +/** + * @brief publish the given command message + * @param [in] cmd command to publish + * @param [in] sim_node pointer to the simulation node + * @param [in] pub_sub_node pointer to the node used for communication + */ +void sendCommand( + const VehicleControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) +{ + for (int i = 0; i < 150; ++i) { + pub_sub_node->pub_control_command_->publish(cmd); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +/** + * @brief publish the given command message + * @param [in] cmd command to publish + * @param [in] sim_node pointer to the simulation node + * @param [in] pub_sub_node pointer to the node used for communication + */ +void sendCommand( + const AckermannControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) +{ + for (int i = 0; i < 150; ++i) { + pub_sub_node->pub_ackermann_command_->publish(cmd); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +VehicleKinematicState comToBaselink(const VehicleKinematicState & com) +{ + auto baselink = com; + float64_t yaw = motion::motion_common::to_angle(com.state.pose.orientation); + baselink.state.pose.position.x -= COM_TO_BASELINK * std::cos(yaw); + baselink.state.pose.position.y -= COM_TO_BASELINK * std::sin(yaw); + return baselink; +} + + +// Check which direction the vehicle is heading on the baselink coordinates. +// y +// | +// | (Fwd-Left) +// | +// ---------(Bwd)------------------(Fwd)----------> x +// | +// (Bwd-Right) | +// | +// +void isOnForward(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t forward_thr = 1.0; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +void isOnBackward(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t backward_thr = -1.0; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +void isOnForwardLeft(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t forward_thr = 1.0; + float64_t left_thr = 0.1f; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + float64_t dy = state.state.pose.position.y - init.state.pose.position.y; + EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); + EXPECT_GT(dy, left_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +void isOnBackwardRight(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t backward_thr = -1.0; + float64_t right_thr = -0.1; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + float64_t dy = state.state.pose.position.y - init.state.pose.position.y; + EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); + EXPECT_LT(dy, right_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +// Send a control command and run the simulation. +// Then check if the vehicle is moving in the desired direction. +TEST(TestSimplePlanningSimulatorIdealSteerVel, TestMoving) +{ + rclcpp::init(0, nullptr); + + for (const auto & vehicle_model_type : vehicle_model_type_vec) { + std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("initialize_source", "INITIAL_POSE_TOPIC"); + node_options.append_parameter_override("cg_to_rear_m", COM_TO_BASELINK); + node_options.append_parameter_override("vehicle_model_type", vehicle_model_type); + const auto sim_node = std::make_shared(node_options); + + const auto pub_sub_node = std::make_shared(); + + const float32_t target_vel = 5.0f; + const float32_t target_acc = 5.0f; + const float32_t target_steer = 0.2f; + + auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; + auto _sendFwdGear = [&]() {sendGear(GearCommand::DRIVE, sim_node, pub_sub_node);}; + auto _sendBwdGear = [&]() {sendGear(GearCommand::REVERSE, sim_node, pub_sub_node);}; + auto _sendCommand = [&](const auto & _cmd) { + sendCommand(_cmd, sim_node, pub_sub_node); + }; + + // check pub-sub connections + { + size_t expected = 1; + EXPECT_EQ(pub_sub_node->pub_control_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_ackermann_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_state_cmd_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_initialpose_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->kinematic_state_sub_->get_publisher_count(), expected); + } + + // check initial pose + _resetInitialpose(); + const auto init_state = *(pub_sub_node->current_state_); + + // go forward + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(cmdGen(sim_node->now(), 0.0f, target_vel, target_acc)); + isOnForward(*(pub_sub_node->current_state_), init_state); + + // go backward + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(cmdGen(sim_node->now(), 0.0f, -target_vel, -target_acc)); + isOnBackward(*(pub_sub_node->current_state_), init_state); + + // go forward left + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(cmdGen(sim_node->now(), target_steer, target_vel, target_acc)); + isOnForwardLeft(*(pub_sub_node->current_state_), init_state); + + // go backward right + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(cmdGen(sim_node->now(), -target_steer, -target_vel, -target_acc)); + isOnBackwardRight(*(pub_sub_node->current_state_), init_state); + } + + rclcpp::shutdown(); +} + +// Send a control command and run the simulation. +// Then check if the vehicle is moving in the desired direction. +TEST(test_simple_planning_simulator, test_moving_ackermann) +{ + rclcpp::init(0, nullptr); + + for (const auto & vehicle_model_type : vehicle_model_type_vec) { + std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("initialize_source", "INITIAL_POSE_TOPIC"); + node_options.append_parameter_override("cg_to_rear_m", COM_TO_BASELINK); + node_options.append_parameter_override("vehicle_model_type", vehicle_model_type); + const auto sim_node = std::make_shared(node_options); + + const auto pub_sub_node = std::make_shared(); + + const float32_t target_vel = 5.0f; + const float32_t target_acc = 5.0f; + const float32_t target_steer = 0.2f; + + auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; + auto _sendFwdGear = [&]() {sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node);}; + auto _sendBwdGear = + [&]() {sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node);}; + auto _sendCommand = [&](const auto & _cmd) { + sendCommand(_cmd, sim_node, pub_sub_node); + }; + + // check pub-sub connections + { + size_t expected = 1; + EXPECT_EQ(pub_sub_node->pub_control_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_ackermann_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_state_cmd_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_initialpose_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->kinematic_state_sub_->get_publisher_count(), expected); + } + + // check initial pose + _resetInitialpose(); + const auto init_state = *(pub_sub_node->current_state_); + + // go forward + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), 0.0f, target_vel, target_acc)); + isOnForward(*(pub_sub_node->current_state_), init_state); + + // go backward + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), 0.0f, -target_vel, -target_acc)); + isOnBackward(*(pub_sub_node->current_state_), init_state); + + // go forward left + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), target_steer, target_vel, target_acc)); + isOnForwardLeft(*(pub_sub_node->current_state_), init_state); + + // go backward right + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), -target_steer, -target_vel, -target_acc)); + isOnBackwardRight(*(pub_sub_node->current_state_), init_state); + } + + rclcpp::shutdown(); +}