Skip to content

Commit

Permalink
feat add surround obstacle checker package (autowarefoundation#42)
Browse files Browse the repository at this point in the history
* release v0.4.0

* fix bug(return true in getPose) (autowarefoundation#686)

* 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 <kenji.miyake@tier4.jp>

* fix message type

* delete unused message from cmake

* remove stopReasonStamped

* change topic name of stop reasons

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

* 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 surround obstacle state (autowarefoundation#785)

* Cleanup code

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

* Add state to surround_obstacle_checker

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

* Guard pushObstaclePoint for StopFactor

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

* remove ROS1 packages temporarily

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

* Revert "remove ROS1 packages temporarily"

This reverts commit 623dc7f7b59bb12639c5ca768f87b8e2d24c19b7.

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

* add COLCON_IGNORE to ros1 packages

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

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

* Surround obstacle checker (autowarefoundation#64)

* Port surround_obstacle_checker to ROS2

* Reviewer comment

* Review comment

* Fix launch files (autowarefoundation#122)

* [surround_obstacle_checker] add parameter and arguments to launch file

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

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

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

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

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

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

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

* 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

* Fix lints in surround_obstacle_checker package (autowarefoundation#130)

* Replace waitForTransform by lookupTransform in surround_obstacle_checker (autowarefoundation#212)

* Fix erroneous waitForTransform in surround_obstacle_checker

* Line length fix

* Ros2 v0.8.0 surround obstacle checker (autowarefoundation#328)

* 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>

* rename vehicle_info_param to vehicle_param_file (autowarefoundation#353)

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* Sensor data qos (autowarefoundation#407)

* Use sensor data qos for pointcloud preprocessor

Signed-off-by: Autoware <autoware@tier4.jp>

* Use sensor data qos for pointcloud

Signed-off-by: Autoware <autoware@tier4.jp>

* Fix lint

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

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

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

* Fix lint

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

Co-authored-by: Autoware <autoware@tier4.jp>

* Ros2 fix topic name part1 (autowarefoundation#408)

* Fix topic name of lane_departure_checker debug

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

* Fix topic name of mpc_follower debug

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

* Fix topic name of velocity_controller debug

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

* Fix topic name of motion_velocity_optimizer debug

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

* Fix topic name of lane_change_planner debug

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

* Fix topic name of behavior_velocity_planner debug

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

* Fix topic name of obstacle_avoidance_planner debug

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

* Fix topic name of behavior_velocity_planner

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

* Fix topic name of motion_velocity_optimizer

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

* Fix topic name of lane_departure_checker

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

* Fix topic name of mpc_follower

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

* Fix topic name of behavior_velocity_planner

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

* Fix topic name of velocity_controller

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

* Fix topic name of lane_change_planner

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

* Fix topic name of obstacle_avoidance_planner

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

* Fix topic name of obstacle_stop_planner

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

* Fix topic name of costmap_generator

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

* Fix topic name of freespace_planner

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

* Fix topic name of surround_obstacle_checker

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

* Fix topic name of costmap_generator

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

* Fix topic name of emergency_handler

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

* Fix lint errors

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

* Fix typo

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

* add use_sim-time option (autowarefoundation#454)

* 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>

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

* Make planning modules components (autowarefoundation#1263)

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

* 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 -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>

* Add surround_obstacle_checker document (autowarefoundation#1895)

* Add surround_obstacle_checker document

* Remove unnecessary unit

* Fix stop condition

* Fix japanese doc and add english doc

* Fix typo

* Add detail information

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

* Fix figure files and explainations

* Add known limits

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

* 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

* 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>

* port surround obstacle checker (autowarefoundation#493)

* change trajectory msg

* change dynamicobject to predictedobject

* run pre-commit

* delete colcon ignore

* deal with non polygon shape

* replace twist with odometry

* rename to README.md (autowarefoundation#550)

* rename to README.md

* dealt with new auto_msgs format

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* [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 <takayuki.murooka@tier4.jp>

* 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 <takayuki.murooka@tier4.jp>

* Auto/fix surround obstacle checker (autowarefoundation#684)

* change trajectory to trajectorypoints in function

* add header to output

* use output_trajectory_points

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

* Fix no ground pointcloud topic name (autowarefoundation#733)

Signed-off-by: j4tfwm6z <proj-jpntaxi@tier4.jp>

Co-authored-by: j4tfwm6z <proj-jpntaxi@tier4.jp>

* fix/rename segmentation namespace (autowarefoundation#742)

* rename segmentation directory

* fix namespace: system stack

* fix namespace: planning

* fix namespace: control stack

* fix namespace: perception stack

* fix readme

* ci(pre-commit): autofix

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Autoware <autoware@tier4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.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>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: j4tfwm6z <proj-jpntaxi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@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 597ff72 commit 09cb841
Show file tree
Hide file tree
Showing 13 changed files with 1,176 additions and 0 deletions.
44 changes: 44 additions & 0 deletions planning/surround_obstacle_checker/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
cmake_minimum_required(VERSION 3.5)
project(surround_obstacle_checker)

### Compile options
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)
endif()

find_package(ament_cmake REQUIRED)
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED COMPONENTS common)
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

ament_auto_add_library(${PROJECT_NAME} SHARED
src/debug_marker.cpp
src/node.cpp
)

target_link_libraries(${PROJECT_NAME}
${PCL_LIBRARIES}
)

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "SurroundObstacleCheckerNode"
EXECUTABLE ${PROJECT_NAME}_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
)
82 changes: 82 additions & 0 deletions planning/surround_obstacle_checker/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
# Surround Obstacle Checker

## Purpose

`surround_obstacle_checker` is a module to prevent moving if any obstacles is near stopping ego vehicle.
This module runs only when ego vehicle is stopping.

## Inner-workings / Algorithms

### Flow chart

![surround_obstacle_checker_flow](./media/surround_obstacle_checker_flow.svg)

![check_distance](./media/check_distance.drawio.svg)

### Algorithms

### Check data

Check that `surround_obstacle_checker` receives no ground pointcloud, dynamic objects and current velocity data.

### Get distance to nearest object

Calculate distance between ego vehicle and the nearest object.
In this function, it calculates the minimum distance between the polygon of ego vehicle and all points in pointclouds and the polygons of dynamic objects.

### Stop requirement

If it satisfies all following conditions, it plans stopping.

- Ego vehicle is stopped
- It satisfies any following conditions
1. The distance to nearest obstacle satisfies following conditions
- If state is `State::PASS`, the distance is less than `surround_check_distance`
- If state is `State::STOP`, the distance is less than `surround_check_recover_distance`
2. If it does not satisfies the condition in 1, elapsed time from the time it satisfies the condition in 1 is less than `state_clear_time`

### States

To prevent chattering, `surround_obstacle_checker` manages two states.
As mentioned in stop condition section, it prevents chattering by changing threshold to find surround obstacle depending on the states.

- `State::PASS` : Stop planning is released
- `State::STOP` :While stop planning

## Inputs / Outputs

### Input

| Name | Type | Description |
| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ |
| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory |
| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid |
| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects |
| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist |
| `/tf` | `tf2_msgs::msg::TFMessage` | TF |
| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static |

### Output

| Name | Type | Description |
| -------------------------- | ---------------------------------------------- | ------------------------ |
| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory |
| `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus` | No start reason |
| `~/output/stop_reasons` | `autoware_planning_msgs::msg::StopReasonArray` | Stop reasons |
| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization |

## Parameters

| Name | Type | Description | Default value |
| :-------------------------------- | :------- | :------------------------------------------------------------------------------------- | :------------ |
| `use_pointcloud` | `bool` | Use pointcloud as obstacle check | `true` |
| `use_dynamic_object` | `bool` | Use dynamic object as obstacle check | `true` |
| `surround_check_distance` | `double` | If objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] | 0.5 |
| `surround_check_recover_distance` | `double` | If no object exists in this distance, transit to "non-surrounding-obstacle" status [m] | 0.8 |
| `state_clear_time` | `double` | Threshold to clear stop state [s] | 2.0 |
| `stop_state_ego_speed` | `double` | Threshold to check ego vehicle stopped [m/s] | 0.1 |

## Assumptions / Known limits

To perform stop planning, it is necessary to get obstacle pointclouds data.
Hence, it does not plan stopping if the obstacle is in blind spot.
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
/**:
ros__parameters:
# obstacle check
use_pointcloud: true # use pointcloud as obstacle check
use_dynamic_object: true # use dynamic object as obstacle check
surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m]
state_clear_time: 2.0

# ego stop state
stop_state_ego_speed: 0.1 #[m/s]
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
// Copyright 2020 Tier IV, Inc.
//
// 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 SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
#define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_

#include <rclcpp/rclcpp.hpp>

#include <autoware_planning_msgs/msg/stop_reason_array.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <memory>
#include <string>

enum class PoseType : int8_t { NoStart = 0 };
enum class PointType : int8_t { NoStart = 0 };
class SurroundObstacleCheckerDebugNode
{
public:
explicit SurroundObstacleCheckerDebugNode(
const double base_link2front, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node);

bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type);
bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type);
void publish();

private:
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
rclcpp::Publisher<autoware_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reason_pub_;
double base_link2front_;

visualization_msgs::msg::MarkerArray makeVisualizationMarker();
autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray();

std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
std::shared_ptr<geometry_msgs::msg::Pose> stop_pose_ptr_;
rclcpp::Clock::SharedPtr clock_;
};

#endif // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,128 @@
// Copyright 2020 Tier IV, Inc.
//
// 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 SURROUND_OBSTACLE_CHECKER__NODE_HPP_
#define SURROUND_OBSTACLE_CHECKER__NODE_HPP_

#include "autoware_utils/trajectory/tmp_conversion.hpp"
#include "surround_obstacle_checker/debug_marker.hpp"

#include <rclcpp/rclcpp.hpp>
#include <vehicle_info_util/vehicle_info_util.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <diagnostic_msgs/msg/key_value.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/format.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <memory>
#include <string>
#include <vector>

using Point2d = boost::geometry::model::d2::point_xy<double>;
using Polygon2d =
boost::geometry::model::polygon<Point2d, false, false>; // counter-clockwise, open
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using TrajectoryPoints = std::vector<TrajectoryPoint>;

enum class State { PASS, STOP };

class SurroundObstacleCheckerNode : public rclcpp::Node
{
public:
explicit SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options);

private:
void pathCallback(const Trajectory::ConstSharedPtr input_msg);
void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg);
void dynamicObjectCallback(
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg);
void currentVelocityCallback(const nav_msgs::msg::Odometry::ConstSharedPtr input_msg);
void insertStopVelocity(const size_t closest_idx, TrajectoryPoints * traj);
bool convertPose(
const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target,
const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose);
bool getPose(
const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose);
void getNearestObstacle(double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
void getNearestObstacleByPointCloud(
double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
void getNearestObstacleByDynamicObject(
double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
bool isObstacleFound(const double min_dist_to_obj);
bool isStopRequired(const bool is_obstacle_found, const bool is_stopped);
size_t getClosestIdx(const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose);
bool checkStop(const TrajectoryPoint & closest_point);
Polygon2d createSelfPolygon();
Polygon2d createObjPolygon(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size);
Polygon2d createObjPolygon(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint);
diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag(
const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose);
std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose);

/*
* ROS
*/
// publisher and subscriber
rclcpp::Subscription<Trajectory>::SharedPtr path_sub_;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr
dynamic_object_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr current_velocity_sub_;
rclcpp::Publisher<Trajectory>::SharedPtr path_pub_;
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr stop_reason_diag_pub_;
std::shared_ptr<SurroundObstacleCheckerDebugNode> debug_ptr_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

// parameter
nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_;
sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_;
autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr_;
vehicle_info_util::VehicleInfo vehicle_info_;
Polygon2d self_poly_;
bool use_pointcloud_;
bool use_dynamic_object_;
double surround_check_distance_;
// For preventing chattering,
// surround_check_recover_distance_ must be bigger than surround_check_distance_
double surround_check_recover_distance_;
double state_clear_time_;
double stop_state_ego_speed_;
bool is_surround_obstacle_;

// State Machine
State state_ = State::PASS;
std::shared_ptr<const rclcpp::Time> last_obstacle_found_time_;
};

#endif // SURROUND_OBSTACLE_CHECKER__NODE_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
<launch>
<arg name="param_path" default="$(find-pkg-share surround_obstacle_checker)/config/surround_obstacle_checker.param.yaml" />

<arg name="input_trajectory" default="input/trajectory" />
<arg name="input_objects" default="/perception/object_recognition/objects" />
<arg name="input_odometry" default="/localization/kinematic_state" />
<arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud" />
<arg name="output_trajectory" default="output/trajectory" />

<node pkg="surround_obstacle_checker" exec="surround_obstacle_checker_node" name="surround_obstacle_checker" output="screen">
<param from="$(var param_path)" />
<remap from="~/output/no_start_reason" to="/planning/scenario_planning/status/no_start_reason" />
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons" />
<remap from="~/output/trajectory" to="$(var output_trajectory)" />
<remap from="~/input/pointcloud" to="$(var input_pointcloud)" />
<remap from="~/input/objects" to="$(var input_objects)" />
<remap from="~/input/odometry" to="$(var input_odometry)" />
<remap from="~/input/trajectory" to="$(var input_trajectory)" />
</node>

</launch>
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading

0 comments on commit 09cb841

Please sign in to comment.