From a7cad4b725ade0a1b0313ef8863d918530d7d4e5 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 31 Mar 2021 13:40:00 +0900 Subject: [PATCH] Refactor obstacle_stop_planner (#3) --- .github/workflows/build_and_test.yml | 4 + .isort.cfg | 3 + ament_cmake_auto_gtest/CMakeLists.txt | 14 + ament_cmake_auto_gtest/README.md | 3 + .../ament_cmake_auto_gtest-extras.cmake | 20 + .../cmake/ament_auto_add_gtest.cmake | 110 ++ ament_cmake_auto_gtest/package.xml | 19 + obstacle_stop_planner_refine/CMakeLists.txt | 41 +- .../config/test_vehicle_info.param.yaml | 11 + .../adaptive_cruise_control.hpp | 188 +-- .../obstacle_stop_planner/debug_marker.hpp | 30 +- .../include/obstacle_stop_planner/node.hpp | 144 +-- .../obstacle_point_cloud.hpp | 126 ++ .../obstacle_stop_planner.hpp | 127 ++ .../one_step_polygon.hpp | 74 ++ .../adaptive_cruise_control_parameter.hpp | 124 ++ .../parameter/slow_down_control_parameter.hpp | 34 + .../parameter/stop_control_parameter.hpp | 33 + .../obstacle_stop_planner/point_helper.hpp | 107 ++ .../obstacle_stop_planner/trajectory.hpp | 43 + .../include/obstacle_stop_planner/util.hpp | 97 ++ .../util/create_vehicle_footprint.hpp | 47 + .../visibility_control.hpp | 37 + obstacle_stop_planner_refine/package.xml | 6 +- .../src/adaptive_cruise_control.cpp | 382 ++---- .../src/debug_marker.cpp | 140 +-- obstacle_stop_planner_refine/src/node.cpp | 1045 +++-------------- .../src/obstacle_stop_planner.cpp | 496 ++++++++ .../src/point_helper.cpp | 254 ++++ .../src/trajectory.cpp | 120 ++ .../obstacle_stop_planner_node_launch_test.py | 61 + .../{src/main.cpp => test/src/gtest_main.cpp} | 15 +- .../src/test_create_vehicle_footprint.cpp | 32 + .../test/src/test_obstacle_stop_planner.cpp | 110 ++ .../src/test_obstacle_stop_planner_node.cpp | 183 +++ .../test/src/test_point_helper.cpp | 46 + .../test/src/test_util.cpp | 22 + .../test/test_obstacle_stop_planner.test | 5 + 38 files changed, 2768 insertions(+), 1585 deletions(-) create mode 100644 .isort.cfg create mode 100644 ament_cmake_auto_gtest/CMakeLists.txt create mode 100644 ament_cmake_auto_gtest/README.md create mode 100644 ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake create mode 100644 ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake create mode 100644 ament_cmake_auto_gtest/package.xml create mode 100644 obstacle_stop_planner_refine/config/test_vehicle_info.param.yaml create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/slow_down_control_parameter.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp create mode 100644 obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp create mode 100644 obstacle_stop_planner_refine/src/point_helper.cpp create mode 100644 obstacle_stop_planner_refine/src/trajectory.cpp create mode 100644 obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py rename obstacle_stop_planner_refine/{src/main.cpp => test/src/gtest_main.cpp} (65%) create mode 100644 obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp create mode 100644 obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp create mode 100644 obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp create mode 100644 obstacle_stop_planner_refine/test/src/test_point_helper.cpp create mode 100644 obstacle_stop_planner_refine/test/src/test_util.cpp create mode 100644 obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml index 54b7c654..bc14e65f 100644 --- a/.github/workflows/build_and_test.yml +++ b/.github/workflows/build_and_test.yml @@ -22,6 +22,10 @@ jobs: with: required-ros-distributions: foxy + - name: Concat build_depends.repos + run: | + curl -sSL https://raw.githubusercontent.com/tier4/AutowareArchitectureProposal.iv/ros2/build_depends.repos | sed '1d' >> build_depends.repos + - name: Run action-ros-ci id: action_ros_ci_step uses: ros-tooling/action-ros-ci@v0.2 diff --git a/.isort.cfg b/.isort.cfg new file mode 100644 index 00000000..a9facc1f --- /dev/null +++ b/.isort.cfg @@ -0,0 +1,3 @@ +[settings] +force_sort_within_sections=true +known_third_party=launch diff --git a/ament_cmake_auto_gtest/CMakeLists.txt b/ament_cmake_auto_gtest/CMakeLists.txt new file mode 100644 index 00000000..768e2eb2 --- /dev/null +++ b/ament_cmake_auto_gtest/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.5) + +project(ament_cmake_auto_gtest NONE) + +find_package(ament_cmake REQUIRED) + +ament_package( + CONFIG_EXTRAS "ament_cmake_auto_gtest-extras.cmake" +) + +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) diff --git a/ament_cmake_auto_gtest/README.md b/ament_cmake_auto_gtest/README.md new file mode 100644 index 00000000..04e7b08d --- /dev/null +++ b/ament_cmake_auto_gtest/README.md @@ -0,0 +1,3 @@ +NOTE: This package is temporary and will be removed after the following pull requests have been merged. + + diff --git a/ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake b/ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake new file mode 100644 index 00000000..cbdf4472 --- /dev/null +++ b/ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake @@ -0,0 +1,20 @@ +# Copyright 2014 Open Source Robotics Foundation, 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. + +# copied from ament_cmake_auto/ament_cmake_auto-extras.cmake + +find_package(ament_cmake_auto QUIET REQUIRED) +find_package(ament_cmake_gtest QUIET REQUIRED) + +include("${ament_cmake_auto_gtest_DIR}/ament_auto_add_gtest.cmake") diff --git a/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake b/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake new file mode 100644 index 00000000..96dcee28 --- /dev/null +++ b/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake @@ -0,0 +1,110 @@ +# Copyright 2014 Open Source Robotics Foundation, 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. + +# +# Add a gtest, automatically linking and including dependencies. +# +# Call add_executable(target ARGN), link it against the gtest libraries +# and register the executable as a test. +# +# If gtest is not available the specified target is not being created and +# therefore the target existence should be checked before being used. +# +# +# All arguments of the CMake function ``add_executable()`` can be +# used beside the custom arguments ``DIRECTORY`` and +# ``NO_TARGET_LINK_LIBRARIES``. +# +# :param target: the name of the executable target +# :type target: string +# :param DIRECTORY: the directory to recursively glob for source +# files with the following extensions: c, cc, cpp, cxx +# :type DIRECTORY: string +# :param NO_TARGET_LINK_LIBRARIES: if set skip linking against +# ``${PROJECT_NAME}_LIBRARIES`` +# :type NO_TARGET_LINK_LIBRARIES: option +# :param ARGN: the list of source files +# :type ARGN: list of strings +# :param RUNNER: the path to the test runner script +# (default: see ament_add_test). +# :type RUNNER: string +# :param TIMEOUT: the test timeout in seconds, +# default defined by ``ament_add_test()`` +# :type TIMEOUT: integer +# :param WORKING_DIRECTORY: the working directory for invoking the +# executable in, default defined by ``ament_add_test()`` +# :type WORKING_DIRECTORY: string +# :param SKIP_LINKING_MAIN_LIBRARIES: if set skip linking against the gtest +# main libraries +# :type SKIP_LINKING_MAIN_LIBRARIES: option +# :param SKIP_TEST: if set mark the test as being skipped +# :type SKIP_TEST: option +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# +# @public +# +macro(ament_auto_add_gtest target) + cmake_parse_arguments(ARG + "WIN32;MACOSX_BUNDLE;EXCLUDE_FROM_ALL;NO_TARGET_LINK_LIBRARIES;SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" + "RUNNER;TIMEOUT;WORKING_DIRECTORY;DIRECTORY" + "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" + ${ARGN}) + if(NOT ARG_DIRECTORY AND NOT ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "ament_auto_add_executable() called without any " + "source files and without a DIRECTORY argument") + endif() + + set(_source_files "") + if(ARG_DIRECTORY) + # glob all source files + file( + GLOB_RECURSE + _source_files + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + "${ARG_DIRECTORY}/*.c" + "${ARG_DIRECTORY}/*.cc" + "${ARG_DIRECTORY}/*.cpp" + "${ARG_DIRECTORY}/*.cxx" + ) + if(NOT _source_files) + message(FATAL_ERROR "ament_auto_add_executable() no source files found " + "in directory '${CMAKE_CURRENT_SOURCE_DIR}/${ARG_DIRECTORY}'") + endif() + endif() + + # parse again to "remove" custom arguments + cmake_parse_arguments(ARG "NO_TARGET_LINK_LIBRARIES" "DIRECTORY" "" ${ARGN}) + ament_add_gtest("${target}" ${ARG_UNPARSED_ARGUMENTS} ${_source_files}) + + # add include directory of this package if it exists + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include") + target_include_directories("${target}" PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include") + endif() + # link against other libraries of this package + if(NOT ${PROJECT_NAME}_LIBRARIES STREQUAL "" AND + NOT ARG_NO_TARGET_LINK_LIBRARIES) + target_link_libraries("${target}" ${${PROJECT_NAME}_LIBRARIES}) + endif() + + # add exported information from found build dependencies + ament_target_dependencies(${target} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endmacro() diff --git a/ament_cmake_auto_gtest/package.xml b/ament_cmake_auto_gtest/package.xml new file mode 100644 index 00000000..0c8f1f8c --- /dev/null +++ b/ament_cmake_auto_gtest/package.xml @@ -0,0 +1,19 @@ + + + + ament_cmake_auto_gtest + 0.9.2 + The auto-magic functions for ease to use of the ament buildsystem in CMake. + Dirk Thomas + Apache License 2.0 + + ament_cmake + ament_cmake_gtest + + ament_cmake_auto + ament_cmake_gtest + + + ament_cmake + + diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index 57cf938c..f233a6c6 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -O3) + add_compile_options(-Wall -Wextra -Wpedantic) add_compile_options(-Wno-unused-parameter) endif() @@ -15,30 +15,37 @@ 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_executable(obstacle_stop_planner_node - src/debug_marker.cpp - src/node.cpp - src/main.cpp - src/adaptive_cruise_control.cpp -) -target_include_directories(obstacle_stop_planner_node - PUBLIC - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} +file(GLOB_RECURSE OBSTACLE_STOP_PLANNER_NODES_NODE_SRC + src/* +) +file(GLOB_RECURSE OBSTACLE_STOP_PLANNER_NODES_NODE_HEADERS + include/obstacle_stop_planner/* ) -target_link_libraries(obstacle_stop_planner_node - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} +# generate component node library +ament_auto_add_library(obstacle_stop_planner_node SHARED + ${OBSTACLE_STOP_PLANNER_NODES_NODE_SRC} + ${OBSTACLE_STOP_PLANNER_NODES_NODE_HEADERS} +) +rclcpp_components_register_node(obstacle_stop_planner_node + PLUGIN "obstacle_stop_planner::ObstacleStopPlannerNode" + EXECUTABLE obstacle_stop_planner_node_exe ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + add_ros_test( + test/obstacle_stop_planner_node_launch_test.py + TIMEOUT "30" + ) + + find_package(ament_cmake_auto_gtest REQUIRED) + ament_auto_add_gtest(test_obstacle_stop_planner + DIRECTORY "test/" + ) endif() ament_auto_package( diff --git a/obstacle_stop_planner_refine/config/test_vehicle_info.param.yaml b/obstacle_stop_planner_refine/config/test_vehicle_info.param.yaml new file mode 100644 index 00000000..b1279b50 --- /dev/null +++ b/obstacle_stop_planner_refine/config/test_vehicle_info.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + wheel_radius: 0.39 + wheel_width: 0.42 + wheel_base: 2.74 # between front wheel center and rear wheel center + wheel_tread: 1.63 # between left wheel center and right wheel center + front_overhang: 1.0 # between front wheel center and vehicle front + rear_overhang: 1.03 # between rear wheel center and vehicle rear + left_overhang: 0.1 # between left wheel center and vehicle left + right_overhang: 0.1 # between right wheel center and vehicle right + vehicle_height: 2.5 diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 40726502..f9637f95 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -16,35 +16,43 @@ #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ #include +#include #include "geometry_msgs/msg/twist_stamped.hpp" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" #include "rclcpp/rclcpp.hpp" -#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "tf2/utils.h" +#include "boost/optional.hpp" +#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_utils/autoware_utils.hpp" +#include "obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp" -namespace motion_planning +namespace obstacle_stop_planner { +using autoware_utils::Point2d; +using autoware_utils::Point3d; +using autoware_utils::Polygon2d; +using autoware_utils::Polygon3d; +using boost::optional; + class AdaptiveCruiseController { public: AdaptiveCruiseController( - rclcpp::Node * node, const double vehicle_width, const double vehicle_length, - const double wheel_base, const double front_overhang); + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const AdaptiveCruiseControlParameter & acc_param); - void insertAdaptiveCruiseVelocity( + std::tuple insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_collision_point_idx, - const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time nearest_collision_point_time, + const geometry_msgs::msg::Pose self_pose, const Point2d & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, - bool * need_to_stop, - autoware_planning_msgs::msg::Trajectory * output_trajectory); + const autoware_planning_msgs::msg::Trajectory & input_trajectory); private: rclcpp::Publisher::SharedPtr pub_debug_; @@ -53,154 +61,52 @@ class AdaptiveCruiseController /* * Parameter */ - double vehicle_width_; - double vehicle_length_; - double wheel_base_; - double front_overhang_; + VehicleInfo vehicle_info_; + AdaptiveCruiseControlParameter param_; rclcpp::Time prev_collision_point_time_; - pcl::PointXYZ prev_collision_point_; + Point2d prev_collision_point_; double prev_target_vehicle_time_ = 0.0; double prev_target_vehicle_dist_ = 0.0; double prev_target_velocity_ = 0.0; bool prev_collision_point_valid_ = false; std::vector est_vel_que_; double prev_upper_velocity_ = 0.0; + double min_behavior_stop_margin_ = 0.0; - struct Param - { - double stop_margin; - double min_behavior_stop_margin; - - //!< @brief use tracking objects for estimating object velocity or not - bool use_object_to_est_vel; - - //!< @brief use pcl for estimating object velocity or not - bool use_pcl_to_est_vel; - - //!< @brief consider forward vehicle velocity to self upper velocity or not - bool consider_obj_velocity; - - //!< @brief The distance to extend the polygon length the object in pointcloud-object matching - double object_polygon_length_margin; - - //!< @brief The distance to extend the polygon width the object in pointcloud-object matching - double object_polygon_width_margin; - - //!< @breif Maximum time difference treated as continuous points in speed estimation using a - // point cloud - double valid_est_vel_diff_time; - - //!< @brief Time width of information used for speed estimation in speed estimation using a - // point cloud - double valid_vel_que_time; - - //!< @brief Maximum value of valid speed estimation results in speed estimation using a point - // cloud - double valid_est_vel_max; - - //!< @brief Minimum value of valid speed estimation results in speed estimation using a point - // cloud - double valid_est_vel_min; - - //!< @brief Embed a stop line if the maximum speed calculated by ACC is lower than this speed - double thresh_vel_to_stop; - - /* parameter for acc */ - //!< @brief threshold of forward obstacle velocity to insert stop line (to stop acc) - double obstacle_stop_velocity_thresh; - - //!< @brief supposed minimum acceleration in emergency stop - double emergency_stop_acceleration; - - //!< @brief supposed minimum acceleration of forward vehicle in emergency stop - double obstacle_emergency_stop_acceleration; - - //!< @brief supposed idling time to start emergency stop - double emergency_stop_idling_time; - - //!< @brief minimum distance of emergency stop - double min_dist_stop; - - //!< @brief supposed maximum acceleration in active cruise control - double max_standard_acceleration; - - //!< @brief supposed minimum acceleration(deceleration) in active cruise control - double min_standard_acceleration; - - //!< @brief supposed idling time to react object in active cruise control - double standard_idling_time; - - //!< @brief minimum distance in active cruise control - double min_dist_standard; - - //!< @brief supposed minimum acceleration of forward obstacle - double obstacle_min_standard_acceleration; - - //!< @brief margin to insert upper velocity - double margin_rate_to_change_vel; - - //!< @brief use time-compensation to calculate distance to forward vehicle - bool use_time_compensation_to_dist; - - //!< @brief gain of lowpass filter of upper velocity - double lowpass_gain_; - - //!< @brief when failed to estimate velocity, use rough velocity estimation or not - bool use_rough_est_vel; - - //!< @brief in rough velocity estimation, front car velocity is - //!< estimated as self current velocity * this value - double rough_velocity_rate; - - /* parameter for pid used in acc */ - //!< @brief coefficient P in PID control (used when target dist -current_dist >=0) - double p_coeff_pos; - - //!< @brief coefficient P in PID control (used when target dist -current_dist <0) - double p_coeff_neg; - - //!< @brief coefficient D in PID control (used when delta_dist >=0) - double d_coeff_pos; - - //!< @brief coefficient D in PID control (used when delta_dist <0) - double d_coeff_neg; - - static constexpr double d_coeff_valid_time = 1.0; - static constexpr double d_coeff_valid_diff_vel = 20.0; - static constexpr double d_max_vel_norm = 3.0; - }; - Param param_; - - double getMedianVel(const std::vector vel_que); - double lowpass_filter(const double current_value, const double prev_value, const double gain); - void calcDistanceToNearestPointOnPath( + double getMedianVel(const std::vector & vel_que); + static double lowpass_filter( + const double current_value, const double prev_value, + const double gain); + double calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, - const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * distance); - double calcTrajYaw( + const geometry_msgs::msg::Pose & self_pose, const Point2d & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time); + static double calcTrajYaw( const autoware_planning_msgs::msg::Trajectory & trajectory, const int collision_point_idx); - bool estimatePointVelocityFromObject( + optional estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const pcl::PointXYZ & nearest_collision_point, double * velocity); - bool estimatePointVelocityFromPcl( - const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * velocity); - double estimateRoughPointVelocity(double current_vel); + const Point2d & nearest_collision_point); + optional estimatePointVelocityFromPcl( + const double traj_yaw, const Point2d & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time); + double estimateRoughPointVelocity(const double current_vel); double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); - double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel); - double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel); - double calcTargetVelocity_P(const double target_dist, const double current_dist); - double calcTargetVelocity_I(const double target_dist, const double current_dist); + double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel) const; + double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel) const; + double calcTargetVelocity_P(const double target_dist, const double current_dist) const; + static double calcTargetVelocity_I(const double target_dist, const double current_dist); double calcTargetVelocity_D(const double target_dist, const double current_dist); double calcTargetVelocityByPID( const double current_vel, const double current_dist, const double obj_vel); - void insertMaxVelocityToPath( - const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, + autoware_planning_msgs::msg::Trajectory insertMaxVelocityToPath( + const geometry_msgs::msg::Pose & /*self_pose*/, + const double current_vel, + const double target_vel, const double dist_to_collision_point, - autoware_planning_msgs::msg::Trajectory * output_trajectory); + const autoware_planning_msgs::msg::Trajectory & input_trajectory) const; void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); /* Debug */ @@ -221,6 +127,6 @@ class AdaptiveCruiseController static constexpr unsigned int num_debug_values_ = 10; }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp index c8fbc388..70694f62 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp @@ -21,22 +21,14 @@ #include "autoware_planning_msgs/msg/stop_reason_array.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "pcl/point_types.h" #include "rclcpp/rclcpp.hpp" -#include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc.hpp" -#define EIGEN_MPL2_ONLY -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" -namespace motion_planning +#include "obstacle_stop_planner/util.hpp" + +namespace obstacle_stop_planner { enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown }; - enum class PointType : int8_t { Stop = 0, SlowDown }; - enum class PoseType : int8_t { Stop = 0, SlowDownStart, SlowDownEnd }; class ObstacleStopPlannerDebugNode @@ -45,11 +37,11 @@ class ObstacleStopPlannerDebugNode explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); ~ObstacleStopPlannerDebugNode() {} bool pushPolygon( - const std::vector & polygon, const double z, const PolygonType & type); - bool pushPolygon(const std::vector & polygon, const PolygonType & type); + const Polygon2d & polygon, const double z, const PolygonType & type); + bool pushPolygon(const Polygon3d & polygon, const PolygonType & type); bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); - bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); + bool pushObstaclePoint(const Point3d & obstacle_point, const PointType & type); visualization_msgs::msg::MarkerArray makeVisualizationMarker(); autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray(); @@ -66,12 +58,12 @@ class ObstacleStopPlannerDebugNode std::shared_ptr slow_down_end_pose_ptr_; std::shared_ptr stop_obstacle_point_ptr_; std::shared_ptr slow_down_obstacle_point_ptr_; - std::vector> vehicle_polygons_; - std::vector> slow_down_range_polygons_; - std::vector> collision_polygons_; - std::vector> slow_down_polygons_; + std::vector vehicle_polygons_; + std::vector slow_down_range_polygons_; + std::vector collision_polygons_; + std::vector slow_down_polygons_; }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index d2e280f3..f8706515 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -14,51 +14,24 @@ #ifndef OBSTACLE_STOP_PLANNER__NODE_HPP_ #define OBSTACLE_STOP_PLANNER__NODE_HPP_ -#include #include -#include #include "rclcpp/rclcpp.hpp" -#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "obstacle_stop_planner/visibility_control.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/expand_stop_range.hpp" -#include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "autoware_debug_msgs/msg/float32_stamped.hpp" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_cloud.h" -#include "pcl/common/transforms.h" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2/utils.h" -#include "tf2_ros/transform_listener.h" -#include "visualization_msgs/msg/marker_array.hpp" -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc.hpp" -#include "obstacle_stop_planner/adaptive_cruise_control.hpp" -#include "obstacle_stop_planner/debug_marker.hpp" - -namespace motion_planning -{ -struct StopPoint -{ - size_t index; - Eigen::Vector2d point; -}; +#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "obstacle_stop_planner/obstacle_stop_planner.hpp" -struct SlowDownPoint +namespace obstacle_stop_planner { - size_t index; - Eigen::Vector2d point; - double velocity; -}; - -class ObstacleStopPlannerNode : public rclcpp::Node +class OBSTACLE_STOP_PLANNER_PUBLIC ObstacleStopPlannerNode : public rclcpp::Node { public: - ObstacleStopPlannerNode(); + explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & options); private: /* @@ -73,36 +46,23 @@ class ObstacleStopPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr expand_stop_range_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_; + sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; /* * Parameter */ - std::unique_ptr acc_controller_; - sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr_; - double wheel_base_, front_overhang_, rear_overhang_, left_overhang_, right_overhang_, - vehicle_width_, vehicle_length_; - double stop_margin_; - double slow_down_margin_; - double min_behavior_stop_margin_; rclcpp::Time prev_col_point_time_; pcl::PointXYZ prev_col_point_; - double expand_slow_down_range_; - double expand_stop_range_; - double max_slow_down_vel_; - double min_slow_down_vel_; - double max_deceleration_; - bool enable_slow_down_; - double extend_distance_; - double step_length_; - double stop_search_radius_; - double slow_down_search_radius_; + bool pointcloud_received_; + bool current_velocity_reveived_; + + std::unique_ptr planner_; + void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); void pathCallback(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg); void dynamicObjectCallback( @@ -110,85 +70,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node void currentVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg); void externalExpandStopRangeCallback( const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg); - -private: - bool convexHull( - const std::vector pointcloud, std::vector & polygon_points); - bool decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - bool decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - std::map & index_map); - bool trimTrajectoryFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - bool trimTrajectoryWithIndexFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - size_t & index); - bool searchPointcloudNearTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius, - const pcl::PointCloud::Ptr input_pointcloud_ptr, - pcl::PointCloud::Ptr output_pointcloud_ptr); - void createOneStepPolygon( - const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, - std::vector & polygon, const double expand_width = 0.0); - bool getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, - geometry_msgs::msg::Pose & self_pose); - bool getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, - Eigen::Vector2d & output_point); - void getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time); - void getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation); - geometry_msgs::msg::Pose getVehicleCenterFromBase(const geometry_msgs::msg::Pose & base_pose); - - void insertStopPoint( - const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag); - - StopPoint searchInsertPoint( - const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec); - - StopPoint createTargetPoint( - const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, - const Eigen::Vector2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path); - - SlowDownPoint createSlowDownStartPoint( - const int idx, const double margin, const double slow_down_target_vel, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path); - - void insertSlowDownStartPoint( - const SlowDownPoint & slow_down_start_point, - const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path); - - void insertSlowDownVelocity( - const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, - autoware_planning_msgs::msg::Trajectory & output_path); - - double calcSlowDownTargetVel(const double lateral_deviation); - bool extendTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const double extend_distance, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - - autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( - double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point); }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__NODE_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp new file mode 100644 index 00000000..deadae9c --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -0,0 +1,126 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ +#define OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ + +#include +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_cloud.h" +#include "pcl/common/transforms.h" +#include "pcl/filters/voxel_grid.h" +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_ros/transform_listener.h" +#include "rclcpp/logger.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" + +namespace obstacle_stop_planner +{ +inline sensor_msgs::msg::PointCloud2::ConstSharedPtr updatePointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + auto obstacle_ros_pointcloud_ptr = std::make_shared(); + pcl::VoxelGrid filter; + pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( + new pcl::PointCloud); + + pcl::fromROSMsg(*msg, *pointcloud_ptr); + + for (const auto & point : pointcloud_ptr->points) { + no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); + } + filter.setInputCloud(no_height_pointcloud_ptr); + filter.setLeafSize(0.05F, 0.05F, 100000.0F); + filter.filter(*no_height_filtered_pointcloud_ptr); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr); + obstacle_ros_pointcloud_ptr->header = msg->header; + return obstacle_ros_pointcloud_ptr; +} + +inline static pcl::PointCloud::Ptr searchPointcloudNearTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const pcl::PointCloud::Ptr input_pointcloud_ptr, + const double search_radius, + const VehicleInfo & param) +{ + pcl::PointCloud::Ptr output_pointcloud_ptr( + new pcl::PointCloud); + const double squared_radius = search_radius * search_radius; + for (const auto & trajectory_point : trajectory.points) { + const auto center_pose = getVehicleCenterFromBase( + trajectory_point.pose, + param.vehicle_length, + param.rear_overhang); + + for (const auto & point : input_pointcloud_ptr->points) { + const double x = center_pose.position.x - point.x; + const double y = center_pose.position.y - point.y; + const double squared_distance = x * x + y * y; + if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);} + } + } + return output_pointcloud_ptr; +} + +inline pcl::PointCloud::Ptr searchCandidateObstacle( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & obstacle_ros_pointcloud_ptr, + const tf2_ros::Buffer & tf_buffer, + const autoware_planning_msgs::msg::Trajectory & trajectory, + const double search_radius, + const VehicleInfo & param, + const rclcpp::Logger & logger) +{ + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer.lookupTransform( + trajectory.header.frame_id, obstacle_ros_pointcloud_ptr->header.frame_id, + obstacle_ros_pointcloud_ptr->header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + logger, + "[obstacle_stop_planner] Failed to look up transform from " << + trajectory.header.frame_id << " to " << obstacle_ros_pointcloud_ptr->header.frame_id); + // do not publish path + return nullptr; + } + + Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl::PointCloud::Ptr obstacle_pointcloud_pcl_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr, *obstacle_pointcloud_pcl_ptr); + pcl::PointCloud::Ptr transformed_obstacle_pointcloud_ptr( + new pcl::PointCloud); + pcl::transformPointCloud( + *obstacle_pointcloud_pcl_ptr, + *transformed_obstacle_pointcloud_ptr, + affine_matrix); + + // search obstacle candidate pointcloud to reduce calculation cost + auto obstacle_candidate_pointcloud_ptr = searchPointcloudNearTrajectory( + trajectory, transformed_obstacle_pointcloud_ptr, + search_radius, + param); + obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; + return obstacle_candidate_pointcloud_ptr; +} + +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp new file mode 100644 index 00000000..67aa30a9 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp @@ -0,0 +1,127 @@ +// Copyright 2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_STOP_PLANNER__OBSTACLE_STOP_PLANNER_HPP_ +#define OBSTACLE_STOP_PLANNER__OBSTACLE_STOP_PLANNER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "obstacle_stop_planner/visibility_control.hpp" +#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/expand_stop_range.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "autoware_debug_msgs/msg/float32_stamped.hpp" +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_cloud.h" +#include "pcl/common/transforms.h" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2/utils.h" +#include "tf2_ros/transform_listener.h" +#include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include "obstacle_stop_planner/debug_marker.hpp" +#include "obstacle_stop_planner/obstacle_point_cloud.hpp" +#include "obstacle_stop_planner/point_helper.hpp" +#include "obstacle_stop_planner/util.hpp" + +namespace obstacle_stop_planner +{ +class OBSTACLE_STOP_PLANNER_PUBLIC ObstacleStopPlanner +{ +public: + ObstacleStopPlanner( + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const StopControlParameter & stop_param, + const SlowDownControlParameter & slow_down_param, + const AdaptiveCruiseControlParameter & acc_param); + + void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); + autoware_planning_msgs::msg::Trajectory + pathCallback( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg, + tf2_ros::Buffer & tf_buffer); + void dynamicObjectCallback( + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg); + void currentVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg); + void externalExpandStopRangeCallback( + const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg); + +private: + rclcpp::Node * node_; + std::shared_ptr debug_ptr_; + // tf2_ros::Buffer tf_buffer_; + // tf2_ros::TransformListener tf_listener_; + sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; + VehicleInfo vehicle_info_; + StopControlParameter stop_param_; + SlowDownControlParameter slow_down_param_; + AdaptiveCruiseControlParameter acc_param_; + + /* + * Parameter + */ + std::unique_ptr acc_controller_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr_; + rclcpp::Time prev_col_point_time_; + pcl::PointXYZ prev_col_point_; + +private: + geometry_msgs::msg::Pose getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer); + autoware_planning_msgs::msg::Trajectory insertSlowDownVelocity( + const size_t slow_down_start_point_idx, + const double slow_down_target_vel, + const double slow_down_vel, + const autoware_planning_msgs::msg::Trajectory & input_path); + double calcSlowDownTargetVel(const double lateral_deviation) const; + static std::tuple::Ptr> getSlowDownPointcloud( + const bool is_slow_down, const bool enable_slow_down, + const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, + const Polygon2d & one_step_polygon, + const pcl::PointCloud::Ptr slow_down_pointcloud, + const bool candidate_slow_down); + std::tuple::Ptr> getCollisionPointcloud( + const pcl::PointCloud::Ptr slow_down_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, const Polygon2d & one_step_polygon, + const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, + const pcl::PointCloud::Ptr collision_pointcloud, + const bool is_collision); + autoware_planning_msgs::msg::Trajectory insertStopPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_collision_point, + const autoware_planning_msgs::msg::Trajectory & input_msg); + autoware_planning_msgs::msg::Trajectory insertSlowDownPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_slow_down_point, + const double slow_down_target_vel, + const double slow_down_margin, + const autoware_planning_msgs::msg::Trajectory & input_msg); +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__OBSTACLE_STOP_PLANNER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp new file mode 100644 index 00000000..1019f27e --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -0,0 +1,74 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ +#define OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ + +#include +#include +#include +#include "boost/geometry.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" +#include "obstacle_stop_planner/util.hpp" + +namespace obstacle_stop_planner +{ +inline Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const double expand_width, const VehicleInfo & vehicle_info) +{ + Polygon2d one_step_move_vehicle_corner_points; + const auto footprint = createVehicleFootprint(vehicle_info, 0.0, expand_width); + + // start step + { + double yaw = getYawFromQuaternion(base_step_pose.orientation); + + boost::geometry::strategy::transform::rotate_transformer< + boost::geometry::radian, double, 2, 2> rotate(yaw); + autoware_utils::LinearRing2d rotate_footprint; + boost::geometry::transform(footprint, rotate_footprint, rotate); + + boost::geometry::strategy::transform::translate_transformer translate( + base_step_pose.position.x, base_step_pose.position.y); + autoware_utils::LinearRing2d transformed_footprint; + boost::geometry::transform(rotate_footprint, transformed_footprint, translate); + one_step_move_vehicle_corner_points.outer() = transformed_footprint; + } + // next step + { + double yaw = getYawFromQuaternion(next_step_pose.orientation); + boost::geometry::strategy::transform::rotate_transformer< + boost::geometry::radian, double, 2, 2> rotate(yaw); + autoware_utils::LinearRing2d rotate_footprint; + boost::geometry::transform(footprint, rotate_footprint, rotate); + + boost::geometry::strategy::transform::translate_transformer translate( + base_step_pose.position.x, base_step_pose.position.y); + autoware_utils::LinearRing2d transformed_footprint; + boost::geometry::transform(rotate_footprint, transformed_footprint, translate); + for (const auto & item : transformed_footprint) { + one_step_move_vehicle_corner_points.outer().emplace_back(item); + } + } + + Polygon2d polygon; + boost::geometry::convex_hull(one_step_move_vehicle_corner_points, polygon); + return polygon; +} + +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp new file mode 100644 index 00000000..1c055b3a --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp @@ -0,0 +1,124 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__PARAMETER__ADAPTIVE_CRUISE_CONTROL_PARAMETER_HPP_ +#define OBSTACLE_STOP_PLANNER__PARAMETER__ADAPTIVE_CRUISE_CONTROL_PARAMETER_HPP_ + +#include + +namespace obstacle_stop_planner +{ +struct OBSTACLE_STOP_PLANNER_PUBLIC AdaptiveCruiseControlParameter +{ + //!< @brief use tracking objects for estimating object velocity or not + bool use_object_to_est_vel; + + //!< @brief use pcl for estimating object velocity or not + bool use_pcl_to_est_vel; + + //!< @brief consider forward vehicle velocity to self upper velocity or not + bool consider_obj_velocity; + + //!< @brief The distance to extend the polygon length the object in pointcloud-object matching + double object_polygon_length_margin; + + //!< @brief The distance to extend the polygon width the object in pointcloud-object matching + double object_polygon_width_margin; + + //!< @breif Maximum time difference treated as continuous points in speed estimation using a + // point cloud + double valid_est_vel_diff_time; + + //!< @brief Time width of information used for speed estimation in speed estimation using a + // point cloud + double valid_vel_que_time; + + //!< @brief Maximum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_max; + + //!< @brief Minimum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_min; + + //!< @brief Embed a stop line if the maximum speed calculated by ACC is lower than this speed + double thresh_vel_to_stop; + + /* parameter for acc */ + //!< @brief threshold of forward obstacle velocity to insert stop line (to stop acc) + double obstacle_stop_velocity_thresh; + + //!< @brief supposed minimum acceleration in emergency stop + double emergency_stop_acceleration; + + //!< @brief supposed minimum acceleration of forward vehicle in emergency stop + double obstacle_emergency_stop_acceleration; + + //!< @brief supposed idling time to start emergency stop + double emergency_stop_idling_time; + + //!< @brief minimum distance of emergency stop + double min_dist_stop; + + //!< @brief supposed maximum acceleration in active cruise control + double max_standard_acceleration; + + //!< @brief supposed minimum acceleration(deceleration) in active cruise control + double min_standard_acceleration; + + //!< @brief supposed idling time to react object in active cruise control + double standard_idling_time; + + //!< @brief minimum distance in active cruise control + double min_dist_standard; + + //!< @brief supposed minimum acceleration of forward obstacle + double obstacle_min_standard_acceleration; + + //!< @brief margin to insert upper velocity + double margin_rate_to_change_vel; + + //!< @brief use time-compensation to calculate distance to forward vehicle + bool use_time_compensation_to_dist; + + //!< @brief gain of lowpass filter of upper velocity + double lowpass_gain_; + + //!< @brief when failed to estimate velocity, use rough velocity estimation or not + bool use_rough_est_vel; + + //!< @brief in rough velocity estimation, front car velocity is + //!< estimated as self current velocity * this value + double rough_velocity_rate; + + /* parameter for pid used in acc */ + //!< @brief coefficient P in PID control (used when target dist -current_dist >=0) + double p_coeff_pos; + + //!< @brief coefficient P in PID control (used when target dist -current_dist <0) + double p_coeff_neg; + + //!< @brief coefficient D in PID control (used when delta_dist >=0) + double d_coeff_pos; + + //!< @brief coefficient D in PID control (used when delta_dist <0) + double d_coeff_neg; + + const double d_coeff_valid_time = 1.0; + const double d_coeff_valid_diff_vel = 20.0; + const double d_max_vel_norm = 3.0; +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__PARAMETER__ADAPTIVE_CRUISE_CONTROL_PARAMETER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/slow_down_control_parameter.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/slow_down_control_parameter.hpp new file mode 100644 index 00000000..1f670a53 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/slow_down_control_parameter.hpp @@ -0,0 +1,34 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__PARAMETER__SLOW_DOWN_CONTROL_PARAMETER_HPP_ +#define OBSTACLE_STOP_PLANNER__PARAMETER__SLOW_DOWN_CONTROL_PARAMETER_HPP_ + +#include + +namespace obstacle_stop_planner +{ +struct OBSTACLE_STOP_PLANNER_PUBLIC SlowDownControlParameter +{ + double slow_down_margin; + double expand_slow_down_range; + double max_slow_down_vel; + double min_slow_down_vel; + double max_deceleration; + bool enable_slow_down; + double slow_down_search_radius; +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__PARAMETER__SLOW_DOWN_CONTROL_PARAMETER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp new file mode 100644 index 00000000..1c1186df --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp @@ -0,0 +1,33 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__PARAMETER__STOP_CONTROL_PARAMETER_HPP_ +#define OBSTACLE_STOP_PLANNER__PARAMETER__STOP_CONTROL_PARAMETER_HPP_ + +#include + +namespace obstacle_stop_planner +{ +struct OBSTACLE_STOP_PLANNER_PUBLIC StopControlParameter +{ + double stop_margin; + double min_behavior_stop_margin; + double step_length; + double extend_distance; + double expand_stop_range; + double stop_search_radius; +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__PARAMETER__STOP_CONTROL_PARAMETER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp new file mode 100644 index 00000000..179b06a7 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -0,0 +1,107 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ +#define OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "geometry_msgs/msg/pose.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "obstacle_stop_planner/util.hpp" +#include "obstacle_stop_planner/parameter/slow_down_control_parameter.hpp" +#include "obstacle_stop_planner/parameter/stop_control_parameter.hpp" + + +namespace obstacle_stop_planner +{ + +struct StopPoint +{ + size_t index; + Point2d point; +}; + +struct SlowDownPoint +{ + size_t index; + Point2d point; + double velocity; +}; + +struct PointStamped +{ + rclcpp::Time time; + Point3d point; +}; + +struct PointDeviation +{ + double deviation; + pcl::PointXYZ point; +}; + +class PointHelper +{ +public: + static Point2d getBackwardPointFromBasePoint( + const Point2d & line_point1, const Point2d & line_point2, + const Point2d & base_point, const double backward_length); + static PointStamped getNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose); + static PointDeviation getLateralNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose); + + static std::tuple + insertStopPoint( + const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, + const autoware_planning_msgs::msg::Trajectory & input_path); + + StopPoint searchInsertPoint( + const size_t idx, const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & trajectory_vec, const Point2d & collision_point_vec, + const StopControlParameter & param) const; + + static StopPoint createTargetPoint( + const size_t idx, const double margin, const Point2d & trajectory_vec, + const Point2d & collision_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path); + + SlowDownPoint createSlowDownStartPoint( + const int idx, const double margin, const double slow_down_target_vel, + const Point2d & trajectory_vec, const Point2d & slow_down_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path, + const double current_velocity_x, + const SlowDownControlParameter & param) const; + + static std::tuple + insertSlowDownStartPoint( + const SlowDownPoint & slow_down_start_point, + const autoware_planning_msgs::msg::Trajectory & base_path, + const autoware_planning_msgs::msg::Trajectory & input_path); + + static autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( + double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point); +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp new file mode 100644 index 00000000..fa9336f6 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -0,0 +1,43 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ +#define OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ + +#include +#include +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/pose.hpp" + +namespace obstacle_stop_planner +{ + +struct DecimateTrajectoryMap +{ + autoware_planning_msgs::msg::Trajectory orig_trajectory; + autoware_planning_msgs::msg::Trajectory decimate_trajectory; + std::map index_map; +}; + +autoware_planning_msgs::msg::Trajectory extendTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const double extend_distance); +DecimateTrajectoryMap decimateTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length); +std::tuple trimTrajectoryWithIndexFromSelfPose( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const geometry_msgs::msg::Pose & self_pose); +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp new file mode 100644 index 00000000..f6a7b5cb --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -0,0 +1,97 @@ +// Copyright 2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_STOP_PLANNER__UTIL_HPP_ +#define OBSTACLE_STOP_PLANNER__UTIL_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/point.hpp" +#include "boost/geometry.hpp" +#include "boost/format.hpp" +#include "boost/assign/list_of.hpp" +#include "tf2/utils.h" +#include "autoware_utils/autoware_utils.hpp" + +using autoware_utils::Point2d; +using autoware_utils::Point3d; +using autoware_utils::Polygon2d; +using autoware_utils::Polygon3d; + +namespace +{ +inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion quat(q.x, q.y, q.z, q.w); + tf2::Matrix3x3 mat(quat); + geometry_msgs::msg::Vector3 rpy; + mat.getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +inline double getYawFromQuaternion(const geometry_msgs::msg::Quaternion & quat) +{ + const auto rpy = rpyFromQuat(quat); + return rpy.z; +} + +inline geometry_msgs::msg::Pose getVehicleCenterFromBase( + const geometry_msgs::msg::Pose & base_pose, + const double vehicle_length, + const double rear_overhang) +{ + geometry_msgs::msg::Pose center_pose; + const double yaw = getYawFromQuaternion(base_pose.orientation); + center_pose.position.x = + base_pose.position.x + (vehicle_length / 2.0 - rear_overhang) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (vehicle_length / 2.0 - rear_overhang) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; +} + +inline Polygon2d getPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, + const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) +{ + Polygon2d obj_poly; + geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); + + double l = size.x * std::cos(obj_rpy.y) + l_margin; + double w = size.y * std::cos(obj_rpy.x) + w_margin; + double co = center_offset; + boost::geometry::exterior_ring(obj_poly) = + boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( + -l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); + + // rotate polygon + // original:clockwise + boost::geometry::strategy::transform::rotate_transformer< + boost::geometry::radian, double, 2, 2> rotate(-obj_rpy.z); + // rotation + Polygon2d rotate_obj_poly; + boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); + // translate polygon + boost::geometry::strategy::transform::translate_transformer translate( + pose.position.x, + pose.position.y); + Polygon2d translate_obj_poly; + boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} +} // namespace + +#endif // OBSTACLE_STOP_PLANNER__UTIL_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp new file mode 100644 index 00000000..b3a92a82 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ + +#include "autoware_utils/autoware_utils.hpp" + +namespace obstacle_stop_planner +{ +inline autoware_utils::LinearRing2d createVehicleFootprint( + const VehicleInfo & vehicle_info, const double top_margin = 0.0, + const double side_margin = 0.0) +{ + using autoware_utils::LinearRing2d; + using autoware_utils::Point2d; + + const auto & i = vehicle_info; + + const double x_front = i.front_overhang + i.wheel_base + top_margin; + const double x_rear = -(i.rear_overhang + top_margin); + const double y_left = i.wheel_tread / 2.0 + i.left_overhang + side_margin; + const double y_right = -(i.wheel_tread / 2.0 + i.right_overhang + side_margin); + + LinearRing2d footprint; + footprint.push_back(Point2d{x_front, y_left}); + footprint.push_back(Point2d{x_front, y_right}); + footprint.push_back(Point2d{x_rear, y_right}); + footprint.push_back(Point2d{x_rear, y_left}); + footprint.push_back(Point2d{x_front, y_left}); + + return footprint; +} +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp new file mode 100644 index 00000000..6151990b --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__VISIBILITY_CONTROL_HPP_ +#define OBSTACLE_STOP_PLANNER__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(OBSTACLE_STOP_PLANNER_BUILDING_DLL) || defined(OBSTACLE_STOP_PLANNER_EXPORTS) + #define OBSTACLE_STOP_PLANNER_PUBLIC __declspec(dllexport) + #define OBSTACLE_STOP_PLANNER_LOCAL + #else + #define OBSTACLE_STOP_PLANNER_PUBLIC __declspec(dllimport) + #define OBSTACLE_STOP_PLANNER_LOCAL + #endif +#elif defined(__linux__) + #define OBSTACLE_STOP_PLANNER_PUBLIC __attribute__((visibility("default"))) + #define OBSTACLE_STOP_PLANNER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define OBSTACLE_STOP_PLANNER_PUBLIC __attribute__((visibility("default"))) + #define OBSTACLE_STOP_PLANNER_LOCAL __attribute__((visibility("hidden"))) +#else + #error "Unsupported Build Configuration" +#endif + +#endif // OBSTACLE_STOP_PLANNER__VISIBILITY_CONTROL_HPP_ diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index 4833a530..a63b95f2 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -11,6 +11,8 @@ ament_cmake_auto rclcpp + rclcpp_components + autoware_debug_msgs autoware_perception_msgs autoware_planning_msgs @@ -25,10 +27,12 @@ tf2_eigen tf2_ros visualization_msgs - libopencv-dev + pcl_ros + ament_cmake_auto_gtest ament_lint_auto ament_lint_common + ros_testing ament_cmake diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index fc2debb5..339667a5 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "boost/algorithm/clamp.hpp" #include "boost/assert.hpp" @@ -26,188 +27,48 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include "obstacle_stop_planner/util.hpp" -namespace bg = boost::geometry; -using Point = bg::model::d2::point_xy; -using Polygon = bg::model::polygon; -using Line = bg::model::linestring; - -namespace -{ -Point convertPointRosToBoost(const geometry_msgs::msg::Point & point) -{ - const Point point2d(point.x, point.y); - return point2d; -} - -geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) -{ - tf2::Quaternion quat(q.x, q.y, q.z, q.w); - tf2::Matrix3x3 mat(quat); - double roll, pitch, yaw; - mat.getRPY(roll, pitch, yaw); - geometry_msgs::msg::Vector3 rpy; - rpy.x = roll; - rpy.y = pitch; - rpy.z = yaw; - return rpy; -} - -Polygon getPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, - const double center_offset, - const double l_margin = 0.0, const double w_margin = 0.0) -{ - Polygon obj_poly; - geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); - - double l = size.x * std::cos(obj_rpy.y) + l_margin; - double w = size.y * std::cos(obj_rpy.x) + w_margin; - double co = center_offset; - bg::exterior_ring(obj_poly) = boost::assign::list_of(l / 2.0 + co, w / 2.0)( - -l / 2.0 + co, w / 2.0)(-l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); - - // rotate polygon - bg::strategy::transform::rotate_transformer rotate( - -obj_rpy.z); // original:clockwise rotation - Polygon rotate_obj_poly; - bg::transform(obj_poly, rotate_obj_poly, rotate); - // translate polygon - bg::strategy::transform::translate_transformer translate( - pose.position.x, pose.position.y); - Polygon translate_obj_poly; - bg::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} - -double getDistanceFromTwoPoint( - const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2) -{ - const double dx = point1.x - point2.x; - const double dy = point1.y - point2.y; - const double dist = std::hypot(dx, dy); - return dist; -} - -constexpr double normalizeRadian( - const double rad, const double min_rad = -boost::math::constants::pi(), - const double max_rad = boost::math::constants::pi()) -{ - const auto value = std::fmod(rad, 2 * boost::math::constants::pi()); - if (min_rad < value && value <= max_rad) { - return value; - } else { - return value - std::copysign(2 * boost::math::constants::pi(), value); - } -} - -constexpr double sign(const double value) -{ - if (value > 0) { - return 1.0; - } else if (value < 0) { - return -1.0; - } else { - return 0.0; - } -} -} // namespace - -namespace motion_planning +namespace obstacle_stop_planner { AdaptiveCruiseController::AdaptiveCruiseController( - rclcpp::Node * node, const double vehicle_width, const double vehicle_length, - const double wheel_base, const double front_overhang) + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const AdaptiveCruiseControlParameter & acc_param) : node_(node), - vehicle_width_(vehicle_width), - vehicle_length_(vehicle_length), - wheel_base_(wheel_base), - front_overhang_(front_overhang) + vehicle_info_(vehicle_info), + param_(acc_param), + prev_collision_point_(0.0, 0.0) { - // get parameter - std::string acc_ns = "adaptive_cruise_control."; - - /* config */ - param_.min_behavior_stop_margin = - node_->get_parameter("stop_planner.min_behavior_stop_margin").as_double() + wheel_base_ + - front_overhang_; - param_.use_object_to_est_vel = - node_->declare_parameter(acc_ns + "use_object_to_estimate_vel", true); - param_.use_pcl_to_est_vel = node_->declare_parameter(acc_ns + "use_pcl_to_estimate_vel", true); - param_.consider_obj_velocity = node_->declare_parameter(acc_ns + "consider_obj_velocity", true); - - /* parameter for acc */ - param_.obstacle_stop_velocity_thresh = - node_->declare_parameter(acc_ns + "obstacle_stop_velocity_thresh", 1.0); - param_.emergency_stop_acceleration = - node_->declare_parameter(acc_ns + "emergency_stop_acceleration", -3.5); - param_.obstacle_emergency_stop_acceleration = - node_->declare_parameter(acc_ns + "obstacle_emergency_stop_acceleration", -5.0); - param_.emergency_stop_idling_time = - node_->declare_parameter(acc_ns + "emergency_stop_idling_time", 0.5); - param_.min_dist_stop = node_->declare_parameter(acc_ns + "min_dist_stop", 4.0); - param_.max_standard_acceleration = - node_->declare_parameter(acc_ns + "max_standard_acceleration", 0.5); - param_.min_standard_acceleration = - node_->declare_parameter(acc_ns + "min_standard_acceleration", -1.0); - param_.standard_idling_time = node_->declare_parameter(acc_ns + "standard_idling_time", 0.5); - param_.min_dist_standard = node_->declare_parameter(acc_ns + "min_dist_standard", 4.0); - param_.obstacle_min_standard_acceleration = - node_->declare_parameter(acc_ns + "obstacle_min_standard_acceleration", -1.5); - param_.margin_rate_to_change_vel = - node_->declare_parameter(acc_ns + "margin_rate_to_change_vel", 0.3); - param_.use_time_compensation_to_dist = - node_->declare_parameter(acc_ns + "use_time_compensation_to_calc_distance", true); - param_.lowpass_gain_ = node_->declare_parameter(acc_ns + "lowpass_gain_of_upper_velocity", 0.6); - - /* parameter for pid in acc */ - param_.p_coeff_pos = node_->declare_parameter(acc_ns + "p_coefficient_positive", 0.1); - param_.p_coeff_neg = node_->declare_parameter(acc_ns + "p_coefficient_negative", 0.3); - param_.d_coeff_pos = node_->declare_parameter(acc_ns + "d_coefficient_positive", 0.0); - param_.d_coeff_neg = node_->declare_parameter(acc_ns + "d_coefficient_negative", 0.1); - - /* parameter for speed estimation of obstacle */ - param_.object_polygon_length_margin = - node_->declare_parameter(acc_ns + "object_polygon_length_margin", 2.0); - param_.object_polygon_width_margin = - node_->declare_parameter(acc_ns + "object_polygon_width_margin", 0.5); - param_.valid_est_vel_diff_time = - node_->declare_parameter(acc_ns + "valid_estimated_vel_diff_time", 1.0); - param_.valid_vel_que_time = node_->declare_parameter(acc_ns + "valid_vel_que_time", 0.5); - param_.valid_est_vel_max = node_->declare_parameter(acc_ns + "valid_estimated_vel_max", 20.0); - param_.valid_est_vel_min = node_->declare_parameter(acc_ns + "valid_estimated_vel_min", -20.0); - param_.thresh_vel_to_stop = node_->declare_parameter(acc_ns + "thresh_vel_to_stop", 0.5); - param_.use_rough_est_vel = - node_->declare_parameter(acc_ns + "use_rough_velocity_estimation", false); - param_.rough_velocity_rate = node_->declare_parameter(acc_ns + "rough_velocity_rate", 0.9); - /* publisher */ pub_debug_ = node_->create_publisher( "~/debug_values", 1); } -void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( +std::tuple +AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_collision_point_idx, - const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time nearest_collision_point_time, + const geometry_msgs::msg::Pose self_pose, const Point2d & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, - const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, - autoware_planning_msgs::msg::Trajectory * output_trajectory) + const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, + const autoware_planning_msgs::msg::Trajectory & input_trajectory) { debug_values_.data.clear(); debug_values_.data.resize(num_debug_values_, 0.0); const double current_velocity = current_velocity_ptr->twist.linear.x; - double col_point_distance; - double point_velocity; + double point_velocity = current_velocity; bool success_estm_vel = false; + auto output_trajectory = input_trajectory; + /* * calc distance to collision point */ - calcDistanceToNearestPointOnPath( + double col_point_distance = calcDistanceToNearestPointOnPath( trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point, - nearest_collision_point_time, &col_point_distance); + nearest_collision_point_time); /* * calc yaw of trajectory at collision point @@ -218,18 +79,18 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( * estimate velocity of collision point */ if (param_.use_pcl_to_est_vel) { - if (estimatePointVelocityFromPcl( - traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity)) - { - success_estm_vel = true; + const auto velocity = estimatePointVelocityFromPcl( + traj_yaw, nearest_collision_point, nearest_collision_point_time); + if (velocity) { + point_velocity = velocity.get(); } } if (param_.use_object_to_est_vel) { - if (estimatePointVelocityFromObject( - object_ptr, traj_yaw, nearest_collision_point, &point_velocity)) - { - success_estm_vel = true; + const auto velocity = estimatePointVelocityFromObject( + object_ptr, traj_yaw, nearest_collision_point); + if (velocity) { + point_velocity = velocity.get(); } } @@ -243,11 +104,10 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "Failed to estimate velocity of forward vehicle. Insert stop line."); - *need_to_stop = true; prev_upper_velocity_ = current_velocity; // reset prev_upper_velocity prev_target_velocity_ = 0.0; pub_debug_->publish(debug_values_); - return; + return std::forward_as_tuple(true, output_trajectory); } // calculate max(target) velocity of self @@ -260,8 +120,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "Upper velocity is too low. Insert stop line."); - *need_to_stop = true; - return; + return std::forward_as_tuple(true, output_trajectory); } /* @@ -269,59 +128,59 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( */ insertMaxVelocityToPath( self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory); - *need_to_stop = false; + return std::forward_as_tuple(false, output_trajectory); } -void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( +double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, - const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * distance) + const geometry_msgs::msg::Pose & self_pose, const Point2d & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time) { - if (trajectory.points.size() == 0) { + double distance = 0.0; + if (trajectory.points.empty()) { RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "input path is too short(size=0)"); - *distance = 0; - return; + return 0; } // get self polygon geometry_msgs::msg::Vector3 self_size; - self_size.x = vehicle_length_; - self_size.y = vehicle_width_; - double self_offset = (wheel_base_ + front_overhang_) - vehicle_length_ / 2.0; - Polygon self_poly = getPolygon(self_pose, self_size, self_offset); + self_size.x = vehicle_info_.vehicle_length; + self_size.y = vehicle_info_.vehicle_width; + double self_offset = (vehicle_info_.wheel_base + vehicle_info_.front_overhang) - + vehicle_info_.vehicle_length / 2.0; + const auto self_poly = getPolygon(self_pose, self_size, self_offset); // get nearest point - Point nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); - if (nearest_point_idx <= 2) { // if too nearest collision point, return direct distance - *distance = boost::geometry::distance(self_poly, nearest_point2d); - debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; - return; + distance = boost::geometry::distance(self_poly, nearest_collision_point); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = distance; + return distance; } /* get total distance to collision point */ double dist_to_point = 0; // get distance from self to next nearest point dist_to_point += boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(trajectory.points.at(1).pose.position)); + autoware_utils::fromMsg(self_pose.position).to_2d(), + autoware_utils::fromMsg(trajectory.points.at(1).pose.position).to_2d()); // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx for (int i = 1; i < nearest_point_idx - 1; i++) { - dist_to_point += getDistanceFromTwoPoint( - trajectory.points.at(i).pose.position, trajectory.points.at(i + 1).pose.position); + dist_to_point += boost::geometry::distance( + autoware_utils::fromMsg(trajectory.points.at(i).pose.position).to_2d(), + autoware_utils::fromMsg(trajectory.points.at(i + 1).pose.position).to_2d()); } // add distance from nearest_collision_point to prev point of nearest_point_idx dist_to_point += boost::geometry::distance( - nearest_point2d, - convertPointRosToBoost(trajectory.points.at(nearest_point_idx - 1).pose.position)); + nearest_collision_point, + autoware_utils::fromMsg(trajectory.points.at(nearest_point_idx - 1).pose.position).to_2d()); // subtract base_link to front - dist_to_point -= param_.min_behavior_stop_margin; + dist_to_point -= min_behavior_stop_margin_; // time compensation if (param_.use_time_compensation_to_dist) { @@ -330,8 +189,9 @@ void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( dist_to_point += prev_target_velocity_ * delay_time; } - *distance = std::max(0.0, dist_to_point); - debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; + distance = std::max(0.0, dist_to_point); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = distance; + return distance; } double AdaptiveCruiseController::calcTrajYaw( @@ -340,26 +200,20 @@ double AdaptiveCruiseController::calcTrajYaw( return tf2::getYaw(trajectory.points.at(collision_point_idx).pose.orientation); } -bool AdaptiveCruiseController::estimatePointVelocityFromObject( +optional AdaptiveCruiseController::estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const pcl::PointXYZ & nearest_collision_point, double * velocity) + const Point2d & nearest_collision_point) { - geometry_msgs::msg::Point nearest_collision_p_ros; - nearest_collision_p_ros.x = nearest_collision_point.x; - nearest_collision_p_ros.y = nearest_collision_point.y; - nearest_collision_p_ros.z = nearest_collision_point.z; - /* get object velocity, and current yaw */ bool get_obj = false; - double obj_vel; - double obj_yaw; - const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); + double obj_vel = 0.0; + double obj_yaw = 0.0; for (const auto & obj : object_ptr->objects) { - const Polygon obj_poly = getPolygon( + const auto obj_poly = getPolygon( obj.state.pose_covariance.pose, obj.shape.dimensions, 0.0, param_.object_polygon_length_margin, param_.object_polygon_width_margin); - if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { + if (boost::geometry::distance(obj_poly, nearest_collision_point) <= 0) { obj_vel = obj.state.twist_covariance.twist.linear.x; obj_yaw = tf2::getYaw(obj.state.pose_covariance.pose.orientation); get_obj = true; @@ -368,23 +222,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( } if (get_obj) { - *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); - debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; - return true; - } else { - return false; + const auto velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = velocity; + return velocity; } + return boost::none; } -bool AdaptiveCruiseController::estimatePointVelocityFromPcl( - const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * velocity) +optional AdaptiveCruiseController::estimatePointVelocityFromPcl( + const double traj_yaw, const Point2d & nearest_collision_point, + const rclcpp::Time & nearest_collision_point_time) { - geometry_msgs::msg::Point nearest_collision_p_ros; - nearest_collision_p_ros.x = nearest_collision_point.x; - nearest_collision_p_ros.y = nearest_collision_point.y; - nearest_collision_p_ros.z = nearest_collision_point.z; - /* estimate velocity */ const double p_dt = nearest_collision_point_time.seconds() - prev_collision_point_time_.seconds(); @@ -396,12 +244,11 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_time_ = nearest_collision_point_time; prev_collision_point_ = nearest_collision_point; prev_collision_point_valid_ = true; - return false; + return boost::none; } - const double p_dx = nearest_collision_point.x - prev_collision_point_.x; - const double p_dy = nearest_collision_point.y - prev_collision_point_.y; - const double p_dist = std::hypot(p_dx, p_dy); - const double p_yaw = std::atan2(p_dy, p_dx); + const double p_dist = boost::geometry::distance(nearest_collision_point, prev_collision_point_); + const auto p_diff = nearest_collision_point - prev_collision_point_; + const double p_yaw = std::atan2(p_diff.x(), p_diff.y()); const double p_vel = p_dist / p_dt; const double est_velocity = p_vel * std::cos(p_yaw - traj_yaw); // valid velocity check @@ -410,7 +257,7 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_ = nearest_collision_point; prev_collision_point_valid_ = true; est_vel_que_.clear(); - return false; + return boost::none; } // append new velocity and remove old velocity from que @@ -418,17 +265,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl( } // calc average(median) velocity from que - *velocity = getMedianVel(est_vel_que_); - debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = *velocity; + const auto velocity = getMedianVel(est_vel_que_); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = velocity; prev_collision_point_time_ = nearest_collision_point_time; prev_collision_point_ = nearest_collision_point; - prev_target_velocity_ = *velocity; + prev_target_velocity_ = velocity; prev_collision_point_valid_ = true; - return true; + return velocity; } -double AdaptiveCruiseController::estimateRoughPointVelocity(double current_vel) +double AdaptiveCruiseController::estimateRoughPointVelocity(const double current_vel) { const double p_dt = node_->now().seconds() - prev_collision_point_time_.seconds(); if (param_.valid_est_vel_diff_time >= p_dt) { @@ -471,7 +318,7 @@ double AdaptiveCruiseController::calcUpperVelocity( } double AdaptiveCruiseController::calcThreshDistToForwardObstacle( - const double current_vel, const double obj_vel) + const double current_vel, const double obj_vel) const { const double current_vel_min = std::max(1.0, std::fabs(current_vel)); const double obj_vel_min = std::max(0.0, obj_vel); @@ -488,7 +335,7 @@ double AdaptiveCruiseController::calcThreshDistToForwardObstacle( } double AdaptiveCruiseController::calcBaseDistToForwardObstacle( - const double current_vel, const double obj_vel) + const double current_vel, const double obj_vel) const { const double obj_vel_min = std::max(0.0, obj_vel); const double minimum_distance = param_.min_dist_standard; @@ -503,10 +350,10 @@ double AdaptiveCruiseController::calcBaseDistToForwardObstacle( } double AdaptiveCruiseController::calcTargetVelocity_P( - const double target_dist, const double current_dist) + const double target_dist, const double current_dist) const { const double diff_dist = current_dist - target_dist; - double add_vel_p; + double add_vel_p = 0.0; if (diff_dist >= 0) { add_vel_p = diff_dist * param_.p_coeff_pos; } else { @@ -516,7 +363,7 @@ double AdaptiveCruiseController::calcTargetVelocity_P( } double AdaptiveCruiseController::calcTargetVelocity_I( - const double target_dist, const double current_dist) + const double /*target_dist*/, const double /*current_dist*/) { // not implemented return 0.0; @@ -525,7 +372,9 @@ double AdaptiveCruiseController::calcTargetVelocity_I( double AdaptiveCruiseController::calcTargetVelocity_D( const double target_dist, const double current_dist) { - if (node_->now().seconds() - prev_target_vehicle_time_ >= param_.d_coeff_valid_time) { + if (node_->now().seconds() - prev_target_vehicle_time_ >= + param_.d_coeff_valid_time) + { // invalid time(prev is too old) return 0.0; } @@ -541,9 +390,10 @@ double AdaptiveCruiseController::calcTargetVelocity_D( double add_vel_d = 0; add_vel_d = diff_vel; - if (add_vel_d >= 0) {diff_vel *= param_.d_coeff_pos;} - if (add_vel_d < 0) {diff_vel *= param_.d_coeff_neg;} - add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm); + add_vel_d = boost::algorithm::clamp( + add_vel_d, + -param_.d_max_vel_norm, + param_.d_max_vel_norm); // add buffer prev_target_vehicle_dist_ = current_dist; @@ -572,20 +422,14 @@ double AdaptiveCruiseController::calcTargetVelocityByPID( return target_vel; } -void AdaptiveCruiseController::insertMaxVelocityToPath( - const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, - const double dist_to_collision_point, autoware_planning_msgs::msg::Trajectory * output_trajectory) +autoware_planning_msgs::msg::Trajectory +AdaptiveCruiseController::insertMaxVelocityToPath( + const geometry_msgs::msg::Pose & /*self_pose*/, const double current_vel, const double target_vel, + const double dist_to_collision_point, + const autoware_planning_msgs::msg::Trajectory & input_trajectory) const { // plus distance from self to next nearest point - double dist = dist_to_collision_point; - double dist_to_first_point = 0.0; - if (output_trajectory->points.size() > 1) { - dist_to_first_point = boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(output_trajectory->points.at(1).pose.position)); - } - dist += dist_to_first_point; - + auto output_trajectory = input_trajectory; double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; // accel = (v_after^2 - v_before^2 ) / 2x double target_acc = (std::pow(target_vel, 2) - std::pow(current_vel, 2)) / (2 * margin_to_insert); @@ -594,14 +438,16 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( target_acc, param_.min_standard_acceleration, param_.max_standard_acceleration); double pre_vel = current_vel; double total_dist = 0.0; - for (size_t i = 1; i < output_trajectory->points.size(); i++) { + for (size_t i = 1; i < output_trajectory.points.size(); i++) { // calc velocity of each point by gradient deceleration - const auto current_p = output_trajectory->points[i]; - const auto prev_p = output_trajectory->points[i - 1]; - const double p_dist = getDistanceFromTwoPoint(current_p.pose.position, prev_p.pose.position); + const auto current_p = output_trajectory.points[i]; + const auto prev_p = output_trajectory.points[i - 1]; + const auto p_dist = boost::geometry::distance( + autoware_utils::fromMsg(current_p.pose.position).to_2d(), + autoware_utils::fromMsg(prev_p.pose.position).to_2d()); total_dist += p_dist; if (current_p.twist.linear.x > target_vel && total_dist >= 0) { - double next_pre_vel; + double next_pre_vel = 0.0; if (std::fabs(clipped_acc) < 1e-05) { next_pre_vel = pre_vel; } else { @@ -616,13 +462,14 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( if (total_dist >= margin_to_insert) { const double max_velocity = std::max(target_vel, next_pre_vel); - if (output_trajectory->points[i].twist.linear.x > max_velocity) { - output_trajectory->points[i].twist.linear.x = max_velocity; + if (output_trajectory.points[i].twist.linear.x > max_velocity) { + output_trajectory.points[i].twist.linear.x = max_velocity; } } pre_vel = next_pre_vel; } } + return output_trajectory; } void AdaptiveCruiseController::registerQueToVelocity( @@ -639,8 +486,9 @@ void AdaptiveCruiseController::registerQueToVelocity( delete_idxs.push_back(i); } } - for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { - est_vel_que_.erase(est_vel_que_.begin() + delete_idxs.at(delete_idx)); + // for (size_t delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { + for (const auto & delete_idx : boost::adaptors::reverse(delete_idxs)) { + est_vel_que_.erase(est_vel_que_.begin() + delete_idx); } // append new que @@ -651,19 +499,19 @@ void AdaptiveCruiseController::registerQueToVelocity( } double AdaptiveCruiseController::getMedianVel( - const std::vector vel_que) + const std::vector & vel_que) { - if (vel_que.size() == 0) { + if (vel_que.empty()) { RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong."); return 0.0; } std::vector raw_vel_que; - for (const auto vel : vel_que) { + for (const auto & vel : vel_que) { raw_vel_que.emplace_back(vel.twist.linear.x); } - double med_vel; + double med_vel = 0.0; if (raw_vel_que.size() % 2 == 0) { size_t med1 = (raw_vel_que.size()) / 2 - 1; size_t med2 = (raw_vel_que.size()) / 2; @@ -687,4 +535,4 @@ double AdaptiveCruiseController::lowpass_filter( return gain * prev_value + (1.0 - gain) * current_value; } -} // namespace motion_planning +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index 35b99c08..86c226aa 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -16,13 +16,9 @@ #include #include "obstacle_stop_planner/debug_marker.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "autoware_utils/autoware_utils.hpp" -// namespace { -// convertPose2Transform -// } - -namespace motion_planning +namespace obstacle_stop_planner { ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( rclcpp::Node * node, const double base_link2front) @@ -35,32 +31,30 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const std::vector & polygon, const double z, const PolygonType & type) + const Polygon2d & polygon, const double z, const PolygonType & type) { - std::vector eigen_polygon; - for (const auto & point : polygon) { - Eigen::Vector3d eigen_point; - eigen_point << point.x, point.y, z; - eigen_polygon.push_back(eigen_point); + Polygon3d polygon3d; + for (const auto & point : polygon.outer()) { + polygon3d.outer().emplace_back(point.to_3d(z)); } - return pushPolygon(eigen_polygon, type); + return pushPolygon(polygon3d, type); } bool ObstacleStopPlannerDebugNode::pushPolygon( - const std::vector & polygon, const PolygonType & type) + const Polygon3d & polygon, const PolygonType & type) { switch (type) { case PolygonType::Vehicle: - if (!polygon.empty()) {vehicle_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {vehicle_polygons_.emplace_back(polygon);} return true; case PolygonType::Collision: - if (!polygon.empty()) {collision_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {collision_polygons_.emplace_back(polygon);} return true; case PolygonType::SlowDownRange: - if (!polygon.empty()) {slow_down_range_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {slow_down_range_polygons_.emplace_back(polygon);} return true; case PolygonType::SlowDown: - if (!polygon.empty()) {slow_down_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {slow_down_polygons_.emplace_back(polygon);} return true; default: return false; @@ -102,13 +96,9 @@ bool ObstacleStopPlannerDebugNode::pushObstaclePoint( } bool ObstacleStopPlannerDebugNode::pushObstaclePoint( - const pcl::PointXYZ & obstacle_point, const PointType & type) + const Point3d & obstacle_point, const PointType & type) { - geometry_msgs::msg::Point ros_point; - ros_point.x = obstacle_point.x; - ros_point.y = obstacle_point.y; - ros_point.z = obstacle_point.z; - return pushObstaclePoint(ros_point, type); + return pushObstaclePoint(autoware_utils::toMsg(obstacle_point), type); } void ObstacleStopPlannerDebugNode::publish() @@ -164,27 +154,23 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; - for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { - for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + for (auto & vehicle_polygon : vehicle_polygons_) { + for (size_t j = 0; j < vehicle_polygon.outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = vehicle_polygons_.at(i).at(j).x(); - point.y = vehicle_polygons_.at(i).at(j).y(); - point.z = vehicle_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + vehicle_polygon.outer().at( + j)); marker.points.push_back(point); } - if (j + 1 == vehicle_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = vehicle_polygons_.at(i).at(0).x(); - point.y = vehicle_polygons_.at(i).at(0).y(); - point.z = vehicle_polygons_.at(i).at(0).z(); + if (j + 1 == vehicle_polygon.outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg( + vehicle_polygon.outer().at( + 0)); marker.points.push_back(point); - } else { - geometry_msgs::msg::Point point; - point.x = vehicle_polygons_.at(i).at(j + 1).x(); - point.y = vehicle_polygons_.at(i).at(j + 1).y(); - point.z = vehicle_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + vehicle_polygon.outer().at( + j + 1)); marker.points.push_back(point); } } @@ -215,27 +201,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; - for (size_t i = 0; i < collision_polygons_.size(); ++i) { - for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + for (auto & collision_polygon : collision_polygons_) { + for (size_t j = 0; j < collision_polygon.outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = collision_polygons_.at(i).at(j).x(); - point.y = collision_polygons_.at(i).at(j).y(); - point.z = collision_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + collision_polygon.outer().at(j)); marker.points.push_back(point); } - if (j + 1 == collision_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = collision_polygons_.at(i).at(0).x(); - point.y = collision_polygons_.at(i).at(0).y(); - point.z = collision_polygons_.at(i).at(0).z(); + if (j + 1 == collision_polygon.outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg( + collision_polygon.outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point; - point.x = collision_polygons_.at(i).at(j + 1).x(); - point.y = collision_polygons_.at(i).at(j + 1).y(); - point.z = collision_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + collision_polygon.outer().at(j + 1)); marker.points.push_back(point); } } @@ -266,27 +246,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; - for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { - for (size_t j = 0; j < slow_down_range_polygons_.at(i).size(); ++j) { + for (auto & slow_down_range_polygon : slow_down_range_polygons_) { + for (size_t j = 0; j < slow_down_range_polygon.outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = slow_down_range_polygons_.at(i).at(j).x(); - point.y = slow_down_range_polygons_.at(i).at(j).y(); - point.z = slow_down_range_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_range_polygon.outer().at(j)); marker.points.push_back(point); } - if (j + 1 == slow_down_range_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = slow_down_range_polygons_.at(i).at(0).x(); - point.y = slow_down_range_polygons_.at(i).at(0).y(); - point.z = slow_down_range_polygons_.at(i).at(0).z(); + if (j + 1 == slow_down_range_polygon.outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_range_polygon.outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point; - point.x = slow_down_range_polygons_.at(i).at(j + 1).x(); - point.y = slow_down_range_polygons_.at(i).at(j + 1).y(); - point.z = slow_down_range_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_range_polygon.outer().at(j + 1)); marker.points.push_back(point); } } @@ -317,27 +291,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; - for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { - for (size_t j = 0; j < slow_down_polygons_.at(i).size(); ++j) { + for (auto & slow_down_polygon : slow_down_polygons_) { + for (size_t j = 0; j < slow_down_polygon.outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = slow_down_polygons_.at(i).at(j).x(); - point.y = slow_down_polygons_.at(i).at(j).y(); - point.z = slow_down_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_polygon.outer().at(j)); marker.points.push_back(point); } - if (j + 1 == slow_down_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = slow_down_polygons_.at(i).at(0).x(); - point.y = slow_down_polygons_.at(i).at(0).y(); - point.z = slow_down_polygons_.at(i).at(0).z(); + if (j + 1 == slow_down_polygon.outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_polygon.outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point; - point.x = slow_down_polygons_.at(i).at(j + 1).x(); - point.y = slow_down_polygons_.at(i).at(j + 1).y(); - point.z = slow_down_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_polygon.outer().at(j + 1)); marker.points.push_back(point); } } @@ -622,4 +590,4 @@ autoware_planning_msgs::msg::StopReasonArray ObstacleStopPlannerDebugNode::makeS return stop_reason_array; } -} // namespace motion_planning +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index b73ee896..14cc2e6f 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -12,120 +12,135 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include -#include -#include -#include "autoware_utils/geometry/geometry.hpp" -#include "diagnostic_msgs/msg/key_value.hpp" -#include "pcl/filters/voxel_grid.h" -#include "tf2/utils.h" -#include "tf2_eigen/tf2_eigen.h" -#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 "obstacle_stop_planner/node.hpp" #include "vehicle_info_util/vehicle_info.hpp" - -#define EIGEN_MPL2_ONLY -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" -namespace -{ -double getYawFromGeometryMsgsQuaternion(const geometry_msgs::msg::Quaternion & quat) -{ - tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); - double roll, pitch, yaw; - tf2::Matrix3x3(tf2_quat).getRPY(roll, pitch, yaw); - - return yaw; -} -std::string 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; -} -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; - diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; - stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stop_reason_diag.name = "stop_reason"; - stop_reason_diag.message = stop_reason; - stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); - stop_reason_diag.values.push_back(stop_reason_diag_kv); - return stop_reason_diag; -} -} // namespace - -namespace motion_planning -{ -namespace bg = boost::geometry; -using Point = bg::model::d2::point_xy; -using Polygon = bg::model::polygon; -using Line = bg::model::linestring; - -ObstacleStopPlannerNode::ObstacleStopPlannerNode() -: Node("obstacle_stop_planner"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) -{ - // Vehicle Parameters - auto vehicle_info(vehicle_info_util::VehicleInfo::create(*this)); - - wheel_base_ = vehicle_info.wheel_base_m_; - front_overhang_ = vehicle_info.front_overhang_m_; - rear_overhang_ = vehicle_info.rear_overhang_m_; - left_overhang_ = vehicle_info.left_overhang_m_; - right_overhang_ = vehicle_info.right_overhang_m_; - vehicle_width_ = vehicle_info.vehicle_width_m_; - vehicle_length_ = vehicle_info.vehicle_length_m_; +#include "obstacle_stop_planner/parameter/stop_control_parameter.hpp" +#include "obstacle_stop_planner/parameter/slow_down_control_parameter.hpp" +#include "obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp" + +namespace obstacle_stop_planner +{ +ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) +: Node{"obstacle_stop_planner", node_options}, + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + pointcloud_received_(false), + current_velocity_reveived_(false) +{ + // Vehicle Info + VehicleInfo vehicle_info; + auto i = vehicle_info_util::VehicleInfo::create(*this); + vehicle_info.wheel_radius = i.wheel_radius_m_; + vehicle_info.wheel_width = i.wheel_width_m_; + vehicle_info.wheel_base = i.wheel_base_m_; + vehicle_info.wheel_tread = i.wheel_tread_m_; + vehicle_info.front_overhang = i.front_overhang_m_; + vehicle_info.rear_overhang = i.rear_overhang_m_; + vehicle_info.left_overhang = i.left_overhang_m_; + vehicle_info.right_overhang = i.right_overhang_m_; + vehicle_info.vehicle_height = i.vehicle_height_m_; + vehicle_info.vehicle_length = i.vehicle_length_m_; + vehicle_info.vehicle_width = i.vehicle_width_m_; + vehicle_info.min_longitudinal_offset = i.min_longitudinal_offset_m_; + vehicle_info.max_longitudinal_offset = i.max_longitudinal_offset_m_; + vehicle_info.min_lateral_offset = i.min_lateral_offset_m_; + vehicle_info.max_lateral_offset = i.max_lateral_offset_m_; + vehicle_info.min_height_offset = i.min_height_offset_m_; + vehicle_info.max_height_offset = i.max_height_offset_m_; // Parameters - stop_margin_ = declare_parameter("stop_planner.stop_margin", 5.0); - min_behavior_stop_margin_ = declare_parameter("stop_planner.min_behavior_stop_margin", 2.0); - step_length_ = declare_parameter("stop_planner.step_length", 1.0); - extend_distance_ = declare_parameter("stop_planner.extend_distance", 0.0); - expand_stop_range_ = declare_parameter("stop_planner.expand_stop_range", 0.0); - - slow_down_margin_ = declare_parameter("slow_down_planner.slow_down_margin", 5.0); - expand_slow_down_range_ = declare_parameter("slow_down_planner.expand_slow_down_range", 1.0); - max_slow_down_vel_ = declare_parameter("slow_down_planner.max_slow_down_vel", 4.0); - min_slow_down_vel_ = declare_parameter("slow_down_planner.min_slow_down_vel", 2.0); - max_deceleration_ = declare_parameter("slow_down_planner.max_deceleration", 2.0); - enable_slow_down_ = declare_parameter("enable_slow_down", false); - - stop_margin_ += wheel_base_ + front_overhang_; - min_behavior_stop_margin_ += wheel_base_ + front_overhang_; - slow_down_margin_ += wheel_base_ + front_overhang_; - stop_search_radius_ = - step_length_ + std::hypot(vehicle_width_ / 2.0 + expand_stop_range_, vehicle_length_ / 2.0); - slow_down_search_radius_ = - step_length_ + - std::hypot(vehicle_width_ / 2.0 + expand_slow_down_range_, vehicle_length_ / 2.0); - debug_ptr_ = std::make_shared(this, wheel_base_ + front_overhang_); - - // Initializer - acc_controller_ = std::make_unique( - this, vehicle_width_, vehicle_length_, wheel_base_, front_overhang_); + obstacle_stop_planner::StopControlParameter stop_param; + stop_param.stop_margin = declare_parameter("stop_planner.stop_margin", 5.0); + stop_param.min_behavior_stop_margin = + declare_parameter("stop_planner.min_behavior_stop_margin", 2.0); + stop_param.step_length = declare_parameter("stop_planner.step_length", 1.0); + stop_param.extend_distance = declare_parameter("stop_planner.extend_distance", 0.0); + stop_param.expand_stop_range = declare_parameter("stop_planner.expand_stop_range", 0.0); + + obstacle_stop_planner::SlowDownControlParameter slow_down_param; + slow_down_param.slow_down_margin = declare_parameter("slow_down_planner.slow_down_margin", 5.0); + slow_down_param.expand_slow_down_range = + declare_parameter("slow_down_planner.expand_slow_down_range", 1.0); + slow_down_param.max_slow_down_vel = declare_parameter("slow_down_planner.max_slow_down_vel", 4.0); + slow_down_param.min_slow_down_vel = declare_parameter("slow_down_planner.min_slow_down_vel", 2.0); + slow_down_param.max_deceleration = declare_parameter("slow_down_planner.max_deceleration", 2.0); + slow_down_param.enable_slow_down = declare_parameter("enable_slow_down", false); + + stop_param.stop_margin += vehicle_info.wheel_base + vehicle_info.front_overhang; + stop_param.min_behavior_stop_margin += + vehicle_info.wheel_base + vehicle_info.front_overhang; + slow_down_param.slow_down_margin += vehicle_info.wheel_base + vehicle_info.front_overhang; + stop_param.stop_search_radius = stop_param.step_length + std::hypot( + vehicle_info.vehicle_width / 2.0 + stop_param.expand_stop_range, + vehicle_info.vehicle_length / 2.0); + slow_down_param.slow_down_search_radius = stop_param.step_length + std::hypot( + vehicle_info.vehicle_width / 2.0 + slow_down_param.expand_slow_down_range, + vehicle_info.vehicle_length / 2.0); + + // Parameters for adaptive_cruise_control + obstacle_stop_planner::AdaptiveCruiseControlParameter acc_param; + std::string acc_ns = "adaptive_cruise_control."; + acc_param.use_object_to_est_vel = + declare_parameter(acc_ns + "use_object_to_estimate_vel", true); + acc_param.use_pcl_to_est_vel = declare_parameter(acc_ns + "use_pcl_to_estimate_vel", true); + acc_param.consider_obj_velocity = declare_parameter(acc_ns + "consider_obj_velocity", true); + acc_param.obstacle_stop_velocity_thresh = + declare_parameter(acc_ns + "obstacle_stop_velocity_thresh", 1.0); + acc_param.emergency_stop_acceleration = + declare_parameter(acc_ns + "emergency_stop_acceleration", -3.5); + acc_param.obstacle_emergency_stop_acceleration = + declare_parameter(acc_ns + "obstacle_emergency_stop_acceleration", -5.0); + acc_param.emergency_stop_idling_time = + declare_parameter(acc_ns + "emergency_stop_idling_time", 0.5); + acc_param.min_dist_stop = declare_parameter(acc_ns + "min_dist_stop", 4.0); + acc_param.max_standard_acceleration = + declare_parameter(acc_ns + "max_standard_acceleration", 0.5); + acc_param.min_standard_acceleration = + declare_parameter(acc_ns + "min_standard_acceleration", -1.0); + acc_param.standard_idling_time = declare_parameter(acc_ns + "standard_idling_time", 0.5); + acc_param.min_dist_standard = declare_parameter(acc_ns + "min_dist_standard", 4.0); + acc_param.obstacle_min_standard_acceleration = + declare_parameter(acc_ns + "obstacle_min_standard_acceleration", -1.5); + acc_param.margin_rate_to_change_vel = + declare_parameter(acc_ns + "margin_rate_to_change_vel", 0.3); + acc_param.use_time_compensation_to_dist = + declare_parameter(acc_ns + "use_time_compensation_to_calc_distance", true); + acc_param.lowpass_gain_ = declare_parameter(acc_ns + "lowpass_gain_of_upper_velocity", 0.6); + + /* parameter for pid in acc */ + acc_param.p_coeff_pos = declare_parameter(acc_ns + "p_coefficient_positive", 0.1); + acc_param.p_coeff_neg = declare_parameter(acc_ns + "p_coefficient_negative", 0.3); + acc_param.d_coeff_pos = declare_parameter(acc_ns + "d_coefficient_positive", 0.0); + acc_param.d_coeff_neg = declare_parameter(acc_ns + "d_coefficient_negative", 0.1); + + /* parameter for speed estimation of obstacle */ + acc_param.object_polygon_length_margin = + declare_parameter(acc_ns + "object_polygon_length_margin", 2.0); + acc_param.object_polygon_width_margin = + declare_parameter(acc_ns + "object_polygon_width_margin", 0.5); + acc_param.valid_est_vel_diff_time = + declare_parameter(acc_ns + "valid_estimated_vel_diff_time", 1.0); + acc_param.valid_vel_que_time = declare_parameter(acc_ns + "valid_vel_que_time", 0.5); + acc_param.valid_est_vel_max = declare_parameter(acc_ns + "valid_estimated_vel_max", 20.0); + acc_param.valid_est_vel_min = declare_parameter(acc_ns + "valid_estimated_vel_min", -20.0); + acc_param.thresh_vel_to_stop = declare_parameter(acc_ns + "thresh_vel_to_stop", 0.5); + acc_param.use_rough_est_vel = + declare_parameter(acc_ns + "use_rough_velocity_estimation", false); + acc_param.rough_velocity_rate = declare_parameter(acc_ns + "rough_velocity_rate", 0.9); + + planner_ = std::make_unique( + this, + vehicle_info, + stop_param, + slow_down_param, + acc_param); // Publishers path_pub_ = this->create_publisher("~/output/trajectory", 1); - stop_reason_diag_pub_ = - this->create_publisher("~/output/stop_reason", 1); // Subscribers obstacle_pointcloud_sub_ = this->create_subscription( @@ -150,847 +165,51 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() void ObstacleStopPlannerNode::obstaclePointcloudCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { - obstacle_ros_pointcloud_ptr_ = std::make_shared(); - pcl::VoxelGrid filter; - pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( - new pcl::PointCloud); - - pcl::fromROSMsg(*input_msg, *pointcloud_ptr); - - for (const auto & point : pointcloud_ptr->points) { - no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); - } - filter.setInputCloud(no_height_pointcloud_ptr); - filter.setLeafSize(0.05f, 0.05f, 100000.0f); - filter.filter(*no_height_filtered_pointcloud_ptr); - pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); - obstacle_ros_pointcloud_ptr_->header = input_msg->header; + planner_->obstaclePointcloudCallback(input_msg); + pointcloud_received_ = true; } + void ObstacleStopPlannerNode::pathCallback( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) { - if (!obstacle_ros_pointcloud_ptr_) { + if (!pointcloud_received_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for obstacle pointcloud..."); return; } - if (!current_velocity_ptr_ && enable_slow_down_) { + if (!current_velocity_reveived_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for current velocity..."); return; } - /* - * extend trajectory to consider obstacles after the goal - */ - autoware_planning_msgs::msg::Trajectory extended_trajectory; - extendTrajectory(*input_msg, extend_distance_, extended_trajectory); - - const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; - autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; - - /* - * trim trajectory from self pose - */ - geometry_msgs::msg::Pose self_pose; - getSelfPose(input_msg->header, tf_buffer_, self_pose); - autoware_planning_msgs::msg::Trajectory trim_trajectory; - size_t trajectory_trim_index; - trimTrajectoryWithIndexFromSelfPose(base_path, self_pose, trim_trajectory, trajectory_trim_index); - - /* - * decimate trajectory for calculation cost - */ - autoware_planning_msgs::msg::Trajectory decimate_trajectory; - std::map decimate_trajectory_index_map; - decimateTrajectory( - trim_trajectory, step_length_, decimate_trajectory, decimate_trajectory_index_map); - - autoware_planning_msgs::msg::Trajectory & trajectory = decimate_trajectory; - - /* - * search candidate obstacle pointcloud - */ - pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( - new pcl::PointCloud); - { - // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_.lookupTransform( - trajectory.header.frame_id, obstacle_ros_pointcloud_ptr_->header.frame_id, - obstacle_ros_pointcloud_ptr_->header.stamp, rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "[obstacle_stop_planner] Failed to look up transform from " << - trajectory.header.frame_id << " to " << obstacle_ros_pointcloud_ptr_->header.frame_id); - // do not publish path - return; - } - - Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl::PointCloud::Ptr obstacle_pointcloud_pcl_ptr_( - new pcl::PointCloud); - pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_pointcloud_pcl_ptr_); - pcl::PointCloud::Ptr transformed_obstacle_pointcloud_ptr( - new pcl::PointCloud); - pcl::transformPointCloud( - *obstacle_pointcloud_pcl_ptr_, *transformed_obstacle_pointcloud_ptr, - affine_matrix); - - // search obstacle candidate pointcloud to reduce calculation cost - const double search_radius = enable_slow_down_ ? slow_down_search_radius_ : stop_search_radius_; - searchPointcloudNearTrajectory( - trajectory, search_radius, transformed_obstacle_pointcloud_ptr, - obstacle_candidate_pointcloud_ptr); - obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; - } - - /* - * check collision, slow_down - */ - // for collision - bool is_collision = false; - size_t decimate_trajectory_collision_index; - pcl::PointXYZ nearest_collision_point; - rclcpp::Time nearest_collision_point_time; - // for slow down - bool candidate_slow_down = false; - bool is_slow_down = false; - size_t decimate_trajectory_slow_down_index; - pcl::PointXYZ nearest_slow_down_point; - pcl::PointXYZ lateral_nearest_slow_down_point; - pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); - double lateral_deviation = 0.0; - for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { - /* - * create one step circle center for vehicle - */ - const auto prev_center_pose = getVehicleCenterFromBase(trajectory.points.at(i).pose); - Point prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = getVehicleCenterFromBase(trajectory.points.at(i + 1).pose); - Point next_center_point(next_center_pose.position.x, next_center_pose.position.y); - /* - * create one step polygon for vehicle - */ - std::vector one_step_move_vehicle_polygon; - createOneStepPolygon( - trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, one_step_move_vehicle_polygon, - expand_stop_range_); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); - // convert boost polygon - Polygon boost_one_step_move_vehicle_polygon; - for (const auto & point : one_step_move_vehicle_polygon) { - boost_one_step_move_vehicle_polygon.outer().push_back(bg::make(point.x, point.y)); - } - boost_one_step_move_vehicle_polygon.outer().push_back( - bg::make( - one_step_move_vehicle_polygon.front().x, one_step_move_vehicle_polygon.front().y)); - - std::vector one_step_move_slow_down_range_polygon; - Polygon boost_one_step_move_slow_down_range_polygon; - if (enable_slow_down_) { - /* - * create one step polygon for slow_down range - */ - createOneStepPolygon( - trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - one_step_move_slow_down_range_polygon, expand_slow_down_range_); - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, - PolygonType::SlowDownRange); - // convert boost polygon - for (const auto & point : one_step_move_slow_down_range_polygon) { - boost_one_step_move_slow_down_range_polygon.outer().push_back( - bg::make(point.x, point.y)); - } - boost_one_step_move_slow_down_range_polygon.outer().push_back( - bg::make( - one_step_move_slow_down_range_polygon.front().x, - one_step_move_slow_down_range_polygon.front().y)); - } - - // check within polygon - pcl::PointCloud::Ptr collision_pointcloud_ptr( - new pcl::PointCloud); - collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - if (!is_slow_down && enable_slow_down_) { - for (size_t j = 0; j < obstacle_candidate_pointcloud_ptr->size(); ++j) { - Point point( - obstacle_candidate_pointcloud_ptr->at(j).x, obstacle_candidate_pointcloud_ptr->at(j).y); - if ( - bg::distance(prev_center_point, point) < slow_down_search_radius_ || - bg::distance(next_center_point, point) < slow_down_search_radius_) - { - if (bg::within(point, boost_one_step_move_slow_down_range_polygon)) { - slow_down_pointcloud_ptr->push_back(obstacle_candidate_pointcloud_ptr->at(j)); - candidate_slow_down = true; - } - } - } - } else { - slow_down_pointcloud_ptr = obstacle_candidate_pointcloud_ptr; - } - for (size_t j = 0; j < slow_down_pointcloud_ptr->size(); ++j) { - Point point(slow_down_pointcloud_ptr->at(j).x, slow_down_pointcloud_ptr->at(j).y); - if ( - bg::distance(prev_center_point, point) < stop_search_radius_ || - bg::distance(next_center_point, point) < stop_search_radius_) - { - if (bg::within(point, boost_one_step_move_vehicle_polygon)) { - collision_pointcloud_ptr->push_back(slow_down_pointcloud_ptr->at(j)); - is_collision = true; - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z, - PolygonType::Collision); - } - } - } - if (candidate_slow_down && !is_collision && !is_slow_down) { - is_slow_down = true; - decimate_trajectory_slow_down_index = i; - debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, - PolygonType::SlowDown); - getNearestPoint( - *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_slow_down_point, - &nearest_collision_point_time); - getLateralNearestPoint( - *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &lateral_nearest_slow_down_point, - &lateral_deviation); - debug_ptr_->pushObstaclePoint(nearest_slow_down_point, PointType::SlowDown); - } - - /* - * search nearest collision point by beginning of path - */ - if (is_collision) { - getNearestPoint( - *collision_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_collision_point, - &nearest_collision_point_time); - debug_ptr_->pushObstaclePoint(nearest_collision_point, PointType::Stop); - decimate_trajectory_collision_index = i; - break; - } - } - - /* - * insert max velocity and judge if there is a need to stop - */ - bool need_to_stop = is_collision; - if (is_collision) { - acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory, decimate_trajectory_collision_index, self_pose, nearest_collision_point, - nearest_collision_point_time, object_ptr_, current_velocity_ptr_, &need_to_stop, &output_msg); - } - - /* - * insert stop point - */ - if (need_to_stop) { - for (size_t i = decimate_trajectory_index_map.at(decimate_trajectory_collision_index) + - trajectory_trim_index; - i < base_path.points.size(); ++i) - { - Eigen::Vector2d trajectory_vec; - { - const double yaw = - getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); - trajectory_vec << std::cos(yaw), std::sin(yaw); - } - Eigen::Vector2d collision_point_vec; - collision_point_vec << nearest_collision_point.x - base_path.points.at(i).pose.position.x, - nearest_collision_point.y - base_path.points.at(i).pose.position.y; - - if ( - trajectory_vec.dot(collision_point_vec) < 0.0 || - (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) - { - const auto stop_point = - searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); - if (stop_point.index <= output_msg.points.size()) { - insertStopPoint(stop_point, base_path, output_msg, stop_reason_diag); - } - break; - } - } - } - - /* - * insert slow_down point - */ - if (is_slow_down) { - for (size_t i = decimate_trajectory_index_map.at(decimate_trajectory_slow_down_index); - i < base_path.points.size(); ++i) - { - Eigen::Vector2d trajectory_vec; - { - const double yaw = - getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); - trajectory_vec << std::cos(yaw), std::sin(yaw); - } - Eigen::Vector2d slow_down_point_vec; - slow_down_point_vec << nearest_slow_down_point.x - base_path.points.at(i).pose.position.x, - nearest_slow_down_point.y - base_path.points.at(i).pose.position.y; - - if ( - trajectory_vec.dot(slow_down_point_vec) < 0.0 || - (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(slow_down_point_vec))) - { - const double slow_down_target_vel = calcSlowDownTargetVel(lateral_deviation); - const auto slow_down_start_point = createSlowDownStartPoint( - i, slow_down_margin_, slow_down_target_vel, trajectory_vec, slow_down_point_vec, - base_path); - - if (slow_down_start_point.index <= output_msg.points.size()) { - insertSlowDownStartPoint(slow_down_start_point, base_path, output_msg); - insertSlowDownVelocity( - slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, - output_msg); - } - break; - } - } - } + const auto output_msg = planner_->pathCallback(input_msg, tf_buffer_); path_pub_->publish(output_msg); - stop_reason_diag_pub_->publish(stop_reason_diag); - debug_ptr_->publish(); } void ObstacleStopPlannerNode::externalExpandStopRangeCallback( const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg) { - expand_stop_range_ = input_msg->expand_stop_range; - stop_search_radius_ = - step_length_ + std::hypot(vehicle_width_ / 2.0 + expand_stop_range_, vehicle_length_ / 2.0); -} - -void ObstacleStopPlannerNode::insertStopPoint( - const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag) -{ - autoware_planning_msgs::msg::TrajectoryPoint stop_trajectory_point = - base_path.points.at(std::max(static_cast(stop_point.index) - 1, 0)); - stop_trajectory_point.pose.position.x = stop_point.point.x(); - stop_trajectory_point.pose.position.y = stop_point.point.y(); - stop_trajectory_point.twist.linear.x = 0.0; - output_path.points.insert(output_path.points.begin() + stop_point.index, stop_trajectory_point); - for (size_t j = stop_point.index; j < output_path.points.size(); ++j) { - output_path.points.at(j).twist.linear.x = 0.0; - } - stop_reason_diag = makeStopReasonDiag("obstacle", stop_trajectory_point.pose); - debug_ptr_->pushPose(stop_trajectory_point.pose, PoseType::Stop); -} - -StopPoint ObstacleStopPlannerNode::searchInsertPoint( - const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) -{ - const auto max_dist_stop_point = - createTargetPoint(idx, stop_margin_, trajectory_vec, collision_point_vec, base_path); - const auto min_dist_stop_point = createTargetPoint( - idx, min_behavior_stop_margin_, trajectory_vec, collision_point_vec, base_path); - - // check if stop point is already inserted by behavior planner - bool is_inserted_already_stop_point = false; - for (int j = max_dist_stop_point.index - 1; j < static_cast(idx); ++j) { - if (base_path.points.at(std::max(j, 0)).twist.linear.x == 0.0) { - is_inserted_already_stop_point = true; - break; - } - } - // insert stop point - StopPoint stop_point; - stop_point.index = - !is_inserted_already_stop_point ? max_dist_stop_point.index : min_dist_stop_point.index; - stop_point.point = - !is_inserted_already_stop_point ? max_dist_stop_point.point : min_dist_stop_point.point; - return stop_point; -} - -StopPoint ObstacleStopPlannerNode::createTargetPoint( - const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, - const Eigen::Vector2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path) -{ - double length_sum = 0.0; - length_sum += trajectory_vec.normalized().dot(collision_point_vec); - Eigen::Vector2d line_start_point, line_end_point; - { - line_start_point << base_path.points.at(0).pose.position.x, - base_path.points.at(0).pose.position.y; - const double yaw = getYawFromGeometryMsgsQuaternion(base_path.points.at(0).pose.orientation); - line_end_point << std::cos(yaw), std::sin(yaw); - } - - StopPoint stop_point{0, Eigen::Vector2d()}; - for (size_t j = idx; 0 < j; --j) { - line_start_point << base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y; - line_end_point << base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y; - if (margin < length_sum) { - stop_point.index = j; - break; - } - length_sum += (line_end_point - line_start_point).norm(); - } - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_start_point, length_sum - margin, stop_point.point); - - return stop_point; -} - -SlowDownPoint ObstacleStopPlannerNode::createSlowDownStartPoint( - const int idx, const double margin, const double slow_down_target_vel, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path) -{ - double length_sum = 0.0; - length_sum += trajectory_vec.normalized().dot(slow_down_point_vec); - Eigen::Vector2d line_start_point{}; - Eigen::Vector2d line_end_point{}; - SlowDownPoint slow_down_point{0, Eigen::Vector2d(), 0.0}; - for (size_t j = idx; 0 < j; --j) { - line_start_point << base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y; - line_end_point << base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y; - if (margin < length_sum) { - slow_down_point.index = j; - break; - } - length_sum += (line_end_point - line_start_point).norm(); - } - const double backward_length = length_sum - margin; - if (backward_length < 0) { - slow_down_point.index = 0; - slow_down_point.point = Eigen::Vector2d( - base_path.points.at(0).pose.position.x, base_path.points.at(0).pose.position.y); - } else { - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_start_point, backward_length, slow_down_point.point); - } - - slow_down_point.velocity = std::max( - std::sqrt(slow_down_target_vel * slow_down_target_vel + 2 * max_deceleration_ * length_sum), - current_velocity_ptr_->twist.linear.x); - return slow_down_point; -} - -void ObstacleStopPlannerNode::insertSlowDownStartPoint( - const SlowDownPoint & slow_down_start_point, - const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) -{ - autoware_planning_msgs::msg::TrajectoryPoint slow_down_start_trajectory_point = - base_path.points.at(std::max(static_cast(slow_down_start_point.index) - 1, 0)); - slow_down_start_trajectory_point.pose.position.x = slow_down_start_point.point.x(); - slow_down_start_trajectory_point.pose.position.y = slow_down_start_point.point.y(); - slow_down_start_trajectory_point.twist.linear.x = slow_down_start_point.velocity; - constexpr double epsilon = 0.001; - const auto & insert_target_point = output_path.points.at(slow_down_start_point.index); - if ( - autoware_utils::calcDistance2d(slow_down_start_trajectory_point, insert_target_point) > - epsilon) - { - output_path.points.insert( - output_path.points.begin() + slow_down_start_point.index, slow_down_start_trajectory_point); - } - debug_ptr_->pushPose(slow_down_start_trajectory_point.pose, PoseType::SlowDownStart); -} - -void ObstacleStopPlannerNode::insertSlowDownVelocity( - const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, - autoware_planning_msgs::msg::Trajectory & output_path) -{ - autoware_planning_msgs::msg::TrajectoryPoint slow_down_end_trajectory_point; - bool is_slow_down_end = false; - - for (size_t j = slow_down_start_point_idx; j < output_path.points.size() - 1; ++j) { - output_path.points.at(j).twist.linear.x = - std::min(slow_down_vel, output_path.points.at(j).twist.linear.x); - const auto dist = std::hypot( - output_path.points.at(j).pose.position.x - output_path.points.at(j + 1).pose.position.x, - output_path.points.at(j).pose.position.y - output_path.points.at(j + 1).pose.position.y); - slow_down_vel = std::max( - slow_down_target_vel, - std::sqrt(std::max(slow_down_vel * slow_down_vel - 2 * max_deceleration_ * dist, 0.0))); - if (!is_slow_down_end && slow_down_vel <= slow_down_target_vel) { - slow_down_end_trajectory_point = output_path.points.at(j + 1); - is_slow_down_end = true; - } - } - if (!is_slow_down_end) { - slow_down_end_trajectory_point = output_path.points.back(); - is_slow_down_end = true; - } - debug_ptr_->pushPose(slow_down_end_trajectory_point.pose, PoseType::SlowDownEnd); -} - -double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) -{ - return min_slow_down_vel_ + (max_slow_down_vel_ - min_slow_down_vel_) * - std::max(lateral_deviation - vehicle_width_ / 2, 0.0) / - expand_slow_down_range_; + planner_->externalExpandStopRangeCallback(input_msg); } void ObstacleStopPlannerNode::dynamicObjectCallback( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg) { - object_ptr_ = input_msg; + planner_->dynamicObjectCallback(input_msg); } void ObstacleStopPlannerNode::currentVelocityCallback( const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg) { - current_velocity_ptr_ = input_msg; -} - -bool ObstacleStopPlannerNode::decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - std::map index_map; - return decimateTrajectory(input_trajectory, step_length, output_trajectory, index_map); -} - -autoware_planning_msgs::msg::TrajectoryPoint ObstacleStopPlannerNode::getExtendTrajectoryPoint( - const double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) -{ - tf2::Transform map2goal; - tf2::fromMsg(goal_point.pose, map2goal); - tf2::Transform local_extend_point; - local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); - tf2::Quaternion q; - q.setRPY(0, 0, 0); - local_extend_point.setRotation(q); - const auto map2extend_point = map2goal * local_extend_point; - geometry_msgs::msg::Pose extend_pose; - tf2::toMsg(map2extend_point, extend_pose); - autoware_planning_msgs::msg::TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = extend_pose; - extend_trajectory_point.twist = goal_point.twist; - extend_trajectory_point.accel = goal_point.accel; - return extend_trajectory_point; -} - -bool ObstacleStopPlannerNode::extendTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const double extend_distance, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - output_trajectory = input_trajectory; - const auto goal_point = input_trajectory.points.back(); - double interpolation_distance = 0.1; - - double extend_sum = 0.0; - while (extend_sum <= (extend_distance - interpolation_distance)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); - output_trajectory.points.push_back(extend_trajectory_point); - extend_sum += interpolation_distance; - } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); - output_trajectory.points.push_back(extend_trajectory_point); - - return true; -} - -bool ObstacleStopPlannerNode::decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - std::map & index_map) -{ - output_trajectory.header = input_trajectory.header; - double trajectory_length_sum = 0.0; - double next_length = 0.0; - const double epsilon = 0.001; - Eigen::Vector2d point1, point2; - - for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { - if (next_length <= trajectory_length_sum + epsilon) { - Eigen::Vector2d line_start_point, line_end_point, interpolated_point; - line_start_point << input_trajectory.points.at(i).pose.position.x, - input_trajectory.points.at(i).pose.position.y; - line_end_point << input_trajectory.points.at(i + 1).pose.position.x, - input_trajectory.points.at(i + 1).pose.position.y; - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_end_point, - -1.0 * (trajectory_length_sum - next_length), interpolated_point); - autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; - trajectory_point = input_trajectory.points.at(i); - trajectory_point.pose.position.x = interpolated_point.x(); - trajectory_point.pose.position.y = interpolated_point.y(); - output_trajectory.points.push_back(trajectory_point); - index_map.insert(std::make_pair(output_trajectory.points.size() - 1, size_t(i))); - next_length += step_length; - continue; - } - const double x = input_trajectory.points.at(i).pose.position.x - - input_trajectory.points.at(i + 1).pose.position.x; - const double y = input_trajectory.points.at(i).pose.position.y - - input_trajectory.points.at(i + 1).pose.position.y; - const double distance = std::sqrt(x * x + y * y); - - trajectory_length_sum += distance; - } - if (!input_trajectory.points.empty()) { - output_trajectory.points.push_back(input_trajectory.points.back()); - index_map.insert( - std::make_pair(output_trajectory.points.size() - 1, input_trajectory.points.size() - 1)); - } - return true; -} - -bool ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory, size_t & index) -{ - double min_distance = 0.0; - size_t min_distance_index = 0; - bool is_init = false; - for (size_t i = 0; i < input_trajectory.points.size(); ++i) { - const double x = input_trajectory.points.at(i).pose.position.x - self_pose.position.x; - const double y = input_trajectory.points.at(i).pose.position.y - self_pose.position.y; - const double squared_distance = x * x + y * y; - if (!is_init || squared_distance < min_distance * min_distance) { - is_init = true; - min_distance = std::sqrt(squared_distance); - min_distance_index = i; - } - } - for (size_t i = min_distance_index; i < input_trajectory.points.size(); ++i) { - output_trajectory.points.push_back(input_trajectory.points.at(i)); - } - output_trajectory.header = input_trajectory.header; - index = min_distance_index; - return true; -} - -bool ObstacleStopPlannerNode::trimTrajectoryFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - size_t index; - return trimTrajectoryWithIndexFromSelfPose( - output_trajectory, self_pose, output_trajectory, index); -} - -bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius, - const pcl::PointCloud::Ptr input_pointcloud_ptr, - pcl::PointCloud::Ptr output_pointcloud_ptr) -{ - const double squared_radius = radius * radius; - for (const auto & trajectory_point : trajectory.points) { - const auto center_pose = getVehicleCenterFromBase(trajectory_point.pose); - for (const auto & point : input_pointcloud_ptr->points) { - const double x = center_pose.position.x - point.x; - const double y = center_pose.position.y - point.y; - const double squared_distance = x * x + y * y; - if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);} - } - } - return true; -} - -void ObstacleStopPlannerNode::createOneStepPolygon( - const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, - std::vector & polygon, const double expand_width) -{ - std::vector one_step_move_vehicle_corner_points; - // start step - { - double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); - } - // next step - { - double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); - } - convexHull(one_step_move_vehicle_corner_points, polygon); -} - -bool ObstacleStopPlannerNode::convexHull( - const std::vector pointcloud, std::vector & polygon_points) -{ - cv::Point2d centroid; - centroid.x = 0; - centroid.y = 0; - for (const auto & point : pointcloud) { - centroid.x += point.x; - centroid.y += point.y; - } - centroid.x = centroid.x / static_cast(pointcloud.size()); - centroid.y = centroid.y / static_cast(pointcloud.size()); - - std::vector normalized_pointcloud; - std::vector normalized_polygon_points; - for (size_t i = 0; i < pointcloud.size(); ++i) { - normalized_pointcloud.push_back( - cv::Point( - (pointcloud.at(i).x - centroid.x) * 1000.0, (pointcloud.at(i).y - centroid.y) * 1000.0)); - } - cv::convexHull(normalized_pointcloud, normalized_polygon_points); - - for (size_t i = 0; i < normalized_polygon_points.size(); ++i) { - cv::Point2d polygon_point; - polygon_point.x = (normalized_polygon_points.at(i).x / 1000.0 + centroid.x); - polygon_point.y = (normalized_polygon_points.at(i).y / 1000.0 + centroid.y); - polygon_points.push_back(polygon_point); - } - return true; -} - -bool ObstacleStopPlannerNode::getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, - geometry_msgs::msg::Pose & self_pose) -{ - try { - geometry_msgs::msg::TransformStamped transform; - transform = - tf_buffer.lookupTransform( - header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds( - 0.1)); - self_pose.position.x = transform.transform.translation.x; - self_pose.position.y = transform.transform.translation.y; - self_pose.position.z = transform.transform.translation.z; - self_pose.orientation.x = transform.transform.rotation.x; - self_pose.orientation.y = transform.transform.rotation.y; - self_pose.orientation.z = transform.transform.rotation.z; - self_pose.orientation.w = transform.transform.rotation.w; - return true; - } catch (tf2::TransformException & ex) { - return false; - } -} -bool ObstacleStopPlannerNode::getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) -{ - Eigen::Vector2d line_vec = line_point2 - line_point1; - Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); - output_point = base_point + backward_vec; - return true; -} - -void ObstacleStopPlannerNode::getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) -{ - double min_norm = 0.0; - bool is_init = false; - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); - Eigen::Vector2d base_pose_vec; - base_pose_vec << std::cos(yaw), std::sin(yaw); - - for (size_t i = 0; i < pointcloud.size(); ++i) { - Eigen::Vector2d pointcloud_vec; - pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, - pointcloud.at(i).y - base_pose.position.y; - double norm = base_pose_vec.dot(pointcloud_vec); - if (norm < min_norm || !is_init) { - min_norm = norm; - *nearest_collision_point = pointcloud.at(i); - *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; - is_init = true; - } - } -} - -void ObstacleStopPlannerNode::getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation) -{ - double min_norm = std::numeric_limits::max(); - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); - Eigen::Vector2d base_pose_vec; - base_pose_vec << std::cos(yaw), std::sin(yaw); - for (size_t i = 0; i < pointcloud.size(); ++i) { - Eigen::Vector2d pointcloud_vec; - pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, - pointcloud.at(i).y - base_pose.position.y; - double norm = - std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); - if (norm < min_norm) { - min_norm = norm; - *lateral_nearest_point = pointcloud.at(i); - } - } - *deviation = min_norm; -} - -geometry_msgs::msg::Pose ObstacleStopPlannerNode::getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose) -{ - geometry_msgs::msg::Pose center_pose; - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); - center_pose.position.x = - base_pose.position.x + (vehicle_length_ / 2.0 - rear_overhang_) * std::cos(yaw); - center_pose.position.y = - base_pose.position.y + (vehicle_length_ / 2.0 - rear_overhang_) * std::sin(yaw); - center_pose.position.z = base_pose.position.z; - center_pose.orientation = base_pose.orientation; - return center_pose; + planner_->currentVelocityCallback(input_msg); + current_velocity_reveived_ = true; } +} // namespace obstacle_stop_planner -} // namespace motion_planning +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + obstacle_stop_planner::ObstacleStopPlannerNode) diff --git a/obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp b/obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp new file mode 100644 index 00000000..22be0a06 --- /dev/null +++ b/obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp @@ -0,0 +1,496 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include "autoware_utils/geometry/geometry.hpp" +#include "pcl/filters/voxel_grid.h" +#include "tf2/utils.h" +#include "tf2_eigen/tf2_eigen.h" +#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 "obstacle_stop_planner/obstacle_stop_planner.hpp" +#include "obstacle_stop_planner/util.hpp" +#include "obstacle_stop_planner/one_step_polygon.hpp" +#include "obstacle_stop_planner/trajectory.hpp" +#include "vehicle_info_util/vehicle_info.hpp" + + +namespace obstacle_stop_planner +{ +ObstacleStopPlanner::ObstacleStopPlanner( + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const StopControlParameter & stop_param, + const SlowDownControlParameter & slow_down_param, + const AdaptiveCruiseControlParameter & acc_param) +: node_(node), + vehicle_info_(vehicle_info), + stop_param_(stop_param), + slow_down_param_(slow_down_param), + acc_param_(acc_param) +{ + debug_ptr_ = std::make_shared( + node_, + vehicle_info_.wheel_base + + vehicle_info_.front_overhang); + + // Initializer + acc_controller_ = std::make_unique( + node_, vehicle_info_, acc_param_); +} + +void ObstacleStopPlanner::obstaclePointcloudCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +{ + obstacle_pointcloud_ptr_ = updatePointCloud(input_msg); +} + +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::pathCallback( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg, + tf2_ros::Buffer & tf_buffer) +{ + /* + * extend trajectory to consider obstacles after the goal + */ + const auto extended_trajectory = extendTrajectory(*input_msg, stop_param_.extend_distance); + + const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; + autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; + + /* + * trim trajectory from self pose + */ + auto self_pose = getSelfPose(input_msg->header, tf_buffer); + autoware_planning_msgs::msg::Trajectory trim_trajectory; + size_t trajectory_trim_index = 0; + std::tie(trim_trajectory, trajectory_trim_index) = + trimTrajectoryWithIndexFromSelfPose(base_path, self_pose); + + /* + * decimate trajectory for calculation cost + */ + DecimateTrajectoryMap decimate_trajectory_map = decimateTrajectory( + trim_trajectory, stop_param_.step_length); + + autoware_planning_msgs::msg::Trajectory & trajectory = + decimate_trajectory_map.decimate_trajectory; + + /* + * search candidate obstacle pointcloud + */ + pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( + new pcl::PointCloud); + + // search obstacle candidate pointcloud to reduce calculation cost + const auto search_radius = slow_down_param_.enable_slow_down ? + slow_down_param_.slow_down_search_radius : stop_param_.stop_search_radius; + + obstacle_candidate_pointcloud_ptr = searchCandidateObstacle( + obstacle_pointcloud_ptr_, + tf_buffer, + trajectory, + search_radius, + vehicle_info_, + node_->get_logger()); + + /* + * check collision, slow_down + */ + // for collision + bool is_collision = false; + size_t decimate_trajectory_collision_index = 0; + Point3d nearest_collision_point; + rclcpp::Time nearest_collision_point_time; + // for slow down + bool candidate_slow_down = false; + bool is_slow_down = false; + size_t decimate_trajectory_slow_down_index = 0; + Point3d nearest_slow_down_point; + pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); + double lateral_deviation = 0.0; + + for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { + /* + * create one step circle center for vehicle + */ + const auto prev_center_pose = getVehicleCenterFromBase( + trajectory.points.at(i).pose, + vehicle_info_.vehicle_length, + vehicle_info_.rear_overhang); + const auto next_center_pose = getVehicleCenterFromBase( + trajectory.points.at(i + 1).pose, + vehicle_info_.vehicle_length, + vehicle_info_.rear_overhang); + + Point2d prev_center_point = autoware_utils::fromMsg(prev_center_pose.position).to_2d(); + Point2d next_center_point = autoware_utils::fromMsg(next_center_pose.position).to_2d(); + + /* + * create one step polygon for vehicle + */ + const auto move_vehicle_polygon = createOneStepPolygon( + trajectory.points.at(i).pose, + trajectory.points.at(i + 1).pose, + stop_param_.expand_stop_range, + vehicle_info_); + debug_ptr_->pushPolygon( + move_vehicle_polygon, + trajectory.points.at(i).pose.position.z, + PolygonType::Vehicle); + + Polygon2d move_slow_down_range_polygon; + if (slow_down_param_.enable_slow_down) { + /* + * create one step polygon for slow_down range + */ + move_slow_down_range_polygon = createOneStepPolygon( + trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + slow_down_param_.expand_slow_down_range, vehicle_info_); + debug_ptr_->pushPolygon( + move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, + PolygonType::SlowDownRange); + } + + // check within polygon + pcl::PointCloud::Ptr collision_pointcloud_ptr( + new pcl::PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + std::tie(candidate_slow_down, slow_down_pointcloud_ptr) = getSlowDownPointcloud( + is_slow_down, slow_down_param_.enable_slow_down, + obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, + slow_down_param_.slow_down_search_radius, + move_slow_down_range_polygon, slow_down_pointcloud_ptr, candidate_slow_down); + + std::tie(is_collision, collision_pointcloud_ptr) = getCollisionPointcloud( + slow_down_pointcloud_ptr, prev_center_point, next_center_point, + stop_param_.stop_search_radius, move_vehicle_polygon, + trajectory.points.at(i), collision_pointcloud_ptr, is_collision); + + if (candidate_slow_down && !is_collision && !is_slow_down) { + is_slow_down = true; + decimate_trajectory_slow_down_index = i; + debug_ptr_->pushPolygon( + move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, + PolygonType::SlowDown); + + const auto nearest_slow_down_pointstamped = PointHelper::getNearestPoint( + *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); + nearest_slow_down_point = nearest_slow_down_pointstamped.point; + nearest_collision_point_time = nearest_slow_down_pointstamped.time; + + const auto lateral_nearest_slow_down_point = PointHelper::getLateralNearestPoint( + *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); + lateral_deviation = lateral_nearest_slow_down_point.deviation; + debug_ptr_->pushObstaclePoint(nearest_slow_down_point, PointType::SlowDown); + } + + /* + * search nearest collision point by beginning of path + */ + if (is_collision) { + const auto nearest_collision_pointstamped = PointHelper::getNearestPoint( + *collision_pointcloud_ptr, trajectory.points.at(i).pose); + nearest_collision_point = nearest_collision_pointstamped.point; + nearest_collision_point_time = nearest_collision_pointstamped.time; + debug_ptr_->pushObstaclePoint(nearest_collision_point, PointType::Stop); + decimate_trajectory_collision_index = i; + break; + } + } + + /* + * insert max velocity and judge if there is a need to stop + */ + bool need_to_stop = is_collision; + if (is_collision) { + std::tie(need_to_stop, output_msg) = acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory_map.decimate_trajectory, + decimate_trajectory_collision_index, + self_pose, nearest_collision_point.to_2d(), + nearest_collision_point_time, object_ptr_, + current_velocity_ptr_, + output_msg); + } + + /* + * insert stop point + */ + if (need_to_stop) { + output_msg = insertStopPoint( + decimate_trajectory_map.index_map.at(decimate_trajectory_collision_index) + + trajectory_trim_index, + base_path, + nearest_collision_point.to_2d(), + output_msg); + } + + /* + * insert slow_down point + */ + if (is_slow_down) { + output_msg = insertSlowDownPoint( + decimate_trajectory_map.index_map.at(decimate_trajectory_slow_down_index), + base_path, + nearest_slow_down_point.to_2d(), + calcSlowDownTargetVel(lateral_deviation), + slow_down_param_.slow_down_margin, + output_msg); + } + debug_ptr_->publish(); + return output_msg; +} + +// collision +std::tuple::Ptr> +ObstacleStopPlanner::getCollisionPointcloud( + const pcl::PointCloud::Ptr slow_down_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, + const Polygon2d & one_step_polygon, + const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, + const pcl::PointCloud::Ptr collision_pointcloud, + const bool is_collision) +{ + auto output_pointcloud = collision_pointcloud; + auto output_collision = is_collision; + + for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { + Point2d point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); + if ( + boost::geometry::distance(prev_center_point, point) < search_radius || + boost::geometry::distance(next_center_point, point) < search_radius) + { + if (boost::geometry::within(point, one_step_polygon)) { + output_pointcloud->push_back(slow_down_pointcloud->at(j)); + output_collision = true; + debug_ptr_->pushPolygon( + one_step_polygon, trajectory_point.pose.position.z, + PolygonType::Collision); + } + } + } + return std::make_tuple(output_collision, output_pointcloud); +} + +// slow down +std::tuple::Ptr> +ObstacleStopPlanner::getSlowDownPointcloud( + const bool is_slow_down, + const bool enable_slow_down, + const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, + const Polygon2d & one_step_polygon, + const pcl::PointCloud::Ptr slow_down_pointcloud, + const bool candidate_slow_down) +{ + auto output_pointcloud = slow_down_pointcloud; + auto output_candidate = candidate_slow_down; + + if (!is_slow_down && enable_slow_down) { + for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { + Point2d point( + obstacle_candidate_pointcloud->at(j).x, obstacle_candidate_pointcloud->at(j).y); + if ( + boost::geometry::distance(prev_center_point, point) < search_radius || + boost::geometry::distance(next_center_point, point) < search_radius) + { + if (boost::geometry::within(point, one_step_polygon)) { + output_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); + output_candidate = true; + } + } + } + } else { + output_pointcloud = obstacle_candidate_pointcloud; + } + return std::make_tuple(output_candidate, output_pointcloud); +} + +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::insertSlowDownPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_slow_down_point, + const double slow_down_target_vel, const double slow_down_margin, + const autoware_planning_msgs::msg::Trajectory & input_msg) +{ + auto output_msg = input_msg; + PointHelper point_helper; + + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { + const double yaw = + getYawFromQuaternion(base_path.points.at(i).pose.orientation); + const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; + const Point2d slow_down_point_vec { + nearest_slow_down_point.x() - base_path.points.at(i).pose.position.x, + nearest_slow_down_point.y() - base_path.points.at(i).pose.position.y}; + + if ( + trajectory_vec.dot(slow_down_point_vec) < 0.0 || + (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(slow_down_point_vec))) + { + const auto slow_down_start_point = point_helper.createSlowDownStartPoint( + i, slow_down_margin, slow_down_target_vel, trajectory_vec, slow_down_point_vec, + base_path, current_velocity_ptr_->twist.linear.x, + slow_down_param_); + + if (slow_down_start_point.index <= output_msg.points.size()) { + autoware_planning_msgs::msg::TrajectoryPoint slowdown_trajectory_point; + std::tie(slowdown_trajectory_point, output_msg) = + PointHelper::insertSlowDownStartPoint( + slow_down_start_point, base_path, output_msg); + debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); + output_msg = insertSlowDownVelocity( + slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, + output_msg); + } + break; + } + } + return output_msg; +} + +// stop +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::insertStopPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_collision_point, + const autoware_planning_msgs::msg::Trajectory & input_msg) +{ + auto output_msg = input_msg; + PointHelper point_helper; + + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { + const double yaw = + getYawFromQuaternion(base_path.points.at(i).pose.orientation); + const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; + const Point2d collision_point_vec { + nearest_collision_point.x() - base_path.points.at(i).pose.position.x, + nearest_collision_point.y() - base_path.points.at(i).pose.position.y}; + + if ( + trajectory_vec.dot(collision_point_vec) < 0.0 || + (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) + { + const auto stop_point = + point_helper.searchInsertPoint( + i, base_path, trajectory_vec, collision_point_vec, stop_param_); + if (stop_point.index <= output_msg.points.size()) { + autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; + std::tie(trajectory_point, output_msg) = + PointHelper::insertStopPoint(stop_point, base_path, output_msg); + debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); + } + break; + } + } + return output_msg; +} + +void ObstacleStopPlanner::externalExpandStopRangeCallback( + const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg) +{ + stop_param_.expand_stop_range = input_msg->expand_stop_range; + stop_param_.stop_search_radius = + stop_param_.step_length + std::hypot( + vehicle_info_.vehicle_width / 2.0 + stop_param_.expand_stop_range, + vehicle_info_.vehicle_length / 2.0); +} + +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::insertSlowDownVelocity( + const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, + const autoware_planning_msgs::msg::Trajectory & input_path) +{ + autoware_planning_msgs::msg::TrajectoryPoint slow_down_end_trajectory_point; + auto output_path = input_path; + bool is_slow_down_end = false; + + for (size_t j = slow_down_start_point_idx; j < output_path.points.size() - 1; ++j) { + output_path.points.at(j).twist.linear.x = + std::min(slow_down_vel, output_path.points.at(j).twist.linear.x); + const auto dist = std::hypot( + output_path.points.at(j).pose.position.x - output_path.points.at(j + 1).pose.position.x, + output_path.points.at(j).pose.position.y - output_path.points.at(j + 1).pose.position.y); + slow_down_vel = std::max( + slow_down_target_vel, + std::sqrt( + std::max( + slow_down_vel * slow_down_vel - 2 * slow_down_param_.max_deceleration * dist, + 0.0))); + if (!is_slow_down_end && slow_down_vel <= slow_down_target_vel) { + slow_down_end_trajectory_point = output_path.points.at(j + 1); + is_slow_down_end = true; + } + } + if (!is_slow_down_end) { + slow_down_end_trajectory_point = output_path.points.back(); + } + debug_ptr_->pushPose(slow_down_end_trajectory_point.pose, PoseType::SlowDownEnd); + return output_path; +} + +double ObstacleStopPlanner::calcSlowDownTargetVel(const double lateral_deviation) const +{ + return slow_down_param_.min_slow_down_vel + + (slow_down_param_.max_slow_down_vel - slow_down_param_.min_slow_down_vel) * + std::max(lateral_deviation - vehicle_info_.vehicle_width / 2, 0.0) / + slow_down_param_.expand_slow_down_range; +} + +void ObstacleStopPlanner::dynamicObjectCallback( + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg) +{ + object_ptr_ = input_msg; +} + +void ObstacleStopPlanner::currentVelocityCallback( + const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg) +{ + current_velocity_ptr_ = input_msg; +} + +geometry_msgs::msg::Pose ObstacleStopPlanner::getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer) +{ + geometry_msgs::msg::Pose self_pose; + try { + geometry_msgs::msg::TransformStamped transform; + transform = + tf_buffer.lookupTransform( + header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds( + 0.1)); + self_pose = autoware_utils::transform2pose(transform.transform); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "could not get self pose from tf_buffer."); + } + return self_pose; +} +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp new file mode 100644 index 00000000..8d561196 --- /dev/null +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -0,0 +1,254 @@ +// Copyright 2021 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 +#include +#include + +#include "obstacle_stop_planner/point_helper.hpp" +#include "pcl_conversions/pcl_conversions.h" + +namespace obstacle_stop_planner +{ +Point3d pointXYZtoPoint3d(pcl::PointXYZ point) +{ + return Point3d{point.x, point.y, point.z}; +} + +Point2d PointHelper::getBackwardPointFromBasePoint( + const Point2d & line_point1, const Point2d & line_point2, + const Point2d & base_point, const double backward_length) +{ + const auto line_vec = line_point2 - line_point1; + const auto backward_vec = backward_length * line_vec.normalized(); + const auto output_point = base_point + backward_vec; + return Point2d{output_point.x(), output_point.y()}; +} + +PointStamped PointHelper::getNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose) +{ + const double yaw = getYawFromQuaternion(base_pose.orientation); + Point2d base_pose_vec {std::cos(yaw), std::sin(yaw)}; + + Point2d pointcloud_vec { + pointcloud.at(0).x - base_pose.position.x, + pointcloud.at(0).y - base_pose.position.y}; + double min_norm = base_pose_vec.dot(pointcloud_vec); + PointStamped nearest_collision_point; + nearest_collision_point.point = pointXYZtoPoint3d(pointcloud.at(0)); + nearest_collision_point.time = pcl_conversions::fromPCL(pointcloud.header).stamp; + + for (size_t i = 1; i < pointcloud.size(); ++i) { + Point2d pointcloud_vec { + pointcloud.at(i).x - base_pose.position.x, + pointcloud.at(i).y - base_pose.position.y}; + double norm = base_pose_vec.dot(pointcloud_vec); + + if (norm < min_norm) { + min_norm = norm; + nearest_collision_point.point = pointXYZtoPoint3d(pointcloud.at(i)); + nearest_collision_point.time = pcl_conversions::fromPCL(pointcloud.header).stamp; + } + } + return nearest_collision_point; +} + +PointDeviation PointHelper::getLateralNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose) +{ + double min_norm = std::numeric_limits::max(); + const double yaw = getYawFromQuaternion(base_pose.orientation); + Point2d base_pose_vec {std::cos(yaw), std::sin(yaw)}; + PointDeviation lateral_nearest_point; + + for (auto i : pointcloud) { + Point2d pointcloud_vec { + i.x - base_pose.position.x, + i.y - base_pose.position.y}; + + double norm = + std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); + if (norm < min_norm) { + min_norm = norm; + lateral_nearest_point.point = i; + } + } + lateral_nearest_point.deviation = min_norm; + return lateral_nearest_point; +} + +std::tuple +PointHelper::insertStopPoint( + const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, + const autoware_planning_msgs::msg::Trajectory & input_path) +{ + auto output_path = input_path; + autoware_planning_msgs::msg::TrajectoryPoint stop_trajectory_point = + base_path.points.at(std::max(static_cast(stop_point.index) - 1, 0)); + stop_trajectory_point.pose.position.x = stop_point.point.x(); + stop_trajectory_point.pose.position.y = stop_point.point.y(); + stop_trajectory_point.twist.linear.x = 0.0; + output_path.points.insert(output_path.points.begin() + stop_point.index, stop_trajectory_point); + for (size_t j = stop_point.index; j < output_path.points.size(); ++j) { + output_path.points.at(j).twist.linear.x = 0.0; + } + return std::make_tuple(stop_trajectory_point, output_path); +} + +StopPoint PointHelper::searchInsertPoint( + const size_t idx, const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & trajectory_vec, const Point2d & collision_point_vec, + const StopControlParameter & param) const +{ + const auto max_dist_stop_point = + createTargetPoint( + idx, param.stop_margin, trajectory_vec, collision_point_vec, + base_path); + const auto min_dist_stop_point = createTargetPoint( + idx, param.min_behavior_stop_margin, trajectory_vec, collision_point_vec, base_path); + + // check if stop point is already inserted by behavior planner + bool is_inserted_already_stop_point = false; + for (size_t j = max_dist_stop_point.index - 1; j < idx; ++j) { + if (base_path.points.at(std::max(j, size_t(0))).twist.linear.x == 0.0) { + is_inserted_already_stop_point = true; + break; + } + } + // insert stop point + StopPoint stop_point; + stop_point.index = + !is_inserted_already_stop_point ? max_dist_stop_point.index : min_dist_stop_point.index; + stop_point.point = + !is_inserted_already_stop_point ? max_dist_stop_point.point : min_dist_stop_point.point; + return stop_point; +} + +StopPoint PointHelper::createTargetPoint( + const size_t idx, const double margin, const Point2d & trajectory_vec, + const Point2d & collision_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path) +{ + double length_sum = 0.0; + length_sum += trajectory_vec.normalized().dot(collision_point_vec); + Point2d line_start_point = autoware_utils::fromMsg(base_path.points.at(0).pose.position).to_2d(); + const double yaw = getYawFromQuaternion(base_path.points.at(0).pose.orientation); + Point2d line_end_point {std::cos(yaw), std::sin(yaw)}; + + StopPoint stop_point{0, Point2d {0.0, 0.0}}; + for (size_t j = idx; 0 < j; --j) { + line_start_point = autoware_utils::fromMsg(base_path.points.at(j - 1).pose.position).to_2d(); + line_end_point = autoware_utils::fromMsg(base_path.points.at(j).pose.position).to_2d(); + if (margin < length_sum) { + stop_point.index = j; + break; + } + length_sum += (line_end_point - line_start_point).norm(); + } + stop_point.point = getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_start_point, length_sum - margin); + + return stop_point; +} + +SlowDownPoint PointHelper::createSlowDownStartPoint( + const int idx, const double margin, const double slow_down_target_vel, + const Point2d & trajectory_vec, const Point2d & slow_down_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path, + const double current_velocity_x, + const SlowDownControlParameter & param) const +{ + double length_sum = 0.0; + length_sum += trajectory_vec.normalized().dot(slow_down_point_vec); + Point2d line_start_point{}; + Point2d line_end_point{}; + + SlowDownPoint slow_down_point{0, Point2d {0.0, 0.0}, 0.0}; + for (size_t j = idx; 0 < j; --j) { + line_start_point = autoware_utils::fromMsg(base_path.points.at(j).pose.position).to_2d(); + line_end_point = autoware_utils::fromMsg(base_path.points.at(j - 1).pose.position).to_2d(); + if (margin < length_sum) { + slow_down_point.index = j; + break; + } + length_sum += (line_end_point - line_start_point).norm(); + } + const double backward_length = length_sum - margin; + if (backward_length < 0) { + slow_down_point.index = 0; + slow_down_point.point = Point2d( + base_path.points.at(0).pose.position.x, base_path.points.at(0).pose.position.y); + } else { + slow_down_point.point = getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_start_point, backward_length); + } + + slow_down_point.velocity = std::max( + std::sqrt( + slow_down_target_vel * slow_down_target_vel + 2 * param.max_deceleration * + length_sum), + current_velocity_x); + return slow_down_point; +} + +std::tuple +PointHelper::insertSlowDownStartPoint( + const SlowDownPoint & slow_down_start_point, + const autoware_planning_msgs::msg::Trajectory & base_path, + const autoware_planning_msgs::msg::Trajectory & input_path) +{ + auto output_path = input_path; + + autoware_planning_msgs::msg::TrajectoryPoint slow_down_start_trajectory_point = + base_path.points.at(std::max(static_cast(slow_down_start_point.index) - 1, 0)); + slow_down_start_trajectory_point.pose.position = + autoware_utils::toMsg(slow_down_start_point.point.to_3d()); + slow_down_start_trajectory_point.twist.linear.x = slow_down_start_point.velocity; + constexpr double epsilon = 0.001; + const auto & insert_target_point = output_path.points.at(slow_down_start_point.index); + if ( + autoware_utils::calcDistance2d(slow_down_start_trajectory_point, insert_target_point) > + epsilon) + { + output_path.points.insert( + output_path.points.begin() + slow_down_start_point.index, slow_down_start_trajectory_point); + } + return std::make_tuple(slow_down_start_trajectory_point, output_path); +} + +autoware_planning_msgs::msg::TrajectoryPoint PointHelper::getExtendTrajectoryPoint( + const double extend_distance, + const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + geometry_msgs::msg::Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + autoware_planning_msgs::msg::TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.twist = goal_point.twist; + extend_trajectory_point.accel = goal_point.accel; + return extend_trajectory_point; +} + +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp new file mode 100644 index 00000000..586da23f --- /dev/null +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -0,0 +1,120 @@ +// Copyright 2021 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 +#include +#include +#include "boost/geometry.hpp" +#include "obstacle_stop_planner/trajectory.hpp" +#include "obstacle_stop_planner/point_helper.hpp" +#include "obstacle_stop_planner/util.hpp" + +namespace obstacle_stop_planner +{ +autoware_planning_msgs::msg::Trajectory extendTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const double extend_distance) +{ + auto output_trajectory = input_trajectory; + const auto goal_point = input_trajectory.points.back(); + double interpolation_distance = 0.1; + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - interpolation_distance)) { + const auto extend_trajectory_point = + PointHelper::getExtendTrajectoryPoint(extend_sum, goal_point); + output_trajectory.points.push_back(extend_trajectory_point); + extend_sum += interpolation_distance; + } + const auto extend_trajectory_point = + PointHelper::getExtendTrajectoryPoint(extend_distance, goal_point); + output_trajectory.points.push_back(extend_trajectory_point); + + return output_trajectory; +} + +DecimateTrajectoryMap decimateTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length) +{ + DecimateTrajectoryMap output; + output.orig_trajectory = input_trajectory; + output.decimate_trajectory.header = input_trajectory.header; + double trajectory_length_sum = 0.0; + double next_length = 0.0; + const double epsilon = 0.001; + + for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { + if (next_length <= trajectory_length_sum + epsilon) { + const auto line_start_point = autoware_utils::fromMsg( + input_trajectory.points.at(i).pose.position).to_2d(); + const auto line_end_point = autoware_utils::fromMsg( + input_trajectory.points.at(i + 1).pose.position).to_2d(); + Point2d interpolated_point = PointHelper::getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_end_point, + -1.0 * (trajectory_length_sum - next_length)); + + autoware_planning_msgs::msg::TrajectoryPoint trajectory_point = input_trajectory.points.at(i); + trajectory_point.pose.position = autoware_utils::toMsg( + interpolated_point.to_3d(input_trajectory.points.at(i).pose.position.z)); + output.decimate_trajectory.points.emplace_back(trajectory_point); + + output.index_map.insert( + std::make_pair(output.decimate_trajectory.points.size() - 1, size_t(i))); + next_length += step_length; + continue; + } + + const auto p1 = autoware_utils::fromMsg(input_trajectory.points.at(i).pose.position); + const auto p2 = autoware_utils::fromMsg(input_trajectory.points.at(i + 1).pose.position); + trajectory_length_sum += boost::geometry::distance(p1.to_2d(), p2.to_2d()); + } + if (!input_trajectory.points.empty()) { + output.decimate_trajectory.points.emplace_back(input_trajectory.points.back()); + output.index_map.insert( + std::make_pair( + output.decimate_trajectory.points.size() - 1, + input_trajectory.points.size() - 1)); + } + return output; +} + +std::tuple +trimTrajectoryWithIndexFromSelfPose( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const geometry_msgs::msg::Pose & self_pose) +{ + autoware_planning_msgs::msg::Trajectory output_trajectory; + + size_t min_distance_index = 0; + const auto self_point = autoware_utils::fromMsg(self_pose.position); + const auto input_point = autoware_utils::fromMsg(input_trajectory.points.at(0).pose.position); + double min_distance = boost::geometry::distance(input_point.to_2d(), self_point.to_2d()); + + for (size_t i = 1; i < input_trajectory.points.size(); ++i) { + const auto p1 = autoware_utils::fromMsg(input_trajectory.points.at(i).pose.position); + const double point_distance = boost::geometry::distance(p1.to_2d(), self_point.to_2d()); + + if (point_distance < min_distance) { + min_distance = point_distance; + min_distance_index = i; + } + } + + for (size_t i = min_distance_index; i < input_trajectory.points.size(); ++i) { + output_trajectory.points.emplace_back(input_trajectory.points.at(i)); + } + output_trajectory.header = input_trajectory.header; + return std::forward_as_tuple(output_trajectory, min_distance_index); +} +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py new file mode 100644 index 00000000..78b1e129 --- /dev/null +++ b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py @@ -0,0 +1,61 @@ +# Copyright 2021 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. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +import os +import unittest + +from ament_index_python import get_package_share_directory +from launch.launch_description import LaunchDescription +from launch_ros.actions import Node +import launch_testing +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + obstacle_stop_planner_node = Node( + package='obstacle_stop_planner_refine', + executable='obstacle_stop_planner_node_exe', + namespace='test', + parameters=[ + os.path.join( + get_package_share_directory('obstacle_stop_planner_refine'), + 'config/obstacle_stop_planner.param.yaml'), + os.path.join( + get_package_share_directory('obstacle_stop_planner_refine'), + 'config/adaptive_cruise_control.param.yaml'), + os.path.join( + get_package_share_directory('obstacle_stop_planner_refine'), + 'config/test_vehicle_info.param.yaml') + ]) + + context = {'obstacle_stop_planner_node': obstacle_stop_planner_node} + + return LaunchDescription([ + obstacle_stop_planner_node, + # Start tests right away - no need to wait for anything + ReadyToTest(), + ]), context + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + + def test_exit_code(self, proc_output, proc_info, obstacle_stop_planner_node): + # Check that process exits with code -15 code: termination request, sent to the program + assertExitCodes(proc_info, [-15], process=obstacle_stop_planner_node) diff --git a/obstacle_stop_planner_refine/src/main.cpp b/obstacle_stop_planner_refine/test/src/gtest_main.cpp similarity index 65% rename from obstacle_stop_planner_refine/src/main.cpp rename to obstacle_stop_planner_refine/test/src/gtest_main.cpp index f002a4cd..e45b3785 100644 --- a/obstacle_stop_planner_refine/src/main.cpp +++ b/obstacle_stop_planner_refine/test/src/gtest_main.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2021 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. @@ -12,15 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "obstacle_stop_planner/node.hpp" - -int main(int argc, char ** argv) +int32_t main(int32_t argc, char ** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp b/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp new file mode 100644 index 00000000..29365791 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp @@ -0,0 +1,32 @@ +// Copyright 2021 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 "gtest/gtest.h" +#include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" + + +TEST(createVehicleFootprint, returnValue) { + VehicleInfo vehicle_info; + vehicle_info.front_overhang = 0.5; + vehicle_info.rear_overhang = 0.3; + vehicle_info.wheel_base = 2.0; + vehicle_info.wheel_tread = 0.1; + vehicle_info.left_overhang = 0.2; + vehicle_info.right_overhang = 0.2; + + const auto ring2d = obstacle_stop_planner::createVehicleFootprint(vehicle_info); + EXPECT_EQ(ring2d.size(), 5U); + EXPECT_EQ(ring2d.at(0).x(), 2.5); + EXPECT_EQ(ring2d.at(0).y(), 0.25); +} diff --git a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp new file mode 100644 index 00000000..4a6d1888 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp @@ -0,0 +1,110 @@ +// Copyright 2021 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 +#include +#include +#include "gtest/gtest.h" + +#include "obstacle_stop_planner/obstacle_stop_planner.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +using obstacle_stop_planner::StopControlParameter; +using obstacle_stop_planner::SlowDownControlParameter; +using obstacle_stop_planner::AdaptiveCruiseControlParameter; +using obstacle_stop_planner::ObstacleStopPlanner; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +VehicleInfo createVehicleParam() +{ + VehicleInfo i; + i.wheel_radius = 1.0F; + i.wheel_width = 1.0F; + i.wheel_base = 1.0F; + i.wheel_tread = 1.0F; + i.front_overhang = 1.0F; + i.rear_overhang = 1.0F; + i.left_overhang = 1.0F; + i.rear_overhang = 1.0F; + i.vehicle_height = 1.0F; + + i.vehicle_length = 1.0F; + i.vehicle_width = 1.0F; + i.min_longitudinal_offset = 1.0F; + i.max_longitudinal_offset = 2.0F; + i.min_lateral_offset = 1.0F; + i.max_lateral_offset = 2.0F; + i.min_height_offset = 1.0F; + i.max_height_offset = 2.0F; + return i; +} + +StopControlParameter createStopParam() +{ + StopControlParameter i; + i.stop_margin = 1.0F; + i.min_behavior_stop_margin = 1.0F; + i.step_length = 1.0F; + i.extend_distance = 1.0F; + i.expand_stop_range = 1.0F; + i.stop_search_radius = 1.0F; + return i; +} + +SlowDownControlParameter createSlowDownParam() +{ + SlowDownControlParameter i; + i.slow_down_margin = 1.0F; + i.expand_slow_down_range = 1.0F; + i.max_slow_down_vel = 2.0F; + i.min_slow_down_vel = 1.0F; + i.max_deceleration = 1.0F; + i.enable_slow_down = true; + i.slow_down_search_radius = 1.0F; + return i; +} + +AdaptiveCruiseControlParameter createAccParam() +{ + AdaptiveCruiseControlParameter i; + i.use_object_to_est_vel = true; + // add parameter + return i; +} + +// class ObstacleStopPlannerTest : public ::testing::Test +// { +// public: +// ObstacleStopPlannerTest() +// { +// const VehicleInfo vehicle_info = createVehicleParam(); + +// const StopControlParameter stop_param = createStopParam(); + +// const SlowDownControlParameter slow_down_param = createSlowDownParam(); + +// const AdaptiveCruiseControlParameter acc_param = createAccParam(); + +// rclcpp::Node dummy_node{"dummy_node"}; + +// planner_ = std::make_shared( +// &dummy_node, +// vehicle_info, +// stop_param, +// slow_down_param, +// acc_param); +// } +// std::shared_ptr planner_; +// }; diff --git a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp new file mode 100644 index 00000000..51afbd83 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp @@ -0,0 +1,183 @@ +// Copyright 2021 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 + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +using sensor_msgs::msg::PointCloud2; +using autoware_planning_msgs::msg::Trajectory; +using namespace std::chrono_literals; + +void init_pcl_msg( + sensor_msgs::msg::PointCloud2 & msg, + const std::string & frame_id, + const std::size_t size) +{ + msg.height = 1U; + msg.is_bigendian = false; + msg.is_dense = false; + msg.header.frame_id = frame_id; + // set the fields + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2Fields( + 4U, + "x", 1U, sensor_msgs::msg::PointField::FLOAT32, + "y", 1U, sensor_msgs::msg::PointField::FLOAT32, + "z", 1U, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1U, sensor_msgs::msg::PointField::FLOAT32); + // allocate memory so that iterators can be used + modifier.resize(size); +} + +Trajectory convertPointsToTrajectoryWithYaw( + const std::vector & points) +{ + Trajectory trajectory; + for (size_t i = 0; i < points.size(); i++) { + autoware_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose.position = autoware_utils::toMsg(points[i]); + double yaw = 0; + if (i > 0) { + const double dx = points[i].x() - points[i - 1].x(); + const double dy = points[i].y() - points[i - 1].y(); + yaw = std::atan2(dy, dx); + } else if (i == 0 && points.size() > 1) { + const double dx = points[i + 1].x() - points[i].x(); + const double dy = points[i + 1].y() - points[i].y(); + yaw = std::atan2(dy, dx); + } + const double roll = 0; + const double pitch = 0; + tf2::Quaternion quaternion; + quaternion.setRPY(roll, pitch, yaw); + traj_point.pose.orientation = tf2::toMsg(quaternion); + trajectory.points.push_back(traj_point); + } + return trajectory; +} +class ObstacleStopPlannerNodeTest : public ::testing::Test +{ +public: + void SetUp() override + { + ASSERT_FALSE(rclcpp::ok()); + rclcpp::init(0, nullptr); + ASSERT_TRUE(rclcpp::ok()); + + fake_node_ = std::make_shared("fake_node"); + + using PubAllocT = rclcpp::PublisherOptionsWithAllocator>; + dummy_pointcloud_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/pointcloud", rclcpp::QoS{1}, PubAllocT{}); + dummy_path_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/trajectory", rclcpp::QoS{1}, PubAllocT{}); + dummy_velocity_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/twist", rclcpp::QoS{1}, PubAllocT{}); + dummy_object_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/objects", rclcpp::QoS{1}, PubAllocT{}); + path_sub_ = + fake_node_->create_subscription( + "/obstacle_stop_planner/output/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::SharedPtr msg) + {this->path_msg_ = *msg;}); + + rclcpp::NodeOptions node_options{}; + + node_options.append_parameter_override("wheel_radius", 0.39F); + node_options.append_parameter_override("wheel_width", 0.42F); + node_options.append_parameter_override("wheel_base", 2.74F); + node_options.append_parameter_override("wheel_tread", 1.63F); + node_options.append_parameter_override("front_overhang", 1.0F); + node_options.append_parameter_override("rear_overhang", 1.03F); + node_options.append_parameter_override("left_overhang", 0.1F); + node_options.append_parameter_override("right_overhang", 0.1F); + node_options.append_parameter_override("vehicle_height", 2.5F); + + node_options.append_parameter_override( + "adaptive_cruise_control.use_object_to_estimate_vel", false); + + planner_ = std::make_shared(node_options); + } + + std::shared_ptr planner_; + rclcpp::Node::SharedPtr fake_node_{nullptr}; + rclcpp::Publisher::SharedPtr dummy_pointcloud_pub_; + rclcpp::Publisher::SharedPtr dummy_path_pub_; + rclcpp::Publisher::SharedPtr dummy_velocity_pub_; + rclcpp::Publisher::SharedPtr dummy_object_pub_; + rclcpp::Subscription::SharedPtr path_sub_; + boost::optional path_msg_; +}; + +TEST_F(ObstacleStopPlannerNodeTest, plan_simple_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(fake_node_); + executor.add_node(planner_); + + // publish point cloud + const uint32_t max_cloud_size = 10; + + sensor_msgs::msg::PointCloud2 test_msg; + init_pcl_msg(test_msg, "base_link", max_cloud_size); + test_msg.header.stamp = fake_node_->now(); + dummy_pointcloud_pub_->publish(test_msg); + + // current velocity + geometry_msgs::msg::TwistStamped twist_msg; + twist_msg.twist.angular.x = 0.0F; + dummy_velocity_pub_->publish(twist_msg); + + executor.spin_some(); + + // create trajectory + std::vector points; + points.emplace_back(0.0, 0.0, 0.0); + points.emplace_back(1.0, 0.0, 0.0); + points.emplace_back(2.0, 0.0, 0.0); + points.emplace_back(3.0, 0.0, 0.0); + points.emplace_back(4.0, 0.0, 0.0); + points.emplace_back(5.0, 0.0, 0.0); + auto trajectory = convertPointsToTrajectoryWithYaw(points); + trajectory.header.frame_id = "base_link"; + trajectory.header.stamp = fake_node_->now(); + dummy_path_pub_->publish(trajectory); + + using namespace std::chrono_literals; + rclcpp::WallRate rate(100ms); + while (rclcpp::ok()) { + executor.spin_some(); + if (path_msg_.has_value()) { + break; + } + rate.sleep(); + } + + // Check data + for (size_t i = 1; i < path_msg_.get().points.size(); i++) { + ASSERT_EQ(trajectory.points[i - 1], path_msg_.get().points[i]); + } +} diff --git a/obstacle_stop_planner_refine/test/src/test_point_helper.cpp b/obstacle_stop_planner_refine/test/src/test_point_helper.cpp new file mode 100644 index 00000000..77baee0f --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_point_helper.cpp @@ -0,0 +1,46 @@ +// Copyright 2021 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 "gtest/gtest.h" +#include "obstacle_stop_planner/point_helper.hpp" + + +TEST(getBackwardPointFromBasePoint, returnValue) { + Point2d line1{1.0, 1.0}; + Point2d line2{1.0, -1.0}; + Point2d base_point{0.0, 0.0}; + double backward_length = 1.0; + auto ret = obstacle_stop_planner::PointHelper::getBackwardPointFromBasePoint( + line1, line2, base_point, backward_length); + + Point2d expect = Point2d{0.0, -1.0}; + EXPECT_EQ(expect, ret); +} + +TEST(getNearestPoint, returnValue) { + geometry_msgs::msg::Pose base_pose; + pcl::PointCloud pc; + pcl::PointXYZ point; + point.x = 10.0; + point.y = 10.0; + point.z = 10.0; + pc.points.push_back(point); + point.x = 20.0; + point.y = 20.0; + point.z = 20.0; + pc.points.push_back(point); + auto ret = obstacle_stop_planner::PointHelper::getNearestPoint(pc, base_pose); + EXPECT_EQ(10.0, ret.point.x()); + EXPECT_EQ(10.0, ret.point.y()); +} diff --git a/obstacle_stop_planner_refine/test/src/test_util.cpp b/obstacle_stop_planner_refine/test/src/test_util.cpp new file mode 100644 index 00000000..ab504268 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_util.cpp @@ -0,0 +1,22 @@ +// Copyright 2021 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 "gtest/gtest.h" +#include "obstacle_stop_planner/util.hpp" + + +TEST(getYawFromQuaternion, returnValue) { + geometry_msgs::msg::Quaternion quat; + EXPECT_EQ(0.0, getYawFromQuaternion(quat)); +} diff --git a/obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test b/obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test new file mode 100644 index 00000000..c4ed7808 --- /dev/null +++ b/obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test @@ -0,0 +1,5 @@ + + + + +