From 09cb841a3fe9371ae56b970d86b580b8e4b6c016 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 3 Dec 2021 16:47:27 +0900 Subject: [PATCH] feat add surround obstacle checker package (#42) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * release v0.4.0 * fix bug(return true in getPose) (#686) * Feature/stop reason (#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 * fix message type * delete unused message from cmake * remove stopReasonStamped * change topic name of stop reasons Co-authored-by: Kenji Miyake * Fix/stop reason (#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 (#785) * Cleanup code Signed-off-by: Kenji Miyake * Add state to surround_obstacle_checker Signed-off-by: Kenji Miyake * Guard pushObstaclePoint for StopFactor Signed-off-by: Kenji Miyake * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit 623dc7f7b59bb12639c5ca768f87b8e2d24c19b7. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * Surround obstacle checker (#64) * Port surround_obstacle_checker to ROS2 * Reviewer comment * Review comment * Fix launch files (#122) * [surround_obstacle_checker] add parameter and arguments to launch file Signed-off-by: mitsudome-r * [obstacle_stop_planner] modify launch file to remap trajectory from argument Signed-off-by: mitsudome-r * [obstacle_avoidance_planner] modify launch file to remap topics from arguments Signed-off-by: mitsudome-r * [motion_velocity_optimizer] modify launch file to enable remapping from argument Signed-off-by: mitsudome-r * Convert calls of Duration to Duration::from_seconds where appropriate (#131) * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Fix lints in surround_obstacle_checker package (#130) * Replace waitForTransform by lookupTransform in surround_obstacle_checker (#212) * Fix erroneous waitForTransform in surround_obstacle_checker * Line length fix * Ros2 v0.8.0 surround obstacle checker (#328) * Rename ROS-related .yaml to .param.yaml (#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake * Fix overlooked parameters Signed-off-by: Kenji Miyake * rename vehicle_info_param to vehicle_param_file (#353) Signed-off-by: Kosuke Murakami * Sensor data qos (#407) * Use sensor data qos for pointcloud preprocessor Signed-off-by: Autoware * Use sensor data qos for pointcloud Signed-off-by: Autoware * Fix lint Signed-off-by: wep21 * Use sensor data qos for livox tag filter and vector map filter Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 Co-authored-by: Autoware * Ros2 fix topic name part1 (#408) * Fix topic name of lane_departure_checker debug Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower debug Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller debug Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer debug Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer Signed-off-by: Takagi, Isamu * Fix topic name of lane_departure_checker Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_stop_planner Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of freespace_planner Signed-off-by: Takagi, Isamu * Fix topic name of surround_obstacle_checker Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of emergency_handler Signed-off-by: Takagi, Isamu * Fix lint errors Signed-off-by: Takagi, Isamu * Fix typo Signed-off-by: Takagi, Isamu * add use_sim-time option (#454) * Fix rolling build errors (#1225) * Add missing include files Signed-off-by: Kenji Miyake * Replace rclcpp::Duration Signed-off-by: Kenji Miyake * Use reference for exceptions Signed-off-by: Kenji Miyake * Use from_seconds Signed-off-by: Kenji Miyake * Sync public repo (#1228) * [simple_planning_simulator] add readme (#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (#460) * Fix pose2twist (#462) Signed-off-by: Takagi, Isamu * Ros2 vehicle info param server (#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix issues in hdd_reader (#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (#463) * Rviz overlay render fix (#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * back to RTD as superclass Signed-off-by: Adam Dabrowski * Rviz overlay render in update (#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski * restored RTD superclass Signed-off-by: Adam Dabrowski Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski * Unify Apache-2.0 license name (#1242) * Make planning modules components (#1263) Signed-off-by: wep21 * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Refactor vehicle info util (#1305) * Update license Signed-off-by: Kenji Miyake * Refactor vehicle_info_util Signed-off-by: Kenji Miyake * Rename and split files Signed-off-by: Kenji Miyake * Fix interfaces Signed-off-by: Kenji Miyake * Fix bug and add error handling Signed-off-by: Kenji Miyake * Add "// namespace" Signed-off-by: Kenji Miyake * Add missing include Signed-off-by: Kenji Miyake * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Add surround_obstacle_checker document (#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 (#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 (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * port surround obstacle checker (#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 (#550) * rename to README.md * dealt with new auto_msgs format Co-authored-by: Takayuki Murooka * [apply_predicted_obj_type] adapt to autoware auto msgs (#564) * fix obj shape * fix obj shape * fix goal pose * rename topic name twist -> odometry (#568) Co-authored-by: Takayuki Murooka * update iv_msgs -> auto_msgs in planning readme (#576) * update iv_msgs -> auto_msgs in planning readme * minor change * some fix * some fix Co-authored-by: Takayuki Murooka * Auto/fix surround obstacle checker (#684) * change trajectory to trajectorypoints in function * add header to output * use output_trajectory_points Co-authored-by: tomoya.kimura * Fix no ground pointcloud topic name (#733) Signed-off-by: j4tfwm6z Co-authored-by: j4tfwm6z * fix/rename segmentation namespace (#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 Co-authored-by: tkimura4 Co-authored-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Nikolai Morin 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 Co-authored-by: Autoware Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Kazuki Miyahara Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski Co-authored-by: Hiroki OTA Co-authored-by: Fumiya Watanabe 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 Co-authored-by: Takayuki Murooka Co-authored-by: j4tfwm6z 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> --- .../surround_obstacle_checker/CMakeLists.txt | 44 ++ planning/surround_obstacle_checker/README.md | 82 +++ .../surround_obstacle_checker.param.yaml | 11 + .../debug_marker.hpp | 53 ++ .../surround_obstacle_checker/node.hpp | 128 +++++ .../surround_obstacle_checker.launch.xml | 21 + .../media/check_distance.drawio.svg | 3 + .../media/surround_obstacle_checker_flow.svg | 38 ++ .../media/surround_obstacle_checker_flow.uml | 17 + .../surround_obstacle_checker/package.xml | 39 ++ .../src/debug_marker.cpp | 182 +++++++ .../surround_obstacle_checker/src/node.cpp | 478 ++++++++++++++++++ .../surround_obstacle_checker-design.ja.md | 80 +++ 13 files changed, 1176 insertions(+) create mode 100644 planning/surround_obstacle_checker/CMakeLists.txt create mode 100644 planning/surround_obstacle_checker/README.md create mode 100644 planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml create mode 100644 planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp create mode 100644 planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp create mode 100644 planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml create mode 100644 planning/surround_obstacle_checker/media/check_distance.drawio.svg create mode 100644 planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.svg create mode 100644 planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.uml create mode 100644 planning/surround_obstacle_checker/package.xml create mode 100644 planning/surround_obstacle_checker/src/debug_marker.cpp create mode 100644 planning/surround_obstacle_checker/src/node.cpp create mode 100644 planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt new file mode 100644 index 000000000000..e01a39d8e906 --- /dev/null +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -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 +) diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md new file mode 100644 index 000000000000..9d3729343d8b --- /dev/null +++ b/planning/surround_obstacle_checker/README.md @@ -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. diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml new file mode 100644 index 000000000000..b57d7506b057 --- /dev/null +++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml @@ -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] diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp new file mode 100644 index 000000000000..c9f5d12a8189 --- /dev/null +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -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 + +#include +#include +#include +#include + +#include +#include + +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::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr stop_reason_pub_; + double base_link2front_; + + visualization_msgs::msg::MarkerArray makeVisualizationMarker(); + autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray(); + + std::shared_ptr stop_obstacle_point_ptr_; + std::shared_ptr stop_pose_ptr_; + rclcpp::Clock::SharedPtr clock_; +}; + +#endif // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp new file mode 100644 index 000000000000..586d5d77a305 --- /dev/null +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -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 +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +using Point2d = boost::geometry::model::d2::point_xy; +using Polygon2d = + boost::geometry::model::polygon; // counter-clockwise, open +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +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::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Subscription::SharedPtr + dynamic_object_sub_; + rclcpp::Subscription::SharedPtr current_velocity_sub_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; + std::shared_ptr 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 last_obstacle_found_time_; +}; + +#endif // SURROUND_OBSTACLE_CHECKER__NODE_HPP_ diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml new file mode 100644 index 000000000000..3a2c0eeb8870 --- /dev/null +++ b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/surround_obstacle_checker/media/check_distance.drawio.svg b/planning/surround_obstacle_checker/media/check_distance.drawio.svg new file mode 100644 index 000000000000..49cd44f90f84 --- /dev/null +++ b/planning/surround_obstacle_checker/media/check_distance.drawio.svg @@ -0,0 +1,3 @@ + + +
ego
ego
surround_check_distance
surround_check_dista...
surround_check_recover_distance
surround_check_recover_distance
State::PASS
State::PASS
ego
ego
State::STOP
State::STOP
ego
ego
State::STOP
State::STOP
ego
ego
State::STOP
State::STOP
Obstacle found
Obstacle found
Obstacle is inside of recover distance
Obstacle is inside o...
Obstacle is outside of recover distance
Obstacle is outside...
Elapsed time is over than state_clear_time
Elapsed time is over...
obstacle
obstacleViewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.svg b/planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.svg new file mode 100644 index 000000000000..35f27a466735 --- /dev/null +++ b/planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.svg @@ -0,0 +1,38 @@ +Subscribe trajectoryCheck dataGet distance to nearest objectChange state to State::STOPChange state to State::PASSPublish trajectory, stop reason and debug dataInsert stop pointCreate debug dataIs stop required?yesno \ No newline at end of file diff --git a/planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.uml b/planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.uml new file mode 100644 index 000000000000..1e278b7e606e --- /dev/null +++ b/planning/surround_obstacle_checker/media/surround_obstacle_checker_flow.uml @@ -0,0 +1,17 @@ +@startuml +(*) --> "Subscribe trajectory" +"Subscribe trajectory" --> "Check data" +"Check data" --> "Get distance to nearest object" + +if "Is stop required?" then + -->[yes] "Change state to State::STOP" +else + ->[no] "Change state to State::PASS" +endif + +"Change state to State::PASS" --> "Publish trajectory, stop reason and debug data" +"Change state to State::STOP" --> "Insert stop point" +"Insert stop point" --> "Create debug data" +"Create debug data" --> "Publish trajectory, stop reason and debug data" +"Publish trajectory, stop reason and debug data" --> (*) +@enduml diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml new file mode 100644 index 000000000000..b2929d7a2ff6 --- /dev/null +++ b/planning/surround_obstacle_checker/package.xml @@ -0,0 +1,39 @@ + + + + surround_obstacle_checker + 0.1.0 + The surround_obstacle_checker package + + tier4 + + Apache License 2.0 + + ament_cmake + eigen3_cmake_module + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_planning_msgs + autoware_utils + diagnostic_msgs + eigen + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp new file mode 100644 index 000000000000..2ccfd6782b7f --- /dev/null +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -0,0 +1,182 @@ +// 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. + +#include "surround_obstacle_checker/debug_marker.hpp" + +#include + +#include + +SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( + const double base_link2front, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) +: base_link2front_(base_link2front), clock_(clock) +{ + debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); + stop_reason_pub_ = + node.create_publisher("~/output/stop_reasons", 1); +} + +bool SurroundObstacleCheckerDebugNode::pushPose( + const geometry_msgs::msg::Pose & pose, const PoseType & type) +{ + switch (type) { + case PoseType::NoStart: + stop_pose_ptr_ = std::make_shared(pose); + return true; + default: + return false; + } +} + +bool SurroundObstacleCheckerDebugNode::pushObstaclePoint( + const geometry_msgs::msg::Point & obstacle_point, const PointType & type) +{ + switch (type) { + case PointType::NoStart: + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + default: + return false; + } +} + +void SurroundObstacleCheckerDebugNode::publish() +{ + /* publish debug marker for rviz */ + const auto visualization_msg = makeVisualizationMarker(); + debug_viz_pub_->publish(visualization_msg); + + /* publish stop reason for autoware api */ + const auto stop_reason_msg = makeStopReasonArray(); + stop_reason_pub_->publish(stop_reason_msg); + + /* reset variables */ + stop_pose_ptr_ = nullptr; + stop_obstacle_point_ptr_ = nullptr; +} + +visualization_msgs::msg::MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = this->clock_->now(); + tf2::Transform tf_base_link2front( + tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(base_link2front_, 0.0, 0.0)); + + // visualize stop line + if (stop_pose_ptr_ != nullptr) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = current_time; + marker.ns = "virtual_wall/no_start"; + marker.id = 0; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(*stop_pose_ptr_, tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + + // visualize stop reason + if (stop_pose_ptr_ != nullptr) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = current_time; + marker.ns = "factor_text/no_start"; + marker.id = 0; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(*stop_pose_ptr_, tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "surround obstacle"; + msg.markers.push_back(marker); + } + + // visualize surround object + if (stop_obstacle_point_ptr_ != nullptr) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = current_time; + marker.ns = "no_start_obstacle_text"; + marker.id = 0; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position = *stop_obstacle_point_ptr_; + marker.pose.position.z += 2.0; // add half of the heights of obj roughly + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + return msg; +} + +autoware_planning_msgs::msg::StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() +{ + // create header + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = this->clock_->now(); + + // create stop reason stamped + autoware_planning_msgs::msg::StopReason stop_reason_msg; + stop_reason_msg.reason = autoware_planning_msgs::msg::StopReason::SURROUND_OBSTACLE_CHECK; + autoware_planning_msgs::msg::StopFactor stop_factor; + + if (stop_pose_ptr_ != nullptr) { + stop_factor.stop_pose = *stop_pose_ptr_; + if (stop_obstacle_point_ptr_ != nullptr) { + stop_factor.stop_factor_points.emplace_back(*stop_obstacle_point_ptr_); + } + stop_reason_msg.stop_factors.emplace_back(stop_factor); + } + + // create stop reason array + autoware_planning_msgs::msg::StopReasonArray stop_reason_array; + stop_reason_array.header = header; + stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); + return stop_reason_array; +} diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp new file mode 100644 index 000000000000..75adc2962cab --- /dev/null +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -0,0 +1,478 @@ +// 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. + +#include "surround_obstacle_checker/node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("surround_obstacle_checker_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) +{ + // Parameters + use_pointcloud_ = this->declare_parameter("use_pointcloud", true); + use_dynamic_object_ = this->declare_parameter("use_dynamic_object", true); + surround_check_distance_ = this->declare_parameter("surround_check_distance", 2.0); + surround_check_recover_distance_ = + this->declare_parameter("surround_check_recover_distance", 2.5); + state_clear_time_ = this->declare_parameter("state_clear_time", 2.0); + stop_state_ego_speed_ = this->declare_parameter("stop_state_ego_speed", 0.1); + debug_ptr_ = std::make_shared( + vehicle_info_.max_longitudinal_offset_m, this->get_clock(), *this); + self_poly_ = createSelfPolygon(); + + // Publishers + path_pub_ = + this->create_publisher("~/output/trajectory", 1); + stop_reason_diag_pub_ = + this->create_publisher("~/output/no_start_reason", 1); + + // Subscriber + path_sub_ = this->create_subscription( + "~/input/trajectory", 1, + std::bind(&SurroundObstacleCheckerNode::pathCallback, this, std::placeholders::_1)); + pointcloud_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&SurroundObstacleCheckerNode::pointCloudCallback, this, std::placeholders::_1)); + dynamic_object_sub_ = + this->create_subscription( + "~/input/objects", 1, + std::bind(&SurroundObstacleCheckerNode::dynamicObjectCallback, this, std::placeholders::_1)); + current_velocity_sub_ = this->create_subscription( + "~/input/odometry", 1, + std::bind(&SurroundObstacleCheckerNode::currentVelocityCallback, this, std::placeholders::_1)); +} + +void SurroundObstacleCheckerNode::pathCallback( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) +{ + if (use_pointcloud_ && !pointcloud_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for pointcloud info..."); + return; + } + + if (use_dynamic_object_ && !object_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for dynamic object info..."); + return; + } + + if (!current_velocity_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for current velocity..."); + return; + } + + // parameter description + TrajectoryPoints output_trajectory_points = + autoware_utils::convertToTrajectoryPointArray(*input_msg); + + diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + + // get current pose in traj frame + geometry_msgs::msg::Pose current_pose; + if (!getPose(input_msg->header.frame_id, "base_link", current_pose)) { + return; + } + + // get closest idx + const size_t closest_idx = getClosestIdx(output_trajectory_points, current_pose); + + // get nearest object + double min_dist_to_obj = std::numeric_limits::max(); + geometry_msgs::msg::Point nearest_obj_point; + getNearestObstacle(&min_dist_to_obj, &nearest_obj_point); + + // check current obstacle status (exist or not) + const auto is_obstacle_found = isObstacleFound(min_dist_to_obj); + + // check current stop status (stop or not) + const auto is_stopped = checkStop(input_msg->points.at(closest_idx)); + + const auto is_stop_required = isStopRequired(is_obstacle_found, is_stopped); + + // insert stop velocity + if (is_stop_required) { + state_ = State::STOP; + + // do not start when there is a obstacle near the ego vehicle. + RCLCPP_WARN_THROTTLE( + get_logger(), *this->get_clock(), 500 /* ms */, + "do not start because there is obstacle near the ego vehicle."); + insertStopVelocity(closest_idx, &output_trajectory_points); + + // visualization for debug + if (is_obstacle_found) { + debug_ptr_->pushObstaclePoint(nearest_obj_point, PointType::NoStart); + } + debug_ptr_->pushPose(input_msg->points.at(closest_idx).pose, PoseType::NoStart); + no_start_reason_diag = makeStopReasonDiag("obstacle", input_msg->points.at(closest_idx).pose); + } else { + state_ = State::PASS; + } + + // publish trajectory and debug info + auto output_msg = autoware_utils::convertToTrajectory(output_trajectory_points); + output_msg.header = input_msg->header; + path_pub_->publish(output_msg); + stop_reason_diag_pub_->publish(no_start_reason_diag); + debug_ptr_->publish(); +} + +void SurroundObstacleCheckerNode::pointCloudCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +{ + pointcloud_ptr_ = input_msg; +} + +void SurroundObstacleCheckerNode::dynamicObjectCallback( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg) +{ + object_ptr_ = input_msg; +} + +void SurroundObstacleCheckerNode::currentVelocityCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +{ + current_velocity_ptr_ = input_msg; +} + +void SurroundObstacleCheckerNode::insertStopVelocity( + const size_t closest_idx, TrajectoryPoints * traj) +{ + // set zero velocity from closest idx to last idx + for (size_t i = closest_idx; i < traj->size(); i++) { + traj->at(i).longitudinal_velocity_mps = 0.0; + } +} + +bool SurroundObstacleCheckerNode::getPose( + const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose) +{ + try { + // get transform from source to target + geometry_msgs::msg::TransformStamped ros_src2tgt = + tf_buffer_.lookupTransform(source, target, tf2::TimePointZero); + // convert geometry_msgs::msg::Transform to geometry_msgs::msg::Pose + tf2::Transform transform; + tf2::fromMsg(ros_src2tgt.transform, transform); + tf2::toMsg(transform, pose); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 500 /* ms */, + "cannot get tf from " << source << " to " << target); + return false; + } + return true; +} + +bool SurroundObstacleCheckerNode::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) +{ + tf2::Transform src2tgt; + try { + // get transform from source to target + geometry_msgs::msg::TransformStamped ros_src2tgt = + tf_buffer_.lookupTransform(source, target, time, tf2::durationFromSec(0.1)); + tf2::fromMsg(ros_src2tgt.transform, src2tgt); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 500 /* ms */, + "cannot get tf from " << source << " to " << target); + return false; + } + + tf2::Transform src2obj; + tf2::fromMsg(pose, src2obj); + tf2::Transform tgt2obj = src2tgt.inverse() * src2obj; + tf2::toMsg(tgt2obj, conv_pose); + return true; +} + +size_t SurroundObstacleCheckerNode::getClosestIdx( + const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose) +{ + double min_dist = std::numeric_limits::max(); + size_t min_dist_idx = 0; + for (size_t i = 0; i < traj.size(); ++i) { + const double x = traj.at(i).pose.position.x - current_pose.position.x; + const double y = traj.at(i).pose.position.y - current_pose.position.y; + const double dist = std::hypot(x, y); + if (dist < min_dist) { + min_dist_idx = i; + min_dist = dist; + } + } + return min_dist_idx; +} + +void SurroundObstacleCheckerNode::getNearestObstacle( + double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) +{ + if (use_pointcloud_) { + getNearestObstacleByPointCloud(min_dist_to_obj, nearest_obj_point); + } + + if (use_dynamic_object_) { + getNearestObstacleByDynamicObject(min_dist_to_obj, nearest_obj_point); + } +} + +void SurroundObstacleCheckerNode::getNearestObstacleByPointCloud( + double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) +{ + // wait to transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, + tf2::durationFromSec(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 500 /* ms */, + "failed to get base_link to " << pointcloud_ptr_->header.frame_id << " transform."); + return; + } + + Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.transform).cast(); + pcl::PointCloud pcl; + pcl::fromROSMsg(*pointcloud_ptr_, pcl); + pcl::transformPointCloud(pcl, pcl, isometry); + for (const auto & p : pcl) { + // create boost point + Point2d boost_p(p.x, p.y); + + // calc distance + const double dist_to_obj = boost::geometry::distance(self_poly_, boost_p); + + // get minimum distance to obj + if (dist_to_obj < *min_dist_to_obj) { + *min_dist_to_obj = dist_to_obj; + nearest_obj_point->x = p.x; + nearest_obj_point->y = p.y; + nearest_obj_point->z = p.z; + } + } +} + +void SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject( + double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) +{ + const auto obj_frame = object_ptr_->header.frame_id; + const auto obj_time = object_ptr_->header.stamp; + for (const auto & obj : object_ptr_->objects) { + // change frame of obj_pose to base_link + geometry_msgs::msg::Pose pose_baselink; + if (!convertPose( + obj.kinematics.initial_pose_with_covariance.pose, obj_frame, "base_link", obj_time, + pose_baselink)) { + return; + } + + // create obj polygon + Polygon2d obj_poly; + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + obj_poly = createObjPolygon(pose_baselink, obj.shape.footprint); + } else { + obj_poly = createObjPolygon(pose_baselink, obj.shape.dimensions); + } + + // calc distance + const double dist_to_obj = boost::geometry::distance(self_poly_, obj_poly); + + // get minimum distance to obj + if (dist_to_obj < *min_dist_to_obj) { + *min_dist_to_obj = dist_to_obj; + *nearest_obj_point = obj.kinematics.initial_pose_with_covariance.pose.position; + } + } +} + +bool SurroundObstacleCheckerNode::isObstacleFound(const double min_dist_to_obj) +{ + const auto is_obstacle_inside_range = min_dist_to_obj < surround_check_distance_; + const auto is_obstacle_outside_range = min_dist_to_obj > surround_check_recover_distance_; + + if (state_ == State::PASS) { + return is_obstacle_inside_range; + } + + if (state_ == State::STOP) { + return !is_obstacle_outside_range; + } + + throw std::runtime_error("invalid state"); +} + +bool SurroundObstacleCheckerNode::isStopRequired( + const bool is_obstacle_found, const bool is_stopped) +{ + if (!is_stopped) { + return false; + } + + if (is_obstacle_found) { + last_obstacle_found_time_ = std::make_shared(this->now()); + return true; + } + + if (state_ != State::STOP) { + return false; + } + + // Keep stop state + if (last_obstacle_found_time_) { + const auto elapsed_time = this->now() - *last_obstacle_found_time_; + if (elapsed_time.seconds() <= state_clear_time_) { + return true; + } + } + + last_obstacle_found_time_ = {}; + return false; +} + +bool SurroundObstacleCheckerNode::checkStop( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & closest_point) +{ + if (std::fabs(current_velocity_ptr_->twist.twist.linear.x) > stop_state_ego_speed_) { + // ego vehicle has high velocity now. not stop. + return false; + } + + const double closest_vel = closest_point.longitudinal_velocity_mps; + const double closest_acc = closest_point.acceleration_mps2; + + if (closest_vel * closest_acc < 0) { + // ego vehicle is about to stop (during deceleration). not stop. + return false; + } + + return true; +} + +Polygon2d SurroundObstacleCheckerNode::createSelfPolygon() +{ + const double front = vehicle_info_.max_longitudinal_offset_m; + const double rear = vehicle_info_.min_longitudinal_offset_m; + const double left = vehicle_info_.max_lateral_offset_m; + const double right = vehicle_info_.min_lateral_offset_m; + + Polygon2d poly; + boost::geometry::exterior_ring(poly) = boost::assign::list_of(front, left)(front, right)( + rear, right)(rear, left)(front, left); + return poly; +} + +Polygon2d SurroundObstacleCheckerNode::createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) +{ + // rename + const double x = pose.position.x; + const double y = pose.position.y; + const double h = size.x; + const double w = size.y; + const double yaw = tf2::getYaw(pose.orientation); + + // create base polygon + Polygon2d obj_poly; + boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( + -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); + + // rotate polygon(yaw) + boost::geometry::strategy::transform::rotate_transformer + rotate(-yaw); // anti-clockwise -> :clockwise rotation + Polygon2d rotate_obj_poly; + boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); + + // translate polygon(x, y) + boost::geometry::strategy::transform::translate_transformer translate(x, y); + Polygon2d translate_obj_poly; + boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +Polygon2d SurroundObstacleCheckerNode::createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) +{ + // rename + const double x = pose.position.x; + const double y = pose.position.y; + const double yaw = tf2::getYaw(pose.orientation); + + // create base polygon + Polygon2d obj_poly; + for (const auto point : footprint.points) { + const Point2d point2d(point.x, point.y); + obj_poly.outer().push_back(point2d); + } + + // rotate polygon(yaw) + boost::geometry::strategy::transform::rotate_transformer + rotate(-yaw); // anti-clockwise -> :clockwise rotation + Polygon2d rotate_obj_poly; + boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); + + // translate polygon(x, y) + boost::geometry::strategy::transform::translate_transformer translate(x, y); + Polygon2d translate_obj_poly; + boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +diagnostic_msgs::msg::DiagnosticStatus SurroundObstacleCheckerNode::makeStopReasonDiag( + const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose) +{ + diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; + no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + no_start_reason_diag.name = "no_start_reason"; + no_start_reason_diag.message = no_start_reason; + no_start_reason_diag_kv.key = "no_start_pose"; + no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose); + no_start_reason_diag.values.push_back(no_start_reason_diag_kv); + return no_start_reason_diag; +} + +std::string SurroundObstacleCheckerNode::jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(SurroundObstacleCheckerNode) diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md new file mode 100644 index 000000000000..fd24ff644bcb --- /dev/null +++ b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -0,0 +1,80 @@ +# Surround Obstacle Checker + +## Purpose + +`surround_obstacle_checker` は、自車が停車中、自車の周囲に障害物が存在する場合に発進しないように停止計画を行うモジュールである。 + +## 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 + +点群、動的物体、自車速度のデータが取得できているかどうかを確認する。 + +### Get distance to nearest object + +自車と最近傍の障害物との距離を計算する。 +ここでは、自車のポリゴンを計算し、点群の各点および各動的物体のポリゴンとの距離をそれぞれ計算することで最近傍の障害物との距離を求める。 + +### Stop condition + +次の条件をすべて満たすとき、自車は停止計画を行う。 + +- 自車が停車していること +- 次のうちいずれかを満たすこと + 1. 最近傍の障害物との距離が次の条件をみたすこと + - `State::PASS` のとき、`surround_check_distance` 未満である + - `State::STOP` のとき、`surround_check_recover_distance` 以下である + 2. 1 を満たしていないとき、1 の条件を満たした時刻からの経過時間が `state_clear_time` 以下であること + +### States + +チャタリング防止のため、`surround_obstacle_checker` では状態を管理している。 +Stop condition の項で述べたように、状態によって障害物判定のしきい値を変更することでチャタリングを防止している。 + +- `State::PASS` ：停止計画解除中 +- `State::STOP` ：停止計画中 + +## 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 + +この機能が動作するためには障害物点群の観測が必要なため、障害物が死角に入っている場合は停止計画を行わない。