Skip to content

Commit

Permalink
feat: add simple planning simulator package (#5)
Browse files Browse the repository at this point in the history
* release v0.4.0

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add sample ros2 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Fix simple planning simulator (autowarefoundation#26)

* simple planning simulator: fix params & launch file

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused file

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix timercallback

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [simple_planning_simulator] add rostopic relay in launch file (autowarefoundation#117)

* [simple_planning_simulator] add rostopic relay in launch file

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add topic_tools as exec_depend

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* reduce terminal ouput for better error message visibility (autowarefoundation#200)

* reduce terminal ouput for better error message visibility

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [costmap_generator] fix waiting for first transform

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix tests

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix test

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Use trajectory for z position source (autowarefoundation#243)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Ros2 v0.8.0 engage (autowarefoundation#342)

* [autoware_vehicle_msgs]: Add engage message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [as]: Update message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [awapi_awiv_adapter]: Update message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [web_controller]: Update message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [vehicle_cmd_gate]: Update message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [autoware_state_monitor]: Update message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [autoware_control_msgs]: Remove EngageMode message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [simple_planning_simulator]: Update message

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Ros2 v0.8.0 fix packages (autowarefoundation#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 (autowarefoundation#352)

* Rename ROS-related .yaml to .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove prefix 'default_' of yaml files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename vehicle_info.yaml to vehicle_info.param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename diagnostic_aggregator's param files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix overlooked parameters

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix typo in simulator module (autowarefoundation#439)

* add use_sim-time option (autowarefoundation#454)

* Format launch files (autowarefoundation#1219)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix rolling build errors (autowarefoundation#1225)

* Add missing include files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Replace rclcpp::Duration

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Use reference for exceptions

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Use from_seconds

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Sync public repo (autowarefoundation#1228)

* [simple_planning_simulator] add readme (autowarefoundation#424)

* add readme of simple_planning_simulator

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update simulator/simple_planning_simulator/README.md

* set transit_margin_time to intersect. planner (autowarefoundation#460)

* Fix pose2twist (autowarefoundation#462)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Ros2 vehicle info param server (autowarefoundation#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 (autowarefoundation#425)

* Fix topic name of traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_visualization

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name of traffic_light_map_based_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_recognition

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix issues in hdd_reader (autowarefoundation#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 (autowarefoundation#463)

* Rviz overlay render fix (autowarefoundation#461)

* Moved painiting in SteeringAngle plugin to update()

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* uncrustified

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* back to RTD as superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* Rviz overlay render in update (autowarefoundation#465)

* Moved painiting in SteeringAngle plugin to update()

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* uncrustified

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* removed unnecessary includes and some dead code

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

* restored RTD superclass

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>
Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai>

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Refactor vehicle info util (autowarefoundation#1305)

* Update license

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Refactor vehicle_info_util

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename and split files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix interfaces

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix bug and add error handling

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add "// namespace"

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add missing include

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix lint errors (autowarefoundation#1378)

* Fix lint errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix variable names

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add pre-commit (autowarefoundation#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 <kmiya@outlook.com>

* 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 <kmiya@outlook.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Add markdownlint and prettier (autowarefoundation#1661)

* Add markdownlint and prettier

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* add cov pub in psim (autowarefoundation#1732)

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* fix some typos (autowarefoundation#1941)

* fix some typos

* fix typo

* Fix typo

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add autoware api (autowarefoundation#1979)

* add sort-package-xml hook in pre-commit (autowarefoundation#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 (autowarefoundation#1894)

* Add IDEAL_ACCEL model interface for simple planning simulator

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Add IDEAL_ACCEL model descriptions

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Fix format

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Change vehicle model type description at config file

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Change formatter to clang-format and black (autowarefoundation#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 <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Back port .auto control packages (autowarefoundation#571)

* Implement Lateral and Longitudinal Control Muxer

* [autowarefoundation#570] Porting wf_simulator

* [autowarefoundation#1189] Deactivate flaky test in 'trajectory_follower_nodes'

* [autowarefoundation#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer'

* [autowarefoundation#1057] Add osqp_interface package

* [autowarefoundation#1057] Add library code for MPC-based lateral control

* [autowarefoundation#1271] Use std::abs instead of abs

* [autowarefoundation#1057] Implement Lateral Controller for Cargo ODD

* [autowarefoundation#1246] Resolve "Test case names currently use snake_case but should be CamelCase"

* [autowarefoundation#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes'

* [autowarefoundation#1058] Add library code of longitudinal controller

* Fix build error for trajectory follower

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix build error for trajectory follower nodes

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [autowarefoundation#1272] Add AckermannControlCommand support to simple_planning_simulator

* [autowarefoundation#1058] Add Longitudinal Controller node

* [autowarefoundation#1058] Rename velocity_controller -> longitudinal_controller

* [autowarefoundation#1058] Update CMakeLists.txt for the longitudinal_controller_node

* [autowarefoundation#1058] Add smoke test python launch file

* [autowarefoundation#1058] Use LowPassFilter1d from trajectory_follower

* [autowarefoundation#1058] Use autoware_auto_msgs

* [autowarefoundation#1058] Changes for .auto (debug msg tmp fix, common func, tf listener)

* [autowarefoundation#1058] Remove unused parameters

* [autowarefoundation#1058] Fix ros test

* [autowarefoundation#1058] Rm default params from declare_parameters + use autoware types

* [autowarefoundation#1058] Use default param file to setup NodeOptions in the ros test

* [autowarefoundation#1058] Fix docstring

* [autowarefoundation#1058] Replace receiving a Twist with a VehicleKinematicState

* [autowarefoundation#1058] Change class variables format to m_ prefix

* [autowarefoundation#1058] Fix plugin name of LongitudinalController in CMakeLists.txt

* [autowarefoundation#1058] Fix copyright dates

* [autowarefoundation#1058] Reorder includes

* [autowarefoundation#1058] Add some tests (~89% coverage without disabling flaky tests)

* [autowarefoundation#1058] Add more tests (90+% coverage without disabling flaky tests)

* [autowarefoundation#1058] Use Float32MultiArrayDiagnostic message for debug and slope

* [autowarefoundation#1058] Calculate wheel_base value from vehicle parameters

* [autowarefoundation#1058] Cleanup redundant logger setting in tests

* [autowarefoundation#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures

* [autowarefoundation#1058] Remove TF listener and use published vehicle state instead

* [autowarefoundation#1058] Change smoke tests to use autoware_testing

* [autowarefoundation#1058] Add plotjuggler cfg for both lateral and longitudinal control

* [autowarefoundation#1058] Improve design documents

* [autowarefoundation#1058] Disable flaky test

* [autowarefoundation#1058] Properly transform vehicle state in longitudinal node

* [autowarefoundation#1058] Fix TF buffer of lateral controller

* [autowarefoundation#1058] Tuning of lateral controller for LGSVL

* [autowarefoundation#1058] Fix formating

* [autowarefoundation#1058] Fix /tf_static sub to be transient_local

* [autowarefoundation#1058] Fix yaw recalculation of reverse trajs in the lateral controller

* modify trajectory_follower for galactic build

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [autowarefoundation#1379] Update trajectory_follower

* [autowarefoundation#1379] Update simple_planning_simulator

* [autowarefoundation#1379] Update trajectory_follower_nodes

* apply trajectory msg modification in control

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move directory

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remote control/trajectory_follower level dorectpry

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove .iv trajectory follower

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use .auto trajectory_follower

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove .iv simple_planning_simulator & osqp_interface

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use .iv simple_planning_simulator & osqp_interface

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add tmp_autoware_auto_dependencies

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* tmporally add autoware_auto_msgs

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* apply .auto message split

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix build depend

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* 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 <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* [simple planning simulator]change type of msg (autowarefoundation#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 (autowarefoundation#606)

* fix console meter

* fix velocity_history

* fix route handler

* change topic name

* update to support velocity report header (autowarefoundation#655)

* update to support velocity report header

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* use maybe_unused

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* adapt to actuation cmd/status as control msg (autowarefoundation#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 (autowarefoundation#658)

* shift -> gear_status

* twist -> velocity_status

* fix topic name (autowarefoundation#674)

* fix topic name

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix gear message name

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Fix psim param path (autowarefoundation#696)

* Fix/psim topics emergency handler awapi (autowarefoundation#702)

* fix emergency handler

* fix awapi

* remove unused topic

* remove duplecated vehicle cmd

* Auto/add turn indicators and hazards (autowarefoundation#717)

* add turn indicators

* add hazard light

* omit name space

* remap topic name

* delete unnecessary blank line

* [simple_planning_simulator]fix bug (autowarefoundation#727)

* input z-axis of trajectory to pose(tf/odometry)

* output 0 velocity when invalid gear is input

* fix gear process in sim (autowarefoundation#728)

* Fix for integration test (autowarefoundation#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 (autowarefoundation#735)

* Refactor vehicle info util (autowarefoundation#1305)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add cov pub in psim (autowarefoundation#1732)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove pose_with_covariance publisher and add covariance information in Odometry

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Fix acceleration for reverse (autowarefoundation#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 <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
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 <kmiya@outlook.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com>
Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
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 <rej55.g@gmail.com>
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>
  • Loading branch information
1 parent 2356788 commit f385924
Show file tree
Hide file tree
Showing 25 changed files with 2,938 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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()};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
44 changes: 44 additions & 0 deletions simulator/simple_planning_simulator/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
Original file line number Diff line number Diff line change
@@ -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] |
<!-- |deadzone_delta_steer | double | dead zone for the steering dynamics | x | x | x | o | o | 0.0 | [rad] | -->


*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
<!-- Required -->
<!-- Things to consider:
- Spoofing (How do you check for and handle fake input?)
- Tampering (How do you check for and handle tampered input?)
- Repudiation (How are you affected by the actions of external actors?).
- Information Disclosure (Can data leak?).
- Denial of Service (How do you handle spamming?).
- Elevation of Privilege (Do you need to change permission levels during execution?) -->


# 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
Loading

0 comments on commit f385924

Please sign in to comment.