Skip to content

Commit

Permalink
feat: add obstacle_stop_planner packages (autowarefoundation#93)
Browse files Browse the repository at this point in the history
* release v0.4.0

* Feature/stop reason (autowarefoundation#712)

* add stop reason msg

* add mock of stop resaon publisher

* change namespace of stop reason

* update stop reason msg

* add toRosPoint

* implement stop reason publisher of blind stop

* implement stop reason publisher of crosswalk

* implement stop reason publisher of intersection

* implement stop reason publisher of stop line

* implement stop reason publisher of trafficlight

* implement stop reason publisher of detection area

* fix bug

* remove unnecessary process

* add remained stop factor

* clean code

* fix bug

* not punlish stop reason if array size is 0

* add stop reason to stuck object in intersection

* add stop factor of obstacle stop planner

* add stop reason of surround_obstacle checker

* Apply review

Signed-off-by: Kenji Miyake <[email protected]>

* fix message type

* delete unused message from cmake

* remove stopReasonStamped

* change topic name of stop reasons

Co-authored-by: Kenji Miyake <[email protected]>

* Fix/stop reason (autowarefoundation#724)

* input stop reason of traffic light

* add comment

* add empty traffic light handling

* change calculation method of traffic light position

* avoid 0 position output

* add debug values of adaptive cruise control (autowarefoundation#742)

* add debug values of adaptive cruise control

* add rqt config file

* cosmetic change (autowarefoundation#738)

Signed-off-by: Yukihiro Saito <[email protected]>

* Fix/acc same pointcloud (autowarefoundation#743)

* add handling with same pointcloud

* clean code

* add obstacle_stop_planner.yaml (autowarefoundation#766)

* add stop_planner.yaml

* change file name

* add explanation of the parameters

* fix wrong start index (autowarefoundation#745)

Signed-off-by: Yukihiro Saito <[email protected]>

* Feature/smooth adaptive cruise (autowarefoundation#789)

* change param

* do not insert max velocity near the ego vehicle position

* Change min_slow_down_vel to 3kmph (autowarefoundation#796)

Signed-off-by: Kenji Miyake <[email protected]>

* Remove duplicated param (autowarefoundation#797)

Signed-off-by: Kenji Miyake <[email protected]>

* Feature/lowpass adaptive cruise control (autowarefoundation#802)

* add lowpass filter

* update xml

* change the method of insert velocity

* change parameter

* change default lowpass gain in acc (autowarefoundation#820)

* change default deceleration in acc

* change lowpass gain

* Reduce calc cost of slow down (#826)

* Cosmetic change

Signed-off-by: wep21 <[email protected]>

* Fix rough search range

Signed-off-by: wep21 <[email protected]>

* Add circle inside/outside judgement

Signed-off-by: wep21 <[email protected]>

* Apply clang

Signed-off-by: wep21 <[email protected]>

* fix uninitialized variables (autowarefoundation#816)

* Revert "Reduce calc cost of slow down (#826)" (autowarefoundation#832)

This reverts commit 845ab7b1b90f8817c8fcb12880b5af6f78c2b183.

* Reduce calc cost stop planner (autowarefoundation#833)

* Reduce calc cost of slow down (#826)

* Cosmetic change

Signed-off-by: wep21 <[email protected]>

* Fix rough search range

Signed-off-by: wep21 <[email protected]>

* Add circle inside/outside judgement

Signed-off-by: wep21 <[email protected]>

* Apply clang

Signed-off-by: wep21 <[email protected]>

* Parameterize step length

Signed-off-by: wep21 <[email protected]>

* Fix search range

Signed-off-by: wep21 <[email protected]>

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <[email protected]>

* Revert "remove ROS1 packages temporarily"

This reverts commit dae63c141d17c2c7d0402ca2b9c5c1f008101631.

Signed-off-by: mitsudome-r <[email protected]>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <[email protected]>

* Rename launch files to launch.xml (autowarefoundation#28)

* Port obstacle stop planner (autowarefoundation#86)

* Port to ROS2

Signed-off-by: Servando German Serrano <[email protected]>

* Use autoware_debug_msgs

Signed-off-by: Servando German Serrano <[email protected]>

* Fix variable name

Signed-off-by: Servando German Serrano <[email protected]>

* Fix launch files (autowarefoundation#122)

* [surround_obstacle_checker] add parameter and arguments to launch file

Signed-off-by: mitsudome-r <[email protected]>

* [obstacle_stop_planner] modify launch file to remap trajectory from argument

Signed-off-by: mitsudome-r <[email protected]>

* [obstacle_avoidance_planner] modify launch file to remap topics from arguments

Signed-off-by: mitsudome-r <[email protected]>

* [motion_velocity_optimizer] modify launch file to enable remapping from argument

Signed-off-by: mitsudome-r <[email protected]>

* Convert calls of Duration to Duration::from_seconds where appropriate (autowarefoundation#131)

* 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

* Added linters to obstacle_stop_planner (autowarefoundation#154)

* Added linters to obstacle_stop_planner

* Remove use of deprecated ConstPtr

* Remove use of deprecated ::Ptr

* Removed unused variables

* Fix sign mismatches

* Fix non-void returns

* Fix cpplint error

* Fix sign mismatching

Co-authored-by: Jilada Eccleston <[email protected]>

* Ros2 v0.8.0 obstacle stop planner (autowarefoundation#300)

* rename before v0.8.0 update

* Update default parameters of slow_down_planner (autowarefoundation#846)

Signed-off-by: Kenji Miyake <[email protected]>

* Fix missing break (autowarefoundation#855)

Signed-off-by: wep21 <[email protected]>

* fix slow down param (autowarefoundation#871)

* Fix typos in planning modules (autowarefoundation#866)

* fix typos in planning

* fix corresponding typos in planning

* revert fixed typos temporarily due to its impact on launchers

* enable to change acc_param file from arg (autowarefoundation#997)

* Feature/refactor obstacle stop planner (autowarefoundation#1000)

* devide functions

* add StopPoint and SlowDownPoint structure

* apply clang-format 6.0

* Feature/update adaptive cruise control (autowarefoundation#995)

* avoid upper velocity chattering

* add option to use  rough velocity estimation

* fix typo

* apply format

* fix comment

* change min velocity by PID (autowarefoundation#1008)

* Feature/obstacle stop after goal margin (autowarefoundation#957)

* devide functions

* add StopPoint and SlowDownPoint structure

* apply clang-format 6.0

* add extend trajectory method

* change extend_distance default value to 0.0

* does not insert when insert point over output_msg points size

* fix acc limit (autowarefoundation#1015)

* Feature/compensate lidar delay to acc (autowarefoundation#1032)

* add compensation to calc dist

* change compensation way

* delete debug output

* separate emerngecy stop param (autowarefoundation#1033)

* improve accuracy of insert stop pose (autowarefoundation#1034)

* add error handling (autowarefoundation#1037)

* expand stop param (autowarefoundation#1068)

* add param

* add awapi topic

Co-authored-by: tomoya.kimura <[email protected]>

* fix default param (autowarefoundation#1071)

* Add expand_stop_range to obstacle_stop_planner.yaml (autowarefoundation#1084)

Signed-off-by: Kenji Miyake <[email protected]>

* Revert "rename before v0.8.0 update"

This reverts commit 395c529ee1a854db35ee4f4d517da2dfb401914c.

* fix clock & restore include guard

* fix code style

* fix node

* fix params

* nest acc params

Co-authored-by: Kenji Miyake <[email protected]>
Co-authored-by: Daisuke Nishimatsu <[email protected]>
Co-authored-by: Taichi Higashide <[email protected]>
Co-authored-by: Kazuki Miyahara <[email protected]>
Co-authored-by: tkimura4 <[email protected]>
Co-authored-by: Yukihiro Saito <[email protected]>

* Ros2 v0.8.0 remove std msgs awapi (autowarefoundation#348)

* [autoware_vehicle_msgs] add BatteryStatus msg

Signed-off-by: mitsudome-r <[email protected]>

* [autoware_planning_msgs] add ExpandStopRange and StopSpeedExceeded messages

Signed-off-by: mitsudome-r <[email protected]>

* [autoware_api_msgs] add DoorControlCommand, StopCommand, and VelocityLimit messages

Signed-off-by: mitsudome-r <[email protected]>

* remove std_msgs related to autoware_awaiv_adapter node

Signed-off-by: mitsudome-r <[email protected]>

* apply ament_uncrustify

Signed-off-by: mitsudome-r <[email protected]>

* fix build failure

Signed-off-by: mitsudome-r <[email protected]>

* fix test failures

Signed-off-by: mitsudome-r <[email protected]>

* address review commends

Signed-off-by: mitsudome-r <[email protected]>

* Rename ROS-related .yaml to .param.yaml (autowarefoundation#352)

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

Signed-off-by: Kenji Miyake <[email protected]>

* Remove prefix 'default_' of yaml files

Signed-off-by: Kenji Miyake <[email protected]>

* Rename vehicle_info.yaml to vehicle_info.param.yaml

Signed-off-by: Kenji Miyake <[email protected]>

* Rename diagnostic_aggregator's param files

Signed-off-by: Kenji Miyake <[email protected]>

* Fix overlooked parameters

Signed-off-by: Kenji Miyake <[email protected]>

* fix insert slow down point bug (autowarefoundation#1159) (autowarefoundation#376)

* fix slow down insert slow down point bug

* refactor code

Co-authored-by: Taichi Higashide <[email protected]>

* fix implement miss (autowarefoundation#409)

* Sensor data qos (autowarefoundation#407)

* Use sensor data qos for pointcloud preprocessor

Signed-off-by: Autoware <[email protected]>

* Use sensor data qos for pointcloud

Signed-off-by: Autoware <[email protected]>

* Fix lint

Signed-off-by: wep21 <[email protected]>

* Use sensor data qos for livox tag filter and vector map filter

Signed-off-by: wep21 <[email protected]>

* Fix lint

Signed-off-by: wep21 <[email protected]>

Co-authored-by: Autoware <[email protected]>

* Ros2 fix topic name part1 (autowarefoundation#408)

* Fix topic name of lane_departure_checker debug

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of mpc_follower debug

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of velocity_controller debug

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of motion_velocity_optimizer debug

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of lane_change_planner debug

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of behavior_velocity_planner debug

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of obstacle_avoidance_planner debug

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of behavior_velocity_planner

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of motion_velocity_optimizer

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of lane_departure_checker

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of mpc_follower

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of behavior_velocity_planner

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of velocity_controller

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of lane_change_planner

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of obstacle_avoidance_planner

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of obstacle_stop_planner

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of costmap_generator

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of freespace_planner

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of surround_obstacle_checker

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of costmap_generator

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of emergency_handler

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix lint errors

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix typo

Signed-off-by: Takagi, Isamu <[email protected]>

* add use_sim-time option (autowarefoundation#454)

* Fix rolling build errors (autowarefoundation#1225)

* Add missing include files

Signed-off-by: Kenji Miyake <[email protected]>

* Replace rclcpp::Duration

Signed-off-by: Kenji Miyake <[email protected]>

* Use reference for exceptions

Signed-off-by: Kenji Miyake <[email protected]>

* Use from_seconds

Signed-off-by: Kenji Miyake <[email protected]>

* Sync public repo (autowarefoundation#1228)

* [simple_planning_simulator] add readme (autowarefoundation#424)

* add readme of simple_planning_simulator

Signed-off-by: Takamasa Horibe <[email protected]>

* 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 <[email protected]>

* 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 <[email protected]>

* Fix topic name of traffic_light_visualization

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix topic name of traffic_light_map_based_detector

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix lint traffic_light_recognition

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix lint traffic_light_classifier

Signed-off-by: Takagi, Isamu <[email protected]>

* Fix lint traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <[email protected]>

* 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 <[email protected]>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <[email protected]>

* uncrustified

Signed-off-by: Adam Dabrowski <[email protected]>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <[email protected]>

* back to RTD as superclass

Signed-off-by: Adam Dabrowski <[email protected]>

* Rviz overlay render in update (autowarefoundation#465)

* Moved painiting in SteeringAngle plugin to update()

Signed-off-by: Adam Dabrowski <[email protected]>

* super class now back to MFD

Signed-off-by: Adam Dabrowski <[email protected]>

* uncrustified

Signed-off-by: Adam Dabrowski <[email protected]>

* acquire data in mutex

Signed-off-by: Adam Dabrowski <[email protected]>

* removed unnecessary includes and some dead code

Signed-off-by: Adam Dabrowski <[email protected]>

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

Signed-off-by: Adam Dabrowski <[email protected]>

* restored RTD superclass

Signed-off-by: Adam Dabrowski <[email protected]>

Co-authored-by: Takamasa Horibe <[email protected]>
Co-authored-by: tkimura4 <[email protected]>
Co-authored-by: Takagi, Isamu <[email protected]>
Co-authored-by: Kazuki Miyahara <[email protected]>
Co-authored-by: Makoto Tokunaga <[email protected]>
Co-authored-by: Adam Dąbrowski <[email protected]>

* Unify Apache-2.0 license name (autowarefoundation#1242)

* Make planning modules components (autowarefoundation#1263)

Signed-off-by: wep21 <[email protected]>

* fix miss spell (autowarefoundation#1268)

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <[email protected]>

* Refactor vehicle info util (autowarefoundation#1305)

* Update license

Signed-off-by: Kenji Miyake <[email protected]>

* Refactor vehicle_info_util

Signed-off-by: Kenji Miyake <[email protected]>

* Rename and split files

Signed-off-by: Kenji Miyake <[email protected]>

* Fix interfaces

Signed-off-by: Kenji Miyake <[email protected]>

* Fix bug and add error handling

Signed-off-by: Kenji Miyake <[email protected]>

* Add "// namespace"

Signed-off-by: Kenji Miyake <[email protected]>

* Add missing include

Signed-off-by: Kenji Miyake <[email protected]>

* add config file of plot juggler (autowarefoundation#1328)

* 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 <[email protected]>

* 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 <[email protected]>
Co-authored-by: pre-commit <[email protected]>
Co-authored-by: Kenji Miyake <[email protected]>

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <[email protected]>

* Fix mistake

Signed-off-by: Kenji Miyake <[email protected]>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <[email protected]>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <[email protected]>

Co-authored-by: Hiroki OTA <[email protected]>

* Fix compiler warnings (autowarefoundation#1837)

* Fix -Wunused-private-field

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wunused-variable

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wformat-security

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Winvalid-constexpr

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wdelete-non-abstract-non-virtual-dtor

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wdelete-abstract-non-virtual-dtor

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Winconsistent-missing-override

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wrange-loop-construct

Signed-off-by: Kenji Miyake <[email protected]>

* Fix "invalid application of 'sizeof' to an incomplete type"

Signed-off-by: Kenji Miyake <[email protected]>

* Ignore -Wgnu-anonymous-struct and -Wnested-anon-types

Signed-off-by: Kenji Miyake <[email protected]>

* Fix lint

Signed-off-by: Kenji Miyake <[email protected]>

* Ignore -Wno-deprecated-declarations in CUDA-related packages

Signed-off-by: Kenji Miyake <[email protected]>

* Fix mistake

Signed-off-by: Kenji Miyake <[email protected]>

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <[email protected]>

* suppress warnings for planning (autowarefoundation#1893)

* add Werror

* add maybe_unused

* fix estm -> estimate

* fix some typos (autowarefoundation#1941)

* fix some typos

* fix typo

* Fix typo

Signed-off-by: Kenji Miyake <[email protected]>

Co-authored-by: Kenji Miyake <[email protected]>

* fix calc_dist in acc (autowarefoundation#1945)

* fix calc_dist in acc

* use baselink2front

* remove unused variable

* fix

* 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/changeable slow down margin (autowarefoundation#1546) (autowarefoundation#1726)

* Feature/changeable slow down margin (autowarefoundation#1546)

* start slow down at SlowDownStart

* add publisher for debug

* update slow down planner implementation

* delete unnecessary publisher

* change function name

* apply clang-format

* refactoring

* refactoring & apply clang-format

* add virtual wall marker

Co-authored-by: satoshi-ota <[email protected]>

* Fix typo

Signed-off-by: wep21 <[email protected]>

* Apply uncrustify

Signed-off-by: wep21 <[email protected]>

* Apply cpplint

Signed-off-by: wep21 <[email protected]>

* Fix package.xml

Signed-off-by: wep21 <[email protected]>

* Remove unused variable

Signed-off-by: wep21 <[email protected]>

Co-authored-by: Satoshi OTA <[email protected]>
Co-authored-by: satoshi-ota <[email protected]>

* Fix/not appropriate velocity overwrite (autowarefoundation#1750)

* fix not appropriate velocity overwrite

* apply format

* cleanup/wrap planner params by using struct (autowarefoundation#1788)

* wrap planner params by using struct

* move bg alias & add address operator

* improve readability of loading parameters

* remove unsuitable vehicle_info alias

* remove unsuitable slow_down/stop planner param alias

* add struct NodeParam

* move using define

* change param order

Co-authored-by: satoshi-ota <[email protected]>

* improvement/replace several lines with autoware_utils func (autowarefoundation#1783)

* clean up

* remove redundant blank line

* LINE_STRIP -> LINE_LIST

Co-authored-by: satoshi-ota <[email protected]>

* Refactor/obstacle stop planner (autowarefoundation#1811)

* use autoware_utils getRPY()

* use point alias & rename variables

* use TrajectoryPoint instead of Eigen::Vector2d

* split insertSlowDownSection() into two part

* use trajectory point alias

* change variables order

* fix build error

* use std::numeric_limits::epsilon() & small change

* add address operator

* fix typo

* apply format

* update TrajectoryPoint insert condition

* use std::hypot

* change alias name

* move using alias & remove unnesessary include

* remove an unnecessary variable

* improve readability

* undo the insert min dist

* remane variables & improve readability

* fix redundant variables name

* tmp : remove scope

* remove unnecessary alias

* rename alias to improve readability

* p_end -> p_back

* undo partial rename

* undo calc order modification

* undo split insertSlowDownSection() into two parts

* prevent TrajectoryPoint overlapping

* remove unnecessary file

* undo logic modification

* initialize struct

* fix constraints

* fix indent

* improve struct initialize

* remove redundant address operator

* initialize TrajectoryPoint

* guard invalid index

* remove std::min

Co-authored-by: satoshi-ota <[email protected]>

* Fix/obstacle search radius in obstacle stop planner (autowarefoundation#1823)

* guard activation both of slow down and stop by a collision obstacle

* fix obstacle search radius bug

* fix indent

* rename variables

* not deactivate slow down planning

* initialize index

* remove .swp

Co-authored-by: satoshi-ota <[email protected]>

* set initial value of lowpass filter in acc (autowarefoundation#1880)

* set initial value of lowpass filter in acc

* add space

* Feature/add obstacle stop doc (autowarefoundation#1890)

* add base doc.

* add svg files

* add flow chart

* add roles

* fix svg

* add discription of flowchart

* add figure to readme

* fix typo

* add plantuml

* fix typo

* upper velocity -> target velocity

* add svg

* add more dicription of stop/slow down planner

* add adaptive cruise document

* fix typo

* apply markdown lint

* fix format

* fix format

* fix svg

* fix typo

* fix format

* Update planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/README.md

Co-authored-by: taikitanaka3 <[email protected]>

* Update planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/README.md

Co-authored-by: taikitanaka3 <[email protected]>

* add adaptive cruise description

* fix deceleration range

* fix description of slow down planner

Co-authored-by: taikitanaka3 <[email protected]>

* add maybe unused (autowarefoundation#1908)

* prevent chattering in adaptive cruise  (autowarefoundation#1954)

* fix typo

* update readme

* add description

* add two threhold of obstacle velocity

* fix calc_dist in acc (autowarefoundation#1945)

* fix calc_dist in acc

* use baselink2front

* remove unused variable

* fix

* remove unused arg (autowarefoundation#1971)

* improve/commonize calc  insert point function (autowarefoundation#1825)

* add insert function

* fix typo

* remove unnecessary address operator

* use common function

* index small change

* rename variables

* fix loop condition

* fix loop condition no use static_cast

* remove .swp

* update insert inplementation

* fix dist_remain plus/minus

* improve readability

Co-authored-by: satoshi-ota <[email protected]>

* Fix package.xml (autowarefoundation#2056)

Signed-off-by: Kenji Miyake <[email protected]>

* fix obstacle stop (autowarefoundation#2083) (autowarefoundation#2086)

Co-authored-by: tkimura4 <[email protected]>

* Feature/keep slow down speed until end point of slow down section (autowarefoundation#1985)

* modify logic: not escape slow down until slow down section end

* refactor: add findNearestFrontIndex()

* remove unused argument & function

* improve readability

* add utility func

* modify logic condition

* fix for createQuaternionFromRPY/Yaw (autowarefoundation#2154)

* fix unexpected slow down in sharp curves (autowarefoundation#2181)

* Fix/insert implementation (autowarefoundation#2186)

* Feature/consider jerk by using external velocity limit (autowarefoundation#2158)

* consider jerk and acc constraint in slow down

* use external velocity limit

* consider constraints

* add publisher for debug

* publish jerk/acc constraints

* add undershoot measures

* add plotjuggle config (slow down planner)

* fix unexpected speed up

* update undershoot guard

* bool initialize

* support new VelocityLimit.msg

* smplify velocity limit logic

* improve readability

* clean up debug publisher

* use planner data

* add publishDebugData()

* clean up trajectory preprocess

* add bad index detection

* split a huge process into two parts

* fix typo(threshould -> threshold)

* use limit selector

* add comment

* update plotjuggler config

* update member variables

Co-authored-by: satoshi-ota <[email protected]>

* Fix/use common planning param (autowarefoundation#2246)

* use common planning param

* jerk_min_slow_down -> slow_down_min_jerk

* fix velocity threshold (autowarefoundation#2264)

* Fix/trajectory preprocess (autowarefoundation#2285)

* fix preprocess

* add index guard

* 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 <[email protected]>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <[email protected]>

* Apply Black

Signed-off-by: Kenji Miyake <[email protected]>

* Apply clang-format

Signed-off-by: Kenji Miyake <[email protected]>

* Fix build errors

Signed-off-by: Kenji Miyake <[email protected]>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <[email protected]>

* Apply clang-format

Signed-off-by: Kenji Miyake <[email protected]>

* Fix build errors

Signed-off-by: Kenji Miyake <[email protected]>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <[email protected]>

* Auto/obstacle stop planner (autowarefoundation#531)

* delete COLCON_IGNORE

* port obstacle stop planner

* run pre commit

* add nav_msgs

* type conversion

* [apply_predicted_obj_type] adapt to autoware auto msgs (autowarefoundation#564)

* fix obj shape

* fix obj shape

* fix goal pose

* rename topic name twist -> odometry (autowarefoundation#568)

Co-authored-by: Takayuki Murooka <[email protected]>

* update iv_msgs -> auto_msgs in planning readme (autowarefoundation#576)

* update iv_msgs -> auto_msgs in planning readme

* minor change

* some fix

* some fix

Co-authored-by: Takayuki Murooka <[email protected]>

* Auto/fix obstacle stop planner (#678)

* change trajectory to trajectory point in node

* update

* update

* delete comment

* fix invalidContainerReference

* substitute header to output

Co-authored-by: tomoya.kimura <[email protected]>

* mix readme

* add common param

Co-authored-by: mitsudome-r <[email protected]>
Co-authored-by: Kenji Miyake <[email protected]>
Co-authored-by: Yukihiro Saito <[email protected]>
Co-authored-by: Taichi Higashide <[email protected]>
Co-authored-by: Kenji Miyake <[email protected]>
Co-authored-by: Daisuke Nishimatsu <[email protected]>
Co-authored-by: Kazuki Miyahara <[email protected]>
Co-authored-by: Nikolai Morin <[email protected]>
Co-authored-by: Servando <[email protected]>
Co-authored-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: Esteve Fernandez <[email protected]>
Co-authored-by: Jilada Eccleston <[email protected]>
Co-authored-by: Shinnosuke Hirakawa <[email protected]>
Co-authored-by: Autoware <[email protected]>
Co-authored-by: Takagi, Isamu <[email protected]>
Co-authored-by: Takamasa Horibe <[email protected]>
Co-authored-by: Makoto Tokunaga <[email protected]>
Co-authored-by: Adam Dąbrowski <[email protected]>
Co-authored-by: Yusuke FUJII <[email protected]>
Co-authored-by: Keisuke Shima <[email protected]>
Co-authored-by: pre-commit <[email protected]>
Co-authored-by: Hiroki OTA <[email protected]>
Co-authored-by: Satoshi OTA <[email protected]>
Co-authored-by: satoshi-ota <[email protected]>
Co-authored-by: taikitanaka3 <[email protected]>
Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <[email protected]>
Co-authored-by: Sugatyon <[email protected]>
Co-authored-by: Takayuki Murooka <[email protected]>
  • Loading branch information
1 parent 4d5fc85 commit 357dfae
Show file tree
Hide file tree
Showing 24 changed files with 6,427 additions and 0 deletions.
51 changes: 51 additions & 0 deletions planning/obstacle_stop_planner/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
cmake_minimum_required(VERSION 3.5)
project(obstacle_stop_planner)

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
endif()

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(PCL REQUIRED)

ament_auto_add_library(obstacle_stop_planner SHARED
src/debug_marker.cpp
src/node.cpp
src/adaptive_cruise_control.cpp
)

target_include_directories(obstacle_stop_planner
PUBLIC
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)

target_link_libraries(obstacle_stop_planner
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
)

rclcpp_components_register_node(obstacle_stop_planner
PLUGIN "motion_planning::ObstacleStopPlannerNode"
EXECUTABLE obstacle_stop_planner_node
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(
INSTALL_TO_SHARE
config
launch
)
282 changes: 282 additions & 0 deletions planning/obstacle_stop_planner/README.md

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
/**:
ros__parameters:
adaptive_cruise_control:
# Adaptive Cruise Control (ACC) config
use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not
use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not
consider_obj_velocity: true # consider forward vehicle velocity to ACC or not

# general parameter for ACC
obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s]
obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s]
emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss]
emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s]
min_dist_stop: 4.0 # minimum distance of emergency stop [m]
obstacle_emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss]
max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss]
min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control
standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s]
min_dist_standard: 4.0 # minimum distance in active cruise control [m]
obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss]
margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-]
use_time_compensation_to_calc_distance: true # use time-compensation to calculate distance to forward vehicle

# pid parameter for ACC
p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-]
p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-]
d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-]
d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-]

# parameter for object velocity estimation
object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m]
object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m]
valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s]
valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s]
valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s]
valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s]
thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s]
lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity
use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####)
rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value
15 changes: 15 additions & 0 deletions planning/obstacle_stop_planner/config/common.param.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
/**:
ros__parameters:
# constraints param for normal driving
normal:
min_acc: -0.5 # min deceleration [m/ss]
max_acc: 1.0 # max acceleration [m/ss]
min_jerk: -0.5 # min jerk [m/sss]
max_jerk: 1.0 # max jerk [m/sss]

# constraints to be observed
limit:
min_acc: -2.5 # min deceleration limit [m/ss]
max_acc: 1.0 # max acceleration limit [m/ss]
min_jerk: -1.5 # min jerk limit [m/sss]
max_jerk: 1.5 # max jerk limit [m/sss]
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
/**:
ros__parameters:
stop_planner:
stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m]
step_length: 1.0 # step length for pointcloud search range [m]
extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
expand_stop_range: 0.0 # margin of vehicle footprint [m]

slow_down_planner:
# slow down planner parameters
forward_margin: 5.0 # margin distance from slow down point to vehicle front [m]
backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m]
expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
max_slow_down_vel: 1.38 # max slow down velocity [m/s]
min_slow_down_vel: 0.28 # min slow down velocity [m/s]

# slow down constraint parameters
consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel
forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s]
forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s]
jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss]
jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
slow_down_vel: 1.38 # target slow down velocity [m/s]
133 changes: 133 additions & 0 deletions planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,133 @@
<?xml version='1.0' encoding='UTF-8'?>
<root version="2.3.8">
<tabbed_widget parent="main_window" name="Main Window">
<Tab containers="1" tab_name="tab1">
<Container>
<DockSplitter orientation="-" count="1" sizes="1">
<DockSplitter orientation="|" count="2" sizes="0.5;0.5">
<DockSplitter orientation="-" count="2" sizes="0.500286;0.499714">
<DockArea name="Estimated Object Velocity">
<plot mode="TimeSeries" style="Lines">
<range bottom="2.587438" top="3.256721" left="48.265130" right="51.387770"/>
<limitY/>
<curve name="" color="" />
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.0" color="#1f77b4">
<transform alias="pcl-based obj vel" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.1" color="#d62728">
<transform alias="dynamic-obj-based obj vel" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.2" color="#1ac938">
<transform alias="final estimated obj vel" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
</plot>
</DockArea>
<DockArea name="Distance To Forward Object">
<plot mode="TimeSeries" style="Lines">
<range bottom="21.768126" top="22.395437" left="48.265130" right="51.387770"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.3" color="#ff7f0e">
<transform alias="dist" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter orientation="-" count="2" sizes="0.500286;0.499714">
<DockArea name="Ego Velocity">
<plot mode="TimeSeries" style="Lines">
<range bottom="-0.045951" top="1.884001" left="45.299350" right="51.387770"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.4" color="#f14cc1">
<transform alias="Current Velocity" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.8" color="#9467bd">
<transform alias="Raw Upper Velocity" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values/data.9" color="#17becf">
<transform alias="Upper Velocity" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
<curve name="/control/trajectory_follower/velocity_controller/debug_values/data.2" color="#bcbd22">
<transform alias="Target Velocity" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
<curve name="/control/trajectory_follower/velocity_controller/debug_values/data.4" color="#1f77b4">
<transform alias="Closest Velocity" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
</plot>
</DockArea>
<DockArea name="Accel">
<plot mode="TimeSeries" style="Lines">
<range bottom="-0.002602" top="0.106692" left="45.299350" right="49.966869"/>
<limitY/>
<curve name="/control/trajectory_follower/velocity_controller/debug_values/data.25" color="#d62728">
<transform alias="Current Accel" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
<curve name="/control/trajectory_follower/velocity_controller/debug_values/data.3" color="#1ac938">
<transform alias="Target Accel" name="Scale/Offset">
<options value_offset="0" value_scale="1.0" time_offset="0"/>
</transform>
</curve>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<currentTabIndex index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad CSV">
<default time_axis=""/>
</plugin>
<plugin ID="DataLoad ROS2 bags">
<use_header_stamp value="false"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
</plugin>
<plugin ID="DataLoad ULog"/>
<plugin ID="MQTT Subscriber"/>
<plugin ID="ROS2 Topic Subscriber">
<use_header_stamp value="false"/>
<discard_large_arrays value="true"/>
<max_array_size value="true"/>
<selected_topics>
<topic name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug_values"/>
<topic name="/control/trajectory_follower/velocity_controller/debug_values"/>
</selected_topics>
</plugin>
<plugin ID="UDP Server"/>
<plugin ID="WebSocket Server"/>
<plugin ID="ZMQ Subscriber"/>
<plugin status="idle" ID="ROS2 Topic Re-Publisher"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>
93 changes: 93 additions & 0 deletions planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
<?xml version='1.0' encoding='UTF-8'?>
<root version="2.3.8">
<tabbed_widget name="Main Window" parent="main_window">
<Tab containers="1" tab_name="tab1">
<Container>
<DockSplitter sizes="0.503972;0.496028" orientation="-" count="2">
<DockArea name="...">
<plot mode="TimeSeries" style="Lines">
<range top="8.000000" bottom="-2.500000" left="96.194865" right="196.174884"/>
<limitY min="-2.5" max="8"/>
<curve name="/planning/scenario_planning/motion_velocity_smoother/closest_acceleration/data" color="#fff700"/>
<curve name="/planning/scenario_planning/motion_velocity_smoother/closest_jerk/data" color="#ffffff"/>
<curve name="/planning/scenario_planning/motion_velocity_smoother/closest_velocity/data" color="#6117cf"/>
<curve name="/planning/scenario_planning/motion_velocity_smoother/closest_max_velocity/data" color="#949cfe"/>
<curve name="/localization/twist/twist/linear/x" color="#1f77b4"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.1" color="#d62728"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.4" color="#1ac938"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.5" color="#ff7f0e"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.7" color="#abfff2"/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.6" color="#10001f"/>
</plot>
</DockArea>
<DockSplitter sizes="0.5;0.5" orientation="|" count="2">
<DockSplitter sizes="0.500891;0.499109" orientation="-" count="2">
<DockArea name="...">
<plot mode="TimeSeries" style="Dots">
<range top="46.453422" bottom="-2.151529" left="96.202174" right="196.174082"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.3" color="#17becf"/>
</plot>
</DockArea>
<DockArea name="...">
<plot mode="TimeSeries" style="Lines">
<range top="0.100000" bottom="-0.100000" left="96.202174" right="196.174082"/>
<limitY/>
<curve name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.2" color="#bcbd22"/>
</plot>
</DockArea>
</DockSplitter>
<DockArea name="...">
<plot mode="XYPlot" style="Dots">
<range top="10.000000" bottom="0.000000" left="-2.151529" right="46.453422"/>
<limitY min="0" max="10"/>
<curve curve_y="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.0" name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.[3;0]" curve_x="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values/data.3" color="#5dff5f"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<currentTabIndex index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad CSV">
<default time_axis=""/>
</plugin>
<plugin ID="DataLoad ROS2 bags">
<use_header_stamp value="false"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
</plugin>
<plugin ID="DataLoad ULog"/>
<plugin ID="ROS2 Topic Subscriber">
<use_header_stamp value="false"/>
<discard_large_arrays value="false"/>
<max_array_size value="true"/>
<selected_topics>
<topic name="/planning/scenario_planning/max_velocity"/>
<topic name="/planning/scenario_planning/motion_velocity_smoother/closest_acceleration"/>
<topic name="/planning/scenario_planning/motion_velocity_smoother/closest_jerk"/>
<topic name="/planning/scenario_planning/motion_velocity_smoother/closest_max_velocity"/>
<topic name="/planning/scenario_planning/motion_velocity_smoother/closest_velocity"/>
<topic name="/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/debug_values"/>
<topic name="/localization/twist"/>
</selected_topics>
</plugin>
<plugin ID="UDP Server"/>
<plugin ID="WebSocket Server"/>
<plugin ID="ZMQ Subscriber"/>
<plugin ID="CSV Exporter" status="idle"/>
<plugin ID="ROS2 Topic Re-Publisher" status="idle"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>
Loading

0 comments on commit 357dfae

Please sign in to comment.