diff --git a/.github/ISSUE_TEMPLATE/bug.yaml b/.github/ISSUE_TEMPLATE/bug.yaml new file mode 100644 index 0000000000000..12a857998a0b2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug.yaml @@ -0,0 +1,71 @@ +name: Bug +description: Report a bug +body: + - type: checkboxes + attributes: + label: Checklist + description: Confirm the following items before proceeding. If one cannot be satisfied, create a discussion thread instead. + options: + - label: I've read the [contribution guidelines](https://github.com/autowarefoundation/autoware/blob/main/CONTRIBUTING.md). + required: true + - label: I've searched other issues and no duplicate issues were found. + required: true + - label: I'm convinced that this is not my fault but a bug. + required: true + + - type: textarea + attributes: + label: Description + description: Write a brief description of the bug. + validations: + required: true + + - type: textarea + attributes: + label: Expected behavior + description: Describe the expected behavior. + validations: + required: true + + - type: textarea + attributes: + label: Actual behavior + description: Describe the actual behavior. + validations: + required: true + + - type: textarea + attributes: + label: Steps to reproduce + description: Write the steps to reproduce the bug. + placeholder: |- + 1. + 2. + 3. + validations: + required: true + + - type: textarea + attributes: + label: Versions + description: Provide the version information. You can omit this if you believe it's irrelevant. + placeholder: |- + - OS: + - ROS 2: + - Autoware: + validations: + required: false + + - type: textarea + attributes: + label: Possible causes + description: Write the possible causes if you have any ideas. + validations: + required: false + + - type: textarea + attributes: + label: Additional context + description: Add any other additional context if it exists. + validations: + required: false diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 0000000000000..48765c24a7b25 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,13 @@ +blank_issues_enabled: false +contact_links: + - name: Question + url: https://github.com/autowarefoundation/autoware/discussions/new?category=q-a + about: Ask a question + + - name: Feature request + url: https://github.com/autowarefoundation/autoware/discussions/new?category=feature-requests + about: Send a feature request + + - name: Idea + url: https://github.com/autowarefoundation/autoware/discussions/new?category=ideas + about: Post an idea diff --git a/.github/ISSUE_TEMPLATE/task.yaml b/.github/ISSUE_TEMPLATE/task.yaml new file mode 100644 index 0000000000000..cd8322f507405 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/task.yaml @@ -0,0 +1,42 @@ +name: Task +description: Plan a task +body: + - type: checkboxes + attributes: + label: Checklist + description: Confirm the following items before proceeding. If one cannot be satisfied, create a discussion thread instead. + options: + - label: I've read the [contribution guidelines](https://github.com/autowarefoundation/autoware/blob/main/CONTRIBUTING.md). + required: true + - label: I've searched other issues and no duplicate issues were found. + required: true + - label: I've agreed with the maintainers that I can plan this task. + required: true + + - type: textarea + attributes: + label: Description + description: Write a brief description of the task. + validations: + required: true + + - type: textarea + attributes: + label: Purpose + description: Describe the purpose of the task. + validations: + required: true + + - type: textarea + attributes: + label: Possible approaches + description: Describe possible approaches for the task. + validations: + required: true + + - type: textarea + attributes: + label: Definition of done + description: Write the definition of done for the task. + validations: + required: true diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 42c750dd7d811..14776dc9f032c 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,49 +1,6 @@ -## Related Issue(required) +**Note**: Confirm our [contribution guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/) before submitting a pull request. - +Click the `Preview` tab and select a PR template: -## Description(required) - - - -## Review Procedure(required) - - - -## Related PR(optional) - - - -## Pre-Review Checklist for the PR Author - -**PR Author should check the checkboxes below when creating the PR.** - -- [ ] Read [commit-guidelines][commit-guidelines] -- [ ] Assign PR to reviewer - -If you are adding new package following items are required: - -- [ ] Documentation with description of the package is available -- [ ] A sample launch file and parameter file are available if the package contains executable nodes - -## Checklist for the PR Reviewer - -**Reviewers should check the checkboxes below before approval.** - -- [ ] Commits are properly organized and messages are according to the guideline -- [ ] PR title describes the changes - -## Post-Review Checklist for the PR Author - -**PR Author should check the checkboxes below before merging.** - -- [ ] All open points are addressed and tracked via issues or tickets - -## CI Checks - -- **Build and test for PR / build-and-test-pr**: Required to pass before the merge. -- **Build and test for PR / clang-tidy-pr**: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines]. -- **Check spelling**: NOT required to pass before the merge. It is up to the reviewer(s). See [here][spell-check-dict] if you want to add some words to the spell check dictionary. - -[commit-guidelines]: https://www.conventionalcommits.org/en/v1.0.0/ -[spell-check-dict]: https://github.com/tier4/autoware-spell-check-dict#how-to-contribute +- [Standard change](?expand=1&template=standard-change.md) +- [Small change](?expand=1&template=small-change.md) diff --git a/.github/PULL_REQUEST_TEMPLATE/small-change.md b/.github/PULL_REQUEST_TEMPLATE/small-change.md new file mode 100644 index 0000000000000..f8ef7e75d2cfc --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/small-change.md @@ -0,0 +1,27 @@ +## Description + + + +## Pre-review checklist for the PR author + +PR author **must** check the checkboxes below when creating the PR. + +- [ ] I've confirmed the [contribution guidelines]. +- [ ] The PR follows the [pull request guidelines]. + +## In-review checklist for the PR reviewers + +Reviewers **must** check the checkboxes below before approval. + +- [ ] The PR follows the [pull request guidelines]. + +## Post-review checklist for the PR author + +PR author **must** check the checkboxes below before merging. + +- [ ] There are no open discussions or they are tracked via tickets. + +After all checkboxes are checked, anyone who has the write access can merge the PR. + +[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/ +[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/ diff --git a/.github/PULL_REQUEST_TEMPLATE/standard-change.md b/.github/PULL_REQUEST_TEMPLATE/standard-change.md new file mode 100644 index 0000000000000..467a2fc7c780a --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/standard-change.md @@ -0,0 +1,42 @@ +## Description + + + +## Related links + + + +## Tests performed + + + +## Notes for reviewers + + + +## Pre-review checklist for the PR author + +PR author **must** check the checkboxes below when creating the PR. + +- [ ] I've confirmed the [contribution guidelines]. +- [ ] The PR follows the [pull request guidelines]. + +## In-review checklist for the PR reviewers + +Reviewers **must** check the checkboxes below before approval. + +- [ ] The PR follows the [pull request guidelines]. +- [ ] The PR has been properly tested. +- [ ] The PR has been reviewed by the code owners. + +## Post-review checklist for the PR author + +PR author **must** check the checkboxes below before merging. + +- [ ] There are no open discussions or they are tracked via tickets. +- [ ] The PR is ready for merge. + +After all checkboxes are checked, anyone who has the write access can merge the PR. + +[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/ +[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/ diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 614496c01d6cc..e5cd1f601bfed 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -4,6 +4,12 @@ - source: CONTRIBUTING.md - source: DISCLAIMER.md - source: LICENSE + - source: .github/ISSUE_TEMPLATE/bug.yaml + - source: .github/ISSUE_TEMPLATE/config.yml + - source: .github/ISSUE_TEMPLATE/task.yaml + - source: .github/PULL_REQUEST_TEMPLATE.md + - source: .github/PULL_REQUEST_TEMPLATE/small-change.md + - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md - source: .github/dependabot.yaml - source: .github/workflows/pre-commit.yaml - source: .github/workflows/pre-commit-optional.yaml diff --git a/common/tier4_perception_rviz_plugin/CMakeLists.txt b/common/tier4_perception_rviz_plugin/CMakeLists.txt index a262c0bf0840f..2bac295514315 100644 --- a/common/tier4_perception_rviz_plugin/CMakeLists.txt +++ b/common/tier4_perception_rviz_plugin/CMakeLists.txt @@ -18,10 +18,10 @@ set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) -add_definitions(-DQT_NO_KEYWORDS) ## Declare a C++ library ament_auto_add_library(tier4_perception_rviz_plugin SHARED + src/tools/util.cpp src/tools/pedestrian_pose.cpp src/tools/car_pose.cpp src/tools/unknown_pose.cpp diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp index 85b91aeb64b41..cac7e6ac72f7e 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp @@ -47,25 +47,188 @@ #include "pedestrian_pose.hpp" -#include -#include -#include - -#include +#include "util.hpp" -#include -#include +#include #include +#include #include #include namespace rviz_plugins { +InteractivePedestrian::InteractivePedestrian(const Ogre::Vector3 & point) +{ + velocity_ = Ogre::Vector3::ZERO; + point_ = point; + theta_ = 0.0; + + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid_.begin(), uuid_.end(), bit_eng); +} + +std::array InteractivePedestrian::uuid() const { return uuid_; } + +void InteractivePedestrian::twist(geometry_msgs::msg::Twist & twist) const +{ + twist.linear.x = velocity_.length(); + twist.linear.y = 0.0; + twist.linear.z = 0.0; +} + +void InteractivePedestrian::transform(tf2::Transform & tf_map2object) const +{ + tf2::Transform tf_object_origin2moved_object; + tf2::Transform tf_map2object_origin; + + { + geometry_msgs::msg::Transform ros_object_origin2moved_object; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta_); + ros_object_origin2moved_object.rotation = tf2::toMsg(quat); + tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object); + } + + { + geometry_msgs::msg::Transform ros_object_map2object_origin; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, 0.0); + ros_object_map2object_origin.rotation = tf2::toMsg(quat); + ros_object_map2object_origin.translation.x = point_.x; + ros_object_map2object_origin.translation.y = point_.y; + ros_object_map2object_origin.translation.z = point_.z; + tf2::fromMsg(ros_object_map2object_origin, tf_map2object_origin); + } + + tf_map2object = tf_map2object_origin * tf_object_origin2moved_object; +} + +void InteractivePedestrian::update(const Ogre::Vector3 & point) +{ + velocity_ = point - point_; + point_ = point; + theta_ = + (velocity_.x < 1.0e-3 && velocity_.y < 1.0e-3) ? theta_ : std::atan2(velocity_.y, velocity_.x); +} + +double InteractivePedestrian::distance(const Ogre::Vector3 & point) +{ + return point_.distance(point); +} + +InteractivePedestrianCollection::InteractivePedestrianCollection() { target_ = nullptr; } + +void InteractivePedestrianCollection::reset() { target_ = nullptr; } + +void InteractivePedestrianCollection::select(const Ogre::Vector3 & point) +{ + const size_t index = nearest(point); + if (index != objects_.size()) { + target_ = objects_[index].get(); + } +} + +boost::optional> InteractivePedestrianCollection::create( + const Ogre::Vector3 & point) +{ + objects_.emplace_back(std::make_unique(point)); + target_ = objects_.back().get(); + + if (target_) { + return target_->uuid(); + } + + return {}; +} + +boost::optional> InteractivePedestrianCollection::remove( + const Ogre::Vector3 & point) +{ + const size_t index = nearest(point); + if (index != objects_.size()) { + const auto removed_uuid = objects_[index].get()->uuid(); + objects_.erase(objects_.begin() + index); + return removed_uuid; + } + + return {}; +} + +boost::optional> InteractivePedestrianCollection::update( + const Ogre::Vector3 & point) +{ + if (target_) { + target_->update(point); + return target_->uuid(); + } + + return {}; +} + +boost::optional InteractivePedestrianCollection::twist( + const std::array & uuid) const +{ + for (const auto & object : objects_) { + if (object->uuid() != uuid) { + continue; + } + + geometry_msgs::msg::Twist ret; + object->twist(ret); + return ret; + } + + return {}; +} + +boost::optional InteractivePedestrianCollection::transform( + const std::array & uuid) const +{ + for (const auto & object : objects_) { + if (object->uuid() != uuid) { + continue; + } + + tf2::Transform ret; + object->transform(ret); + return ret; + } + + return {}; +} + +size_t InteractivePedestrianCollection::nearest(const Ogre::Vector3 & point) +{ + const size_t npos = objects_.size(); + if (objects_.empty()) { + return npos; + } + + std::vector distances; + for (const auto & object : objects_) { + distances.push_back(object->distance(point)); + } + + const auto compare = [&](int x, int y) { return distances[x] < distances[y]; }; + std::vector indices(distances.size()); + std::iota(indices.begin(), indices.end(), 0); + std::sort(indices.begin(), indices.end(), compare); + + const size_t index = indices[0]; + return distances[index] < 2.0 ? index : npos; +} PedestrianInitialPoseTool::PedestrianInitialPoseTool() { shortcut_key_ = 'l'; + enable_interactive_property_ = new rviz_common::properties::BoolProperty( + "Interactive", false, "Enable/Disable interactive action by manipulating mouse.", + getPropertyContainer()); + property_frame_ = new rviz_common::properties::TfFrameProperty( + "Target Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING, + "The TF frame where the point cloud is output.", getPropertyContainer(), nullptr, true); topic_property_ = new rviz_common::properties::StringProperty( "Pose Topic", "/simulation/dummy_perception_publisher/object_info", "The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()), @@ -103,10 +266,16 @@ void PedestrianInitialPoseTool::updateTopic() dummy_object_info_pub_ = raw_node->create_publisher( topic_property_->getStdString(), 1); clock_ = raw_node->get_clock(); + move_tool_.initialize(context_); + property_frame_->setFrameManager(context_->getFrameManager()); } void PedestrianInitialPoseTool::onPoseSet(double x, double y, double theta) { + if (enable_interactive_property_->getBool()) { + return; + } + dummy_perception_publisher::msg::Object output_msg; std::string fixed_frame = context_->getFixedFrame().toStdString(); @@ -165,6 +334,108 @@ void PedestrianInitialPoseTool::onPoseSet(double x, double y, double theta) dummy_object_info_pub_->publish(output_msg); } +void PedestrianInitialPoseTool::publishObjectMsg( + const std::array & uuid, const uint32_t action) +{ + Object output_msg; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + output_msg.header.frame_id = fixed_frame; + output_msg.header.stamp = clock_->now(); + + // action + output_msg.action = action; + + // id + output_msg.id.uuid = uuid; + + if (action == Object::DELETE) { + dummy_object_info_pub_->publish(output_msg); + return; + } + + const auto object_tf = objects_.transform(uuid); + const auto object_twist = objects_.twist(uuid); + + if (!object_tf || !object_twist) { + return; + } + + // semantic + output_msg.classification.label = ObjectClassification::PEDESTRIAN; + output_msg.classification.probability = 1.0; + + // shape + output_msg.shape.type = Shape::CYLINDER; + const double width = 0.8; + const double length = 0.8; + output_msg.shape.dimensions.x = length; + output_msg.shape.dimensions.y = width; + output_msg.shape.dimensions.z = 2.0; + + // initial state + // pose + tf2::toMsg(object_tf.get(), output_msg.initial_state.pose_covariance.pose); + output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[0] = + std_dev_x_->getFloat() * std_dev_x_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[7] = + std_dev_y_->getFloat() * std_dev_y_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[14] = + std_dev_z_->getFloat() * std_dev_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[35] = + std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + // twist + output_msg.initial_state.twist_covariance.twist = object_twist.get(); + + dummy_object_info_pub_->publish(output_msg); +} + +int PedestrianInitialPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (!enable_interactive_property_->getBool()) { + return PoseTool::processMouseEvent(event); + } + + if (event.rightDown()) { + const auto point = get_point_from_mouse(event); + if (point) { + if (event.shift()) { + const auto uuid = objects_.create(point.value()); + publishObjectMsg(uuid.get(), Object::ADD); + } else if (event.alt()) { + const auto uuid = objects_.remove(point.value()); + publishObjectMsg(uuid.get(), Object::DELETE); + } else { + objects_.select(point.value()); + } + } + return 0; + } + + if (event.rightUp()) { + objects_.reset(); + return 0; + } + + if (event.right()) { + const auto point = get_point_from_mouse(event); + if (point) { + const auto uuid = objects_.update(point.value()); + publishObjectMsg(uuid.get(), Object::MODIFY); + } + return 0; + } + + return move_tool_.processMouseEvent(event); +} + +int PedestrianInitialPoseTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + PoseTool::processKeyEvent(event, panel); + return move_tool_.processKeyEvent(event, panel); +} } // end namespace rviz_plugins #include diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp index c463dbceeefd1..f7375eb734297 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -51,15 +51,71 @@ #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 #include #include +#include #include #include +#include +#include +#include #include #endif #include +#include + +#include +#include + +#include +#include +#include + namespace rviz_plugins { + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::Shape; +using dummy_perception_publisher::msg::Object; + +class InteractivePedestrian +{ +public: + explicit InteractivePedestrian(const Ogre::Vector3 & point); + ~InteractivePedestrian() {} + + std::array uuid() const; + void twist(geometry_msgs::msg::Twist & twist) const; + void transform(tf2::Transform & tf_map2object) const; + void update(const Ogre::Vector3 & point); + double distance(const Ogre::Vector3 & point); + +private: + std::array uuid_; + Ogre::Vector3 point_; + Ogre::Vector3 velocity_; + double theta_; +}; + +class InteractivePedestrianCollection +{ +public: + InteractivePedestrianCollection(); + ~InteractivePedestrianCollection() {} + + void reset(); + void select(const Ogre::Vector3 & point); + boost::optional> create(const Ogre::Vector3 & point); + boost::optional> remove(const Ogre::Vector3 & point); + boost::optional> update(const Ogre::Vector3 & point); + boost::optional twist(const std::array & uuid) const; + boost::optional transform(const std::array & uuid) const; + +private: + size_t nearest(const Ogre::Vector3 & point); + InteractivePedestrian * target_; + std::vector> objects_; +}; class PedestrianInitialPoseTool : public rviz_default_plugins::tools::PoseTool { Q_OBJECT @@ -68,6 +124,8 @@ class PedestrianInitialPoseTool : public rviz_default_plugins::tools::PoseTool PedestrianInitialPoseTool(); virtual ~PedestrianInitialPoseTool() {} virtual void onInitialize(); + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) override; protected: virtual void onPoseSet(double x, double y, double theta); @@ -79,6 +137,9 @@ private Q_SLOTS: rclcpp::Clock::SharedPtr clock_; rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + rviz_default_plugins::tools::MoveTool move_tool_; + + rviz_common::properties::BoolProperty * enable_interactive_property_; rviz_common::properties::StringProperty * topic_property_; rviz_common::properties::FloatProperty * std_dev_x_; rviz_common::properties::FloatProperty * std_dev_y_; @@ -86,6 +147,11 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * std_dev_theta_; rviz_common::properties::FloatProperty * position_z_; rviz_common::properties::FloatProperty * velocity_; + rviz_common::properties::TfFrameProperty * property_frame_; + + InteractivePedestrianCollection objects_; + + void publishObjectMsg(const std::array & uuid, const uint32_t action); }; } // namespace rviz_plugins diff --git a/common/tier4_perception_rviz_plugin/src/tools/util.cpp b/common/tier4_perception_rviz_plugin/src/tools/util.cpp new file mode 100644 index 0000000000000..50b68c2e85fd8 --- /dev/null +++ b/common/tier4_perception_rviz_plugin/src/tools/util.cpp @@ -0,0 +1,37 @@ +// Copyright 2022 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 "util.hpp" + +#include +#include + +#include +#include +#include + +std::optional get_point_from_mouse(rviz_common::ViewportMouseEvent & event) +{ + using rviz_rendering::RenderWindowOgreAdapter; + const auto viewport = RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow()); + const auto w = viewport->getActualWidth(); + const auto h = viewport->getActualHeight(); + const auto x = static_cast(event.x) / static_cast(w); + const auto y = static_cast(event.y) / static_cast(h); + + const auto plane = Ogre::Plane(Ogre::Vector3::UNIT_Z, 0.0); + const auto ray = viewport->getCamera()->getCameraToViewportRay(x, y); + const auto intersect = ray.intersects(plane); + return intersect.first ? std::optional(ray.getPoint(intersect.second)) : std::nullopt; +} diff --git a/planning/behavior_velocity_planner/test/src/test_geometry.cpp b/common/tier4_perception_rviz_plugin/src/tools/util.hpp similarity index 65% rename from planning/behavior_velocity_planner/test/src/test_geometry.cpp rename to common/tier4_perception_rviz_plugin/src/tools/util.hpp index 0d7abf4bf7271..2126af001aeb5 100644 --- a/planning/behavior_velocity_planner/test/src/test_geometry.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/util.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2022 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,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils.hpp" +#ifndef TOOLS__UTIL_HPP_ +#define TOOLS__UTIL_HPP_ -#include +#include -#include +#include -#include +#include + +std::optional get_point_from_mouse(rviz_common::ViewportMouseEvent & event); + +#endif // TOOLS__UTIL_HPP_ diff --git a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 393bb949d048d..6e7ac3e9efe81 100644 --- a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -1,52 +1,64 @@ /**: ros__parameters: can_assign_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN - 1, 1, 1, 1, 0, 0, 0, 0, #CAR - 1, 1, 1, 1, 0, 0, 0, 0, #TRUCK - 1, 1, 1, 1, 0, 0, 0, 0, #BUS - 0, 0, 0, 0, 1, 1, 1, 0, #BICYCLE - 0, 0, 0, 0, 1, 1, 1, 0, #MOTORBIKE - 0, 0, 0, 0, 1, 1, 1, 0, #PEDESTRIAN - 0, 0, 0, 0, 0, 0, 0, 1] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 1, 1, 1, 1, 1, 0, 0, 0, #CAR + 1, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 1, 1, 1, 1, 1, 0, 0, 0, #BUS + 1, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + max_dist_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN - 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #CAR - 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #TRUCK - 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #BUS - 3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #BICYCLE - 3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #MOTORBIKE - 2.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #PEDESTRIAN - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN max_area_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN - 12.10, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #CAR - 19.75, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #TRUCK - 32.40, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #BUS - 2.50, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #BICYCLE - 3.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #MOTORBIKE - 2.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00, 10000.00, #PEDESTRIAN - 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN + 12.10, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #CAR + 19.75, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #TRUCK + 32.40, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #BUS + 32.40, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #TRAILER + 3.00, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN min_area_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN - 3.600, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #CAR - 6.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #TRUCK - 10.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #BUS - 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #BICYCLE - 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #MOTORBIKE - 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #PEDESTRIAN - 0.500, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR + 6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN max_rad_matrix: # If value is greater than pi, it will be ignored. - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN - 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #CAR - 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #TRUCK - 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #BUS - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #PEDESTRIAN - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + min_iou_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index 8150af7436212..40b64b7d87012 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -1,90 +1,173 @@ /**: ros__parameters: - # trajectory total/fixing length - trajectory_length: 300 # total trajectory length[m] - forward_fixing_distance: 5.0 # forward fixing length from base_link[m] - backward_fixing_distance: 3.0 # backward fixing length from base_link[m] - - # clearance(distance) when generating trajectory - clearance_from_road: 0.2 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road - - # clearance for unique points - clearance_for_straight_line: 0.05 # minimum optimizing range around straight points - clearance_for_joint: 0.1 # minimum optimizing range around joint points - clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing - range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending - clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line - - # avoiding param - max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects[m/s] - max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects[m/s] - center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, center line width around path points used for judging that object is required to avoid or not - acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range - - # solving quadratic programming - qp_max_iteration: 10000 # max iteration when solving QP - qp_eps_abs: 1.0e-8 # eps abs when solving OSQP - qp_eps_rel: 1.0e-11 # eps rel when solving OSQP - qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending - qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending - qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing - qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing - - # constrain space - is_getting_constraints_close2path_points: true # generate trajectory closer to path points - max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate - coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction - coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction - keep_space_shape_x: 0.2 # keep space for x direction from base_link[m] - keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true, keep space for y direction from base_link[m] - max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m] - - is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid - enable_avoidance: true # enable avoidance function - is_using_vehicle_config: true # use vehicle config - num_sampling_points: 100 # number of optimizing points - num_joint_buffer_points: 3 # number of joint buffer points - num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - num_fix_points_for_extending: 50 # number of fixing points when extending - delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m] - delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point[m] - delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point - delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + option: + # publish + is_publishing_debug_visualization_marker: true + is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid + is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid + is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid + + is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area + + # show + is_showing_debug_info: false + is_showing_calculation_time: false + + # other + enable_avoidance: false # enable avoidance function + enable_pre_smoothing: true # enable EB + skip_optimization: false # skip MPT and EB + reset_prev_optimization: false + + common: + # sampling + num_sampling_points: 100 # number of optimizing points + + # trajectory total/fixing length + trajectory_length: 300.0 # total trajectory length[m] + + forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] + forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] + + backward_fixing_distance: 5.0 # backward fixing length from base_link [m] + delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] + + delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] + delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point + delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + + num_fix_points_for_extending: 50 # number of fixing points when extending + max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] + + object: # avoiding object + max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] + max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] + + avoiding_object_type: + unknown: true + car: true + truck: true + bus: true + bicycle: true + motorbike: true + pedestrian: true + animal: true # mpt param - # vehicle param for mpt - max_steer_deg: 40.0 # max steering angle [deg] - steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] - - # trajectory param for mpt - num_curvature_sampling_points: 5 # number of sampling points when calculating curvature - delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m] - forward_fixing_mpt_distance: 10 # number of fixing points around ego vehicle - - # optimization param for mpt - is_hard_fixing_terminal_point: false # hard fixing terminal point - base_point_weight: 2000.0 # slack weight for lateral error around base_link - top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point - mid_point_weight: 1000.0 # slack weight for lateral error around the middle point - lat_error_weight: 10.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - steer_input_weight: 0.01 # weight for steering input - steer_rate_weight: 1.0 # weight for steering rate - steer_acc_weight: 0.000001 # weight for steering acceleration - terminal_lat_error_weight: 0.0 # weight for lateral error at terminal point - terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point - terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point - terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point - zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero + mpt: + option: + steer_limit_constraint: true + fix_points_around_ego: true + # plan_from_ego: false + visualize_sampling_num: 1 + enable_manual_warm_start: true + enable_warm_start: true # false + + common: + num_curvature_sampling_points: 5 # number of sampling points when calculating curvature + delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] + + kinematics: + max_steer_deg: 40.0 # max steering angle [deg] + + # If this parameter is commented out, the parameter is set as below by default. + # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8` + # The 0.8 scale is adopted as it performed the best. + # optimization_center_offset: 2.3 # optimization center offset from base link # replanning & trimming trajectory param outside algorithm - min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m] - min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second] - max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m] - distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change + replan: + max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] + max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] + max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] + + # advanced parameters to improve performance as much as possible + advanced: + option: + # check if planned trajectory is outside drivable area + drivability_check: + # true: vehicle shape is considered as a set of circles + # false: vehicle shape is considered as footprint (= rectangle) + use_vehicle_circles: false + + # parameters only when use_vehicle_circles is true + vehicle_circles: + use_manual_vehicle_circles: false + num_for_constraints: 4 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.9 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 + + eb: + common: + num_joint_buffer_points: 3 # number of joint buffer points + num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx + delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. + num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points + + clearance: + clearance_for_straight_line: 0.05 # minimum optimizing range around straight points + clearance_for_joint: 0.1 # minimum optimizing range around joint points + clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing + + qp: + max_iteration: 10000 # max iteration when solving QP + eps_abs: 1.0e-8 # eps abs when solving OSQP + eps_rel: 1.0e-10 # eps rel when solving OSQP + + mpt: + clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory + hard_clearance_from_road: 0.0 # clearance from road boundary[m] + soft_clearance_from_road: 0.1 # clearance from road boundary[m] + soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] + clearance_from_object: 1.0 # clearance from object[m] + extra_desired_clearance_from_road: 0.0 # extra desired clearance from road + + weight: + soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point + soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point + + lat_error_weight: 100.0 # weight for lateral error + yaw_error_weight: 0.0 # weight for yaw error + yaw_error_rate_weight: 0.0 # weight for yaw error rate + steer_input_weight: 10.0 # weight for steering input + steer_rate_weight: 10.0 # weight for steering rate + + obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error + obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error + obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error + near_objects_length: 30.0 # weight for yaw error + + terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point + terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point + terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point + terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point + + # check if planned trajectory is outside drivable area + collision_free_constraints: + option: + l_inf_norm: true + soft_constraint: true + hard_constraint: false + # two_step_soft_constraint: false + + vehicle_circles: + use_manual_vehicle_circles: false + num_for_constraints: 3 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.8 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index e729845a4ba7c..5f02f35bab1a0 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -45,7 +45,7 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi Input pose source with measurement covariance matrix. -- measured_twist_with_covariance (geometry_msgs/PoseWithCovarianceStamped) +- measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) Input twist source with measurement covariance matrix. diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt new file mode 100644 index 0000000000000..546b7442fb12b --- /dev/null +++ b/perception/detected_object_validation/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.5) +project(detected_object_validation) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# Ignore -Wnonportable-include-path in Clang for mussp +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-nonportable-include-path) +endif() + +### Find Packages +find_package(ament_cmake_auto REQUIRED) + +### Find OpenCV Dependencies +find_package(OpenCV REQUIRED) + +### Find Eigen Dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +### Find dependencies listed in the package.xml +ament_auto_find_build_dependencies() + +include_directories( + include + SYSTEM + ${OpenCV_INCLUDE_DIRS} +) + +# Generate exe file +set(OCCUPANCY_GRID_BASED_VALIDATOR_SRC + src/occupancy_grid_based_validator.cpp +) + +ament_auto_add_library(occupancy_grid_based_validator SHARED + ${OCCUPANCY_GRID_BASED_VALIDATOR_SRC} +) + +target_link_libraries(occupancy_grid_based_validator + ${OpenCV_LIBRARIES} + Eigen3::Eigen +) + +rclcpp_components_register_node(occupancy_grid_based_validator + PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator" + EXECUTABLE occupancy_grid_based_validator_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/perception/detected_object_validation/README.md b/perception/detected_object_validation/README.md new file mode 100644 index 0000000000000..2bb1f9d3a4e8d --- /dev/null +++ b/perception/detected_object_validation/README.md @@ -0,0 +1,50 @@ +# detected_object_validation (occupancy grid based validator) + +## Purpose + +The purpose of this package is to eliminate obvious false positives of DetectedObjects. + +## Inner-workings / Algorithms + +Compare the occupancy grid map with the DetectedObject, and if a larger percentage of obstacles are in freespace, delete them. + +![debug sample image](image/debug_image.png) + +Basically, it takes an occupancy grid map as input and generates a binary image of freespace or other. + +A mask image is generated for each DetectedObject and the average value (percentage) in the mask image is calculated. +If the percentage is low, it is deleted. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------- | ----------------------------------------------------- | ----------------------------------------------------------- | +| `~/input/detected_objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | DetectedObjects | +| `~/input/occupancy_grid_map` | `nav_msgs::msg::OccupancyGrid` | OccupancyGrid with no time series calculation is preferred. | + +### Output + +| Name | Type | Description | +| ------------------ | ----------------------------------------------------- | ------------------------- | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | validated DetectedObjects | + +## Parameters + +| Name | Type | Description | +| ---------------- | ----- | -------------------------------------------------- | +| `mean_threshold` | float | The percentage threshold of allowed non-freespace. | +| `enable_debug` | bool | Whether to display debug images or not? | + +## Assumptions / Known limits + +Currently, only vehicle represented as BoundingBox are supported. + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/detected_object_validation/image/debug_image.png b/perception/detected_object_validation/image/debug_image.png new file mode 100644 index 0000000000000..7ac379a243d17 Binary files /dev/null and b/perception/detected_object_validation/image/debug_image.png differ diff --git a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp new file mode 100644 index 0000000000000..5602b5ee2eb27 --- /dev/null +++ b/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -0,0 +1,74 @@ +// Copyright 2022 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 OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#define OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace occupancy_grid_based_validator +{ +class OccupancyGridBasedValidator : public rclcpp::Node +{ +public: + explicit OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Publisher::SharedPtr objects_pub_; + message_filters::Subscriber objects_sub_; + message_filters::Subscriber occ_grid_sub_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + typedef message_filters::sync_policies::ApproximateTime< + autoware_auto_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + float mean_threshold_; + bool enable_debug_; + + void onObjectsAndOccGrid( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & input_occ_grid); + + cv::Mat fromOccupancyGrid(const nav_msgs::msg::OccupancyGrid & occupancy_grid); + std::optional getMask( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const autoware_auto_perception_msgs::msg::DetectedObject & object); + std::optional getMask( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const autoware_auto_perception_msgs::msg::DetectedObject & object, cv::Mat mask); + void toPolygon2d( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + std::vector & vertices); + void showDebugImage( + const nav_msgs::msg::OccupancyGrid & ros_occ_grid, + const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid); +}; +} // namespace occupancy_grid_based_validator + +#endif // OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml new file mode 100644 index 0000000000000..542f7a5c4463f --- /dev/null +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml new file mode 100644 index 0000000000000..a29206a0d7489 --- /dev/null +++ b/perception/detected_object_validation/package.xml @@ -0,0 +1,29 @@ + + + detected_object_validation + 1.0.0 + The ROS2 detected_object_validation package + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + eigen3_cmake_module + + autoware_auto_perception_msgs + message_filters + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp new file mode 100644 index 0000000000000..bc7788ee2af32 --- /dev/null +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -0,0 +1,258 @@ +// Copyright 2022 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 "occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" + +#include + +#include + +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace +{ +boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + /*target*/ target_frame_id, /*src*/ source_frame_id, time, + rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); + return boost::none; + } +} + +bool transformDetectedObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_auto_perception_msgs::msg::DetectedObjects & output_msg) +{ + output_msg = input_msg; + + /* transform to world coordinate */ + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (size_t i = 0; i < output_msg.objects.size(); ++i) { + tf2::fromMsg( + output_msg.objects.at(i).kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, output_msg.objects.at(i).kinematics.pose_with_covariance.pose); + // Note: Covariance is not transformed. + } + } + return true; +} + +} // namespace + +namespace occupancy_grid_based_validator +{ +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +using Shape = autoware_auto_perception_msgs::msg::Shape; + +OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("occupancy_grid_based_validator", node_options), + objects_sub_(this, "~/input/detected_objects", rclcpp::QoS{1}.get_rmw_qos_profile()), + occ_grid_sub_(this, "~/input/occupancy_grid_map", rclcpp::QoS{1}.get_rmw_qos_profile()), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + sync_(SyncPolicy(10), objects_sub_, occ_grid_sub_) +{ + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback( + std::bind(&OccupancyGridBasedValidator::onObjectsAndOccGrid, this, _1, _2)); + objects_pub_ = create_publisher( + "~/output/objects", rclcpp::QoS{1}); + + mean_threshold_ = declare_parameter("mean_threshold", 0.6); + enable_debug_ = declare_parameter("enable_debug", false); +} + +void OccupancyGridBasedValidator::onObjectsAndOccGrid( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & input_occ_grid) +{ + autoware_auto_perception_msgs::msg::DetectedObjects output; + output.header = input_objects->header; + + // Transform to occ grid frame + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!transformDetectedObjects( + *input_objects, input_occ_grid->header.frame_id, tf_buffer_, transformed_objects)) + return; + + // Convert ros data type to cv::Mat + cv::Mat occ_grid = fromOccupancyGrid(*input_occ_grid); + + // Get vehicle mask image and calculate mean within mask. + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + const auto & transformed_object = transformed_objects.objects.at(i); + const auto & object = input_objects->objects.at(i); + const auto & label = object.classification.front().label; + const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || + Label::TRAILER == label; + if (is_vehicle) { + auto mask = getMask(*input_occ_grid, transformed_object); + const float mean = mask ? cv::mean(occ_grid, mask.value())[0] * 0.01 : 1.0; + if (mean_threshold_ < mean) output.objects.push_back(object); + } else { + output.objects.push_back(object); + } + } + + objects_pub_->publish(output); + + if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid); +} + +std::optional OccupancyGridBasedValidator::getMask( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + cv::Mat mask = cv::Mat::zeros(occupancy_grid.info.height, occupancy_grid.info.width, CV_8UC1); + return getMask(occupancy_grid, object, mask); +} + +std::optional OccupancyGridBasedValidator::getMask( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const autoware_auto_perception_msgs::msg::DetectedObject & object, cv::Mat mask) +{ + const auto & resolution = occupancy_grid.info.resolution; + const auto & origin = occupancy_grid.info.origin; + std::vector vertices; + std::vector pixel_vertices; + toPolygon2d(object, vertices); + + bool is_polygon_within_image = true; + for (const auto & vertex : vertices) { + const float px = (vertex.x - origin.position.x) / resolution; + const float py = (vertex.y - origin.position.y) / resolution; + const bool is_point_within_image = (0 <= px && px < mask.cols && 0 <= py && py < mask.rows); + + if (!is_point_within_image) is_polygon_within_image = false; + + pixel_vertices.push_back(cv::Point2f(px, py)); + } + + if (is_polygon_within_image && !pixel_vertices.empty()) { + cv::fillConvexPoly(mask, pixel_vertices, cv::Scalar(255)); + return mask; + } else { + return std::nullopt; + } +} + +cv::Mat OccupancyGridBasedValidator::fromOccupancyGrid( + const nav_msgs::msg::OccupancyGrid & occupancy_grid) +{ + cv::Mat cv_occ_grid = + cv::Mat::zeros(occupancy_grid.info.height, occupancy_grid.info.width, CV_8UC1); + for (size_t i = 0; i < occupancy_grid.data.size(); ++i) { + size_t y = i / occupancy_grid.info.width; + size_t x = i % occupancy_grid.info.width; + const auto & data = occupancy_grid.data[i]; + cv_occ_grid.at(y, x) = + std::min(std::max(data, static_cast(0)), static_cast(50)) * 2; + } + return cv_occ_grid; +} + +void OccupancyGridBasedValidator::toPolygon2d( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + std::vector & vertices) +{ + if (object.shape.type == Shape::BOUNDING_BOX) { + const auto & pose = object.kinematics.pose_with_covariance.pose; + double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + Eigen::Matrix2d rotation; + rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + Eigen::Vector2d offset0, offset1, offset2, offset3; + offset0 = rotation * + Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + offset1 = rotation * + Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + offset2 = rotation * + Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + offset3 = rotation * + Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + vertices.push_back(cv::Point2f(pose.position.x + offset0.x(), pose.position.y + offset0.y())); + vertices.push_back(cv::Point2f(pose.position.x + offset1.x(), pose.position.y + offset1.y())); + vertices.push_back(cv::Point2f(pose.position.x + offset2.x(), pose.position.y + offset2.y())); + vertices.push_back(cv::Point2f(pose.position.x + offset3.x(), pose.position.y + offset3.y())); + } else if (object.shape.type == Shape::CYLINDER) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5, "CYLINDER type is not supported"); + } else if (object.shape.type == Shape::POLYGON) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5, "POLYGON type is not supported"); + } +} + +void OccupancyGridBasedValidator::showDebugImage( + const nav_msgs::msg::OccupancyGrid & ros_occ_grid, + const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid) +{ + cv::namedWindow("removed_objects_image", cv::WINDOW_NORMAL); + cv::namedWindow("passed_objects_image", cv::WINDOW_NORMAL); + cv::Mat removed_objects_image = occ_grid.clone(); + cv::Mat passed_objects_image = occ_grid.clone(); + + // Get vehicle mask image and calculate mean within mask. + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto & label = object.classification.front().label; + const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || + Label::TRAILER == label; + if (is_vehicle) { + auto mask = getMask(ros_occ_grid, object); + const float mean = mask ? cv::mean(occ_grid, mask.value())[0] * 0.01 : 1.0; + if (mean_threshold_ < mean) { + auto mask = getMask(ros_occ_grid, object, passed_objects_image); + if (mask) passed_objects_image = mask.value(); + } else { + auto mask = getMask(ros_occ_grid, object, removed_objects_image); + if (mask) removed_objects_image = mask.value(); + } + } + } + cv::imshow("removed_objects_image", removed_objects_image); + cv::imshow("passed_objects_image", passed_objects_image); + cv::waitKey(2); +} + +} // namespace occupancy_grid_based_validator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_based_validator::OccupancyGridBasedValidator) diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml index 0c969ba0077af..1c4dc51cc7084 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/detection_by_tracker/package.xml @@ -17,6 +17,7 @@ rclcpp_components shape_estimation tf2 + tf2_geometry_msgs tf2_ros tier4_autoware_utils tier4_perception_msgs diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 86de43c839ce6..1e290e43a6330 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -6,6 +6,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() @@ -16,12 +17,13 @@ ament_auto_find_build_dependencies() find_package(Eigen3 REQUIRED) ament_auto_add_library(map_based_prediction_node SHARED - src/map_based_prediction_ros.cpp - src/map_based_prediction.cpp + src/map_based_prediction_node.cpp + src/path_generator.cpp + src/debug.cpp ) rclcpp_components_register_node(map_based_prediction_node - PLUGIN "map_based_prediction::MapBasedPredictionROS" + PLUGIN "map_based_prediction::MapBasedPredictionNode" EXECUTABLE map_based_prediction ) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/Readme.md similarity index 94% rename from perception/map_based_prediction/README.md rename to perception/map_based_prediction/Readme.md index ee43ee89c81b1..4b436bf9c1e00 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/Readme.md @@ -50,6 +50,7 @@ | Parameter | Type | Description | | ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | +| `enable_delay_compensation` | bool | flag to enable the time delay compensation for the position of the object | | `prediction_time_horizon` | double | predict time duration for predicted path [s] | | `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | | `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | @@ -57,6 +58,7 @@ | `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | | `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | | `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | +| `object_buffer_time_length` | double | Time span of object history to store the information [s] | | `history_time_length` | double | Time span of object information used for prediction [s] | | `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | | `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml new file mode 100644 index 0000000000000..ef67fa7b00aed --- /dev/null +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + enable_delay_compensation: true + prediction_time_horizon: 10.0 #[s] + prediction_sampling_delta_time: 0.5 #[s] + min_velocity_for_map_based_prediction: 1.0 #[m/s] + dist_threshold_for_searching_lanelet: 3.0 #[m] + delta_yaw_threshold_for_searching_lanelet: 0.785 #[rad] + sigma_lateral_offset: 0.5 #[m] + sigma_yaw_angle_deg: 5.0 #[angle degree] + object_buffer_time_length: 2.0 #[s] + history_time_length: 1.0 #[s] + dist_ratio_threshold_to_left_bound: -0.5 #[ratio] + dist_ratio_threshold_to_right_bound: 0.5 #[ratio + diff_dist_threshold_to_left_bound: 0.29 #[m] + diff_dist_threshold_to_right_bound: -0.29 #[m] + reference_path_resolution: 0.5 #[m] diff --git a/perception/map_based_prediction/include/cubic_spline.hpp b/perception/map_based_prediction/include/cubic_spline.hpp deleted file mode 100644 index d55f8ee9dde43..0000000000000 --- a/perception/map_based_prediction/include/cubic_spline.hpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2018 Forrest -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef CUBIC_SPLINE_HPP_ -#define CUBIC_SPLINE_HPP_ - -#include - -#include -#include -#include -#include -#include -#include - -namespace map_based_prediction -{ -static std::vector vec_diff(const std::vector & input) -{ - std::vector output; - for (unsigned int i = 1; i < input.size(); i++) { - output.push_back(input[i] - input[i - 1]); - } - return output; -} - -static std::vector cum_sum(const std::vector & input) -{ - std::vector output; - double temp = 0; - for (unsigned int i = 0; i < input.size(); i++) { - temp += input[i]; - output.push_back(temp); - } - return output; -} - -class Spline -{ -public: - std::vector x; - std::vector y; - int nx; - std::vector h; - std::vector a; - std::vector b; - std::vector c; - // Eigen::VectorXf c; - std::vector d; - - Spline() {} - // d_i * (x-x_i)^3 + c_i * (x-x_i)^2 + b_i * (x-x_i) + a_i - Spline(const std::vector & x_, const std::vector & y_) - : x(x_), y(y_), nx(x_.size()), h(vec_diff(x_)), a(y_) - { - Eigen::MatrixXd A = calc_A(); - Eigen::VectorXd B = calc_B(); - // std::cerr << "A det " << A.determinant() << std::endl; - // std::cerr << "A QR" << A.colPivHouseholderQr().solve(B) << std::endl; - Eigen::VectorXd c_eigen = A.colPivHouseholderQr().solve(B); - // std::cerr << "c eigen " << c_eigen << std::endl; - double * c_pointer = c_eigen.data(); - c.assign(c_pointer, c_pointer + c_eigen.rows()); - - for (int i = 0; i < nx - 1; i++) { - d.push_back((c[i + 1] - c[i]) / (3.0 * h[i])); - b.push_back((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2 * c[i]) / 3.0); - } - } - - double calc(double t) - { - if (t < x.front() || t > x.back()) { - std::cout << "Dangerous" << std::endl; - std::cout << t << std::endl; - throw std::invalid_argument("received value out of the pre-defined range"); - } - int seg_id = bisect(t, 0, nx); - double dx = t - x[seg_id]; - return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx; - } - - double calc(double t, double s) - { - if (t < 0 || t > s) { - std::cout << "Dangerous" << std::endl; - std::cout << t << std::endl; - throw std::invalid_argument("received value out of the pre-defined range"); - } - int seg_id = bisect(t, 0, nx); - double dx = t - x[seg_id]; - return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx; - } - - double calc_d(double t) - { - if (t < x.front() || t > x.back()) { - std::cout << "Dangerous" << std::endl; - std::cout << t << std::endl; - throw std::invalid_argument("received value out of the pre-defined range"); - } - int seg_id = bisect(t, 0, nx - 1); - double dx = t - x[seg_id]; - return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx; - } - - double calc_d(double t, double s) - { - if (t < 0 || t > s) { - std::cout << "Dangerous" << std::endl; - std::cout << t << std::endl; - throw std::invalid_argument("received value out of the pre-defined range"); - } - int seg_id = bisect(t, 0, nx - 1); - double dx = t - x[seg_id]; - return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx; - } - - double calc_dd(double t) - { - if (t < x.front() || t > x.back()) { - std::cout << "Dangerous" << std::endl; - std::cout << t << std::endl; - throw std::invalid_argument("received value out of the pre-defined range"); - } - int seg_id = bisect(t, 0, nx); - double dx = t - x[seg_id]; - return 2 * c[seg_id] + 6 * d[seg_id] * dx; - } - - double calc_dd(double t, double s) - { - if (t < 0.0 || t > s) { - std::cout << "Dangerous" << std::endl; - std::cout << t << std::endl; - throw std::invalid_argument("received value out of the pre-defined range"); - } - int seg_id = bisect(t, 0, nx); - double dx = t - x[seg_id]; - return 2 * c[seg_id] + 6 * d[seg_id] * dx; - } - -private: - Eigen::MatrixXd calc_A() - { - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(nx, nx); - A(0, 0) = 1; - for (int i = 0; i < nx - 1; i++) { - if (i != nx - 2) { - A(i + 1, i + 1) = 2 * (h[i] + h[i + 1]); - } - A(i + 1, i) = h[i]; - A(i, i + 1) = h[i]; - } - A(0, 1) = 0.0; - A(nx - 1, nx - 2) = 0.0; - A(nx - 1, nx - 1) = 1.0; - return A; - } - Eigen::VectorXd calc_B() - { - Eigen::VectorXd B = Eigen::VectorXd::Zero(nx); - for (int i = 0; i < nx - 2; i++) { - B(i + 1) = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] - 3.0 * (a[i + 1] - a[i]) / h[i]; - } - return B; - } - - int bisect(double t, int start, int end) - { - int mid = (start + end) / 2; - if (t == x[mid] || end - start <= 1) { - return mid; - } else if (t > x[mid]) { - return bisect(t, mid, end); - } else { - return bisect(t, start, mid); - } - } -}; - -class Spline2D -{ -public: - Spline sx; - Spline sy; - std::vector s; - double max_s_value_; - - Spline2D(const std::vector & x, const std::vector & y) - { - s = calc_s(x, y); - sx = Spline(s, x); - sy = Spline(s, y); - max_s_value_ = *std::max_element(s.begin(), s.end()); - } - - std::array calc_position(double s_t) - { - double x = sx.calc(s_t, max_s_value_); - double y = sy.calc(s_t, max_s_value_); - return {{x, y}}; - } - - double calc_curvature(double s_t) - { - double dx = sx.calc_d(s_t, max_s_value_); - double ddx = sx.calc_dd(s_t, max_s_value_); - double dy = sy.calc_d(s_t, max_s_value_); - double ddy = sy.calc_dd(s_t, max_s_value_); - return (ddy * dx - ddx * dy) / (dx * dx + dy * dy); - } - - double calc_yaw(double s_t) - { - double dx = sx.calc_d(s_t, max_s_value_); - double dy = sy.calc_d(s_t, max_s_value_); - return std::atan2(dy, dx); - } - -private: - std::vector calc_s(const std::vector & x, const std::vector & y) - { - std::vector ds; - std::vector out_s{0}; - std::vector dx = vec_diff(x); - std::vector dy = vec_diff(y); - - for (unsigned int i = 0; i < dx.size(); i++) { - ds.push_back(std::sqrt(dx[i] * dx[i] + dy[i] * dy[i])); - } - - std::vector cum_ds = cum_sum(ds); - out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end()); - return out_s; - } -}; -} // namespace map_based_prediction - -#endif // CUBIC_SPLINE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction.hpp b/perception/map_based_prediction/include/map_based_prediction.hpp deleted file mode 100644 index 19678f8c2a482..0000000000000 --- a/perception/map_based_prediction/include/map_based_prediction.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2018-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 MAP_BASED_PREDICTION_HPP_ -#define MAP_BASED_PREDICTION_HPP_ - -#include -#include -#include -#include - -#include - -namespace map_based_prediction -{ -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; -using autoware_auto_perception_msgs::msg::TrackedObjects; - -struct DynamicObjectWithLanes -{ - TrackedObject object; - std::vector> lanes; - std::vector confidence; -}; - -struct DynamicObjectWithLanesArray -{ - std_msgs::msg::Header header; - std::vector objects; -}; - -class Spline2D; - -class MapBasedPrediction -{ -private: - double interpolating_resolution_; - double time_horizon_; - double sampling_delta_time_; - - bool getPredictedPath( - const double height, const double current_d_position, const double current_d_velocity, - const double current_s_position, const double current_s_velocity, - const std_msgs::msg::Header & origin_header, Spline2D & spline2d, PredictedPath & path) const; - - void getLinearPredictedPath( - const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - PredictedPath & predicted_path) const; - - void normalizeLikelihood(PredictedObjectKinematics & predicted_object_kinematics); - - PredictedObjectKinematics convertToPredictedKinematics( - const TrackedObjectKinematics & tracked_object); - -public: - MapBasedPrediction( - double interpolating_resolution, double time_horizon, double sampling_delta_time); - - bool doPrediction( - const DynamicObjectWithLanesArray & in_objects, std::vector & out_objects); - - bool doLinearPrediction( - const PredictedObjects & in_objects, std::vector & out_objects); - - PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); -}; -} // namespace map_based_prediction - -#endif // MAP_BASED_PREDICTION_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp new file mode 100644 index 0000000000000..57f2fe3f2120c --- /dev/null +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -0,0 +1,200 @@ +// 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 MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ +#define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ + +#include "map_based_prediction/path_generator.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace map_based_prediction +{ + +struct ObjectData +{ + std_msgs::msg::Header header; + lanelet::ConstLanelets current_lanelets; + lanelet::ConstLanelets future_possible_lanelets; + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist twist; + double time_delay; +}; + +enum class Maneuver { + LANE_FOLLOW = 0, + LEFT_LANE_CHANGE = 1, + RIGHT_LANE_CHANGE = 2, +}; + +struct LaneletData +{ + lanelet::Lanelet lanelet; + float probability; +}; + +struct PredictedRefPath +{ + float probability; + PosePath path; + Maneuver maneuver; +}; + +using LaneletsData = std::vector; +using ManeuverProbability = std::unordered_map; +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +class MapBasedPredictionNode : public rclcpp::Node +{ +public: + explicit MapBasedPredictionNode(const rclcpp::NodeOptions & node_options); + +private: + // ROS Publisher and Subscriber + rclcpp::Publisher::SharedPtr pub_objects_; + rclcpp::Publisher::SharedPtr pub_debug_markers_; + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_map_; + + // Object History + std::unordered_map> objects_history_; + + // Lanelet Map Pointers + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr routing_graph_ptr_; + std::shared_ptr traffic_rules_ptr_; + + // Pose Transform Listener + tier4_autoware_utils::TransformListener transform_listener_{this}; + + // Path Generator + std::shared_ptr path_generator_; + + // Parameters + bool enable_delay_compensation_; + double prediction_time_horizon_; + double prediction_sampling_time_interval_; + double min_velocity_for_map_based_prediction_; + double debug_accumulated_time_; + double dist_threshold_for_searching_lanelet_; + double delta_yaw_threshold_for_searching_lanelet_; + double sigma_lateral_offset_; + double sigma_yaw_angle_deg_; + double object_buffer_time_length_; + double history_time_length_; + double dist_ratio_threshold_to_left_bound_; + double dist_ratio_threshold_to_right_bound_; + double diff_dist_threshold_to_left_bound_; + double diff_dist_threshold_to_right_bound_; + double reference_path_resolution_; + + // Member Functions + void mapCallback(const HADMapBin::ConstSharedPtr msg); + void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); + + PredictedObjectKinematics convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object); + + PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); + + void removeOldObjectsHistory(const double current_time); + + LaneletsData getCurrentLanelets(const TrackedObject & object); + bool checkCloseLaneletCondition( + const std::pair & lanelet, const TrackedObject & object, + const lanelet::BasicPoint2d & search_point); + float calculateLocalLikelihood( + const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; + static double getObjectYaw(const TrackedObject & object); + + void updateObjectsHistory( + const std_msgs::msg::Header & header, const TrackedObject & object, + const LaneletsData & current_lanelets_data); + + std::vector getPredictedReferencePath( + const TrackedObject & object, const LaneletsData & current_lanelets_data, + const double object_detected_time); + Maneuver predictObjectManeuver( + const TrackedObject & object, const LaneletData & current_lanelet, + const double object_detected_time); + void addValidPath( + const lanelet::routing::LaneletPaths & candidate_paths, + lanelet::routing::LaneletPaths & valid_paths); + geometry_msgs::msg::Pose compensateTimeDelay( + const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist, + const double dt) const; + double calcRightLateralOffset( + const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose); + double calcLeftLateralOffset( + const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose); + ManeuverProbability calculateManeuverProbability( + const Maneuver & predicted_maneuver, const lanelet::routing::LaneletPaths & left_paths, + const lanelet::routing::LaneletPaths & right_paths, + const lanelet::routing::LaneletPaths & center_paths); + + void addReferencePaths( + const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, + const float path_probability, const ManeuverProbability & maneuver_probability, + const Maneuver & maneuver, std::vector & reference_paths); + std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); + PosePath resamplePath(const PosePath & base_path) const; + + void updateFuturePossibleLanelets( + const TrackedObject & object, const lanelet::routing::LaneletPaths & paths); + + bool isDuplicated( + const std::pair & target_lanelet, + const LaneletsData & lanelets_data); + bool isDuplicated( + const PredictedPath & predicted_path, const std::vector & predicted_paths); + + visualization_msgs::msg::Marker getDebugMarker( + const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); +}; +} // namespace map_based_prediction + +#endif // MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp new file mode 100644 index 0000000000000..14922078095fb --- /dev/null +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -0,0 +1,95 @@ +// 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 MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ +#define MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace map_based_prediction +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +struct FrenetPoint +{ + double s; + double d; + float s_vel; + float d_vel; + float s_acc; + float d_acc; +}; + +using FrenetPath = std::vector; +using PosePath = std::vector; + +class PathGenerator +{ +public: + PathGenerator(const double time_horizon, const double sampling_time_interval); + + PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); + + PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const; + + PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); + + PredictedPath generatePathForOnLaneVehicle( + const TrackedObject & object, const PosePath & ref_paths); + +private: + // Parameters + double time_horizon_; + double sampling_time_interval_; + + // Member functions + PredictedPath generateStraightPath(const TrackedObject & object) const; + + PredictedPath generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path); + + FrenetPath generateFrenetPath( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); + Eigen::Vector3d calcLatCoefficients( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + Eigen::Vector2d calcLonCoefficients( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + + PosePath interpolateReferencePath( + const PosePath & base_path, const FrenetPath & frenet_predicted_path); + + PredictedPath convertToPredictedPath( + const TrackedObject & object, const FrenetPath & frenet_predicted_path, + const PosePath & ref_path); + + FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path); +}; +} // namespace map_based_prediction + +#endif // MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction_ros.hpp b/perception/map_based_prediction/include/map_based_prediction_ros.hpp deleted file mode 100644 index 693a711cfa05e..0000000000000 --- a/perception/map_based_prediction/include/map_based_prediction_ros.hpp +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright 2018-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 MAP_BASED_PREDICTION_ROS_HPP_ -#define MAP_BASED_PREDICTION_ROS_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace tf2_ros -{ -class Buffer; -class TransformListener; -} // namespace tf2_ros - -namespace lanelet -{ -class Lanelet; -class LaneletMap; -using LaneletMapPtr = std::shared_ptr; -namespace routing -{ -class RoutingGraph; -} // namespace routing -namespace traffic_rules -{ -class TrafficRules; -} // namespace traffic_rules -} // namespace lanelet - -namespace map_based_prediction -{ -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; - -struct ObjectData -{ - lanelet::ConstLanelets current_lanelets; - lanelet::ConstLanelets future_possible_lanelets; - geometry_msgs::msg::PoseStamped pose; -}; - -enum class Maneuver { - LANE_FOLLOW, - LEFT_LANE_CHANGE, - RIGHT_LANE_CHANGE, -}; - -class MapBasedPrediction; - -class MapBasedPredictionROS : public rclcpp::Node -{ -private: - double prediction_time_horizon_; - double prediction_sampling_delta_time_; - double min_velocity_for_map_based_prediction_; - double interpolating_resolution_; - double debug_accumulated_time_; - double dist_threshold_for_searching_lanelet_; - double delta_yaw_threshold_for_searching_lanelet_; - double sigma_lateral_offset_; - double sigma_yaw_angle_; - double history_time_length_; - double dist_ratio_threshold_to_left_bound_; - double dist_ratio_threshold_to_right_bound_; - double diff_dist_threshold_to_left_bound_; - double diff_dist_threshold_to_right_bound_; - - rclcpp::Subscription::SharedPtr sub_objects_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Publisher::SharedPtr pub_objects_; - rclcpp::Publisher::SharedPtr pub_markers_; - - std::unordered_map> object_buffer_; - - std::shared_ptr tf_buffer_ptr_; - std::shared_ptr tf_listener_ptr_; - - std::shared_ptr lanelet_map_ptr_; - std::shared_ptr routing_graph_ptr_; - std::shared_ptr traffic_rules_ptr_; - std::shared_ptr map_based_prediction_; - - bool getSelfPose(geometry_msgs::msg::Pose & self_pose, const std_msgs::msg::Header & header); - bool getSelfPoseInMap(geometry_msgs::msg::Pose & self_pose); - - double getObjectYaw(const TrackedObject & object); - double calculateLikelihood( - const std::vector & path, const TrackedObject & object); - - void addValidPath( - const lanelet::routing::LaneletPaths & candidate_paths, - lanelet::routing::LaneletPaths & valid_paths); - - void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); - void mapCallback(const HADMapBin::ConstSharedPtr msg); - - bool getClosestLanelets( - const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, - lanelet::ConstLanelets & closest_lanelets); - - bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const lanelet::BasicPoint2d & search_point); - - void removeInvalidObject(const double current_time); - bool updateObjectBuffer( - const std_msgs::msg::Header & header, const TrackedObject & object, - lanelet::ConstLanelets & current_lanelets); - void updatePossibleLanelets( - const std::string object_id, const lanelet::routing::LaneletPaths & paths); - - double calcRightLateralOffset( - const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); - double calcLeftLateralOffset( - const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); - Maneuver detectLaneChange( - const TrackedObject & object, const lanelet::ConstLanelet & current_lanelet, - const double current_time); - -public: - explicit MapBasedPredictionROS(const rclcpp::NodeOptions & node_options); -}; -} // namespace map_based_prediction - -#endif // MAP_BASED_PREDICTION_ROS_HPP_ diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml index 573fac87e379e..884b4947b3571 100644 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -1,33 +1,13 @@ - - - - - - - - - - - - + + + - - - - - - - - - - - - + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 2a2dd0690ee76..58843ee976164 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -11,6 +11,7 @@ ament_cmake autoware_auto_perception_msgs + interpolation lanelet2_extension rclcpp rclcpp_components @@ -19,6 +20,7 @@ tf2_ros tier4_autoware_utils unique_identifier_msgs + visualization_msgs ament_lint_auto autoware_lint_common diff --git a/perception/map_based_prediction/src/debug.cpp b/perception/map_based_prediction/src/debug.cpp new file mode 100644 index 0000000000000..f78879ba209a1 --- /dev/null +++ b/perception/map_based_prediction/src/debug.cpp @@ -0,0 +1,49 @@ +// 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 "map_based_prediction/map_based_prediction_node.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" + +namespace map_based_prediction +{ +visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( + const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num) +{ + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = "maneuver"; + + marker.id = static_cast(obj_num); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = object.kinematics.pose_with_covariance.pose; + marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0); + + // Color by maneuver + double r = 0.0; + double g = 0.0; + double b = 0.0; + if (maneuver == Maneuver::LEFT_LANE_CHANGE) { + g = 1.0; + } else if (maneuver == Maneuver::RIGHT_LANE_CHANGE) { + r = 1.0; + } else { + b = 1.0; + } + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.8); + + return marker; +} +} // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction.cpp b/perception/map_based_prediction/src/map_based_prediction.cpp deleted file mode 100644 index 7bd9f7498590d..0000000000000 --- a/perception/map_based_prediction/src/map_based_prediction.cpp +++ /dev/null @@ -1,303 +0,0 @@ -// 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 - -namespace map_based_prediction -{ -MapBasedPrediction::MapBasedPrediction( - double interpolating_resolution, double time_horizon, double sampling_delta_time) -: interpolating_resolution_(interpolating_resolution), - time_horizon_(time_horizon), - sampling_delta_time_(sampling_delta_time) -{ -} - -bool MapBasedPrediction::doPrediction( - const DynamicObjectWithLanesArray & in_objects, std::vector & out_objects) -{ - for (auto & object_with_lanes : in_objects.objects) { - PredictedObject tmp_object; - tmp_object = convertToPredictedObject(object_with_lanes.object); - for (size_t path_id = 0; path_id < object_with_lanes.lanes.size(); ++path_id) { - std::vector tmp_x; - std::vector tmp_y; - std::vector path = object_with_lanes.lanes.at(path_id); - for (size_t i = 0; i < path.size(); i++) { - if (i > 0) { - double dist = tier4_autoware_utils::calcDistance2d(path.at(i), path.at(i - 1)); - if (dist < interpolating_resolution_) { - continue; - } - } - tmp_x.push_back(path.at(i).position.x); - tmp_y.push_back(path.at(i).position.y); - } - - // Generate Splined Trajectory Arc-Length Position and Yaw angle - Spline2D spline2d(tmp_x, tmp_y); - std::vector interpolated_points; - std::vector interpolated_yaws; - for (double s = 0.0; s < spline2d.s.back(); s += interpolating_resolution_) { - std::array point1 = spline2d.calc_position(s); - geometry_msgs::msg::Point g_point; - g_point.x = point1[0]; - g_point.y = point1[1]; - g_point.z = object_with_lanes.object.kinematics.pose_with_covariance.pose.position.z; - interpolated_points.push_back(g_point); - interpolated_yaws.push_back(spline2d.calc_yaw(s)); - } - - // calculate initial position in Frenet coordinate - // Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame - // Path Planning for Highly Automated Driving on Embedded GPUs - geometry_msgs::msg::Point object_point = - object_with_lanes.object.kinematics.pose_with_covariance.pose.position; - - const size_t nearest_segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(interpolated_points, object_point); - const double l = tier4_autoware_utils::calcLongitudinalOffsetToSegment( - interpolated_points, nearest_segment_idx, object_point); - const double current_s_position = - tier4_autoware_utils::calcSignedArcLength(interpolated_points, 0, nearest_segment_idx) + l; - if (current_s_position > spline2d.s.back()) { - continue; - } - std::array s_point = spline2d.calc_position(current_s_position); - geometry_msgs::msg::Point nearest_point = - tier4_autoware_utils::createPoint(s_point[0], s_point[1], object_point.z); - double current_d_position = tier4_autoware_utils::calcDistance2d(nearest_point, object_point); - const double lane_yaw = spline2d.calc_yaw(current_s_position); - const std::vector origin_v = {std::cos(lane_yaw), std::sin(lane_yaw)}; - const std::vector object_v = { - object_point.x - nearest_point.x, object_point.y - nearest_point.y}; - const double cross2d = object_v.at(0) * origin_v.at(1) - object_v.at(1) * origin_v.at(0); - if (cross2d < 0) { - current_d_position *= -1; - } - - // Does not consider orientation of twist since predicting lane-direction - const double current_d_velocity = - object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.y; - const double current_s_velocity = - std::fabs(object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.x); - - // Predict Path - PredictedPath predicted_path; - getPredictedPath( - object_point.z, current_d_position, current_d_velocity, current_s_position, - current_s_velocity, in_objects.header, spline2d, predicted_path); - // substitute confidence value - predicted_path.confidence = object_with_lanes.confidence.at(path_id); - - // Check if the path is already generated - const double CLOSE_PATH_THRESHOLD = 0.1; - bool duplicate_flag = false; - for (const auto & prev_path : tmp_object.kinematics.predicted_paths) { - const auto prev_path_end = prev_path.path.back().position; - const auto current_path_end = predicted_path.path.back().position; - const double dist = tier4_autoware_utils::calcDistance2d(prev_path_end, current_path_end); - if (dist < CLOSE_PATH_THRESHOLD) { - duplicate_flag = true; - break; - } - } - - if (duplicate_flag) { - continue; - } - - tmp_object.kinematics.predicted_paths.push_back(predicted_path); - } - - normalizeLikelihood(tmp_object.kinematics); - out_objects.push_back(tmp_object); - } - return true; -} - -bool MapBasedPrediction::doLinearPrediction( - const PredictedObjects & in_objects, std::vector & out_objects) -{ - for (const auto & object : in_objects.objects) { - PredictedPath path; - getLinearPredictedPath( - object.kinematics.initial_pose_with_covariance.pose, - object.kinematics.initial_twist_with_covariance.twist, path); - PredictedObject tmp_object; - tmp_object = object; - tmp_object.kinematics.predicted_paths.push_back(path); - out_objects.push_back(tmp_object); - } - - return true; -} - -PredictedObject MapBasedPrediction::convertToPredictedObject(const TrackedObject & tracked_object) -{ - PredictedObject output; - output.object_id = tracked_object.object_id; - output.existence_probability = tracked_object.existence_probability; - output.classification = tracked_object.classification; - output.kinematics = convertToPredictedKinematics(tracked_object.kinematics); - output.shape = tracked_object.shape; - return output; -} - -PredictedObjectKinematics MapBasedPrediction::convertToPredictedKinematics( - const TrackedObjectKinematics & tracked_object) -{ - PredictedObjectKinematics output; - output.initial_pose_with_covariance = tracked_object.pose_with_covariance; - output.initial_twist_with_covariance = tracked_object.twist_with_covariance; - output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; - return output; -} - -void MapBasedPrediction::normalizeLikelihood( - PredictedObjectKinematics & predicted_object_kinematics) -{ - // might not be the smartest way - double sum_confidence = 0.0; - for (const auto & path : predicted_object_kinematics.predicted_paths) { - sum_confidence += path.confidence; - } - - for (auto & path : predicted_object_kinematics.predicted_paths) { - path.confidence = path.confidence / sum_confidence; - } -} - -bool MapBasedPrediction::getPredictedPath( - const double height, const double current_d_position, const double current_d_velocity, - const double current_s_position, const double current_s_velocity, - [[maybe_unused]] const std_msgs::msg::Header & origin_header, Spline2D & spline2d, - PredictedPath & path) const -{ - // Quintic polynomial for d - // A = np.array([[T**3, T**4, T**5], - // [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], - // [6 * T, 12 * T ** 2, 20 * T ** 3]]) - // A_inv = np.matrix([[10/(T**3), -4/(T**2), 1/(2*T)], - // [-15/(T**4), 7/(T**3), -1/(T**2)], - // [6/(T**5), -3/(T**4), 1/(2*T**3)]]) - // b = np.matrix([[xe - self.a0 - self.a1 * T - self.a2 * T**2], - // [vxe - self.a1 - 2 * self.a2 * T], - // [axe - 2 * self.a2]]) - double target_d_position = 0; - - double t = time_horizon_; - Eigen::Matrix3d a_3_inv; - a_3_inv << 10 / std::pow(t, 3), -4 / std::pow(t, 2), 1 / (2 * t), -15 / std::pow(t, 4), - 7 / std::pow(t, 3), -1 / std::pow(t, 2), 6 / std::pow(t, 5), -3 / std::pow(t, 4), - 1 / (2 * std::pow(t, 3)); - - double target_d_velocity = current_d_velocity; - double target_d_acceleration = 0; - Eigen::Vector3d b_3; - b_3 << target_d_position - current_d_position - current_d_velocity * t, - target_d_velocity - current_d_velocity, target_d_acceleration; - - Eigen::Vector3d x_3; - x_3 = a_3_inv * b_3; - - // Quadric polynomial - // A_inv = np.matrix([[1/(T**2), -1/(3*T)], - // [-1/(2*T**3), 1/(4*T**2)]]) - // b = np.matrix([[vxe - self.a1 - 2 * self.a2 * T], - // [axe - 2 * self.a2]]) - Eigen::Matrix2d a_2_inv; - a_2_inv << 1 / std::pow(t, 2), -1 / (3 * t), -1 / (2 * std::pow(t, 3)), 1 / (4 * std::pow(t, 2)); - double target_s_velocity = current_s_velocity; - Eigen::Vector2d b_2; - b_2 << target_s_velocity - current_s_velocity, 0; - Eigen::Vector2d x_2; - x_2 = a_2_inv * b_2; - - // sampling points from calculated path - double dt = sampling_delta_time_; - std::vector d_vec; - for (double i = 0; i < t; i += dt) { - const double calculated_d = current_d_position + current_d_velocity * i + 0 * 2 * i * i + - x_3(0) * i * i * i + x_3(1) * i * i * i * i + - x_3(2) * i * i * i * i * i; - const double calculated_s = current_s_position + current_s_velocity * i + 2 * 0 * i * i + - x_2(0) * i * i * i + x_2(1) * i * i * i * i; - - geometry_msgs::msg::Pose tmp_point; - if (calculated_s > spline2d.s.back()) { - break; - } - std::array p = spline2d.calc_position(calculated_s); - double yaw = spline2d.calc_yaw(calculated_s); - tmp_point.position.x = p[0] + std::cos(yaw - M_PI / 2.0) * calculated_d; - tmp_point.position.y = p[1] + std::sin(yaw - M_PI / 2.0) * calculated_d; - tmp_point.position.z = height; - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, yaw); - tmp_point.orientation = tf2::toMsg(quat); - path.path.push_back(tmp_point); - if (path.path.size() >= path.path.max_size()) { - break; - } - } - - path.time_step = rclcpp::Duration::from_seconds(dt); - return true; -} - -void MapBasedPrediction::getLinearPredictedPath( - const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - PredictedPath & predicted_path) const -{ - const double & sampling_delta_time = sampling_delta_time_; - const double & time_horizon = time_horizon_; - const double ep = 0.001; - - for (double dt = 0.0; dt < time_horizon + ep; dt += sampling_delta_time) { - geometry_msgs::msg::Pose tmp_pose; - geometry_msgs::msg::Pose object_frame_pose; - geometry_msgs::msg::Pose world_frame_pose; - object_frame_pose.position.x = object_twist.linear.x * dt; - object_frame_pose.position.y = object_twist.linear.y * dt; - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, 0.0); - object_frame_pose.orientation = tf2::toMsg(quat); - tf2::Transform tf_object2future; - tf2::Transform tf_world2object; - tf2::Transform tf_world2future; - - tf2::fromMsg(object_pose, tf_world2object); - tf2::fromMsg(object_frame_pose, tf_object2future); - tf_world2future = tf_world2object * tf_object2future; - tf2::toMsg(tf_world2future, world_frame_pose); - tmp_pose = world_frame_pose; - predicted_path.path.push_back(tmp_pose); - if (predicted_path.path.size() >= predicted_path.path.max_size()) { - break; - } - } - - predicted_path.confidence = 1.0; - predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_delta_time); -} -} // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp new file mode 100644 index 0000000000000..d2d95fb5e3eac --- /dev/null +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -0,0 +1,999 @@ +// 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 "map_based_prediction/map_based_prediction_node.hpp" + +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace +{ +std::string toHexString(const unique_identifier_msgs::msg::UUID & id) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; + } + return ss.str(); +} + +lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & data) +{ + lanelet::ConstLanelets lanelets; + for (const auto & lanelet_data : data) { + lanelets.push_back(lanelet_data.lanelet); + } + + return lanelets; +} +} // namespace + +namespace map_based_prediction +{ +MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) +: Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) +{ + enable_delay_compensation_ = declare_parameter("enable_delay_compensation", true); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon", 10.0); + prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time", 0.5); + min_velocity_for_map_based_prediction_ = + declare_parameter("min_velocity_for_map_based_prediction", 1.0); + dist_threshold_for_searching_lanelet_ = + declare_parameter("dist_threshold_for_searching_lanelet", 3.0); + delta_yaw_threshold_for_searching_lanelet_ = + declare_parameter("delta_yaw_threshold_for_searching_lanelet", 0.785); + sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset", 0.5); + sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg", 5.0); + object_buffer_time_length_ = declare_parameter("object_buffer_time_length", 2.0); + history_time_length_ = declare_parameter("history_time_length", 1.0); + dist_ratio_threshold_to_left_bound_ = + declare_parameter("dist_ratio_threshold_to_left_bound", -0.5); + dist_ratio_threshold_to_right_bound_ = + declare_parameter("dist_ratio_threshold_to_right_bound", 0.5); + diff_dist_threshold_to_left_bound_ = declare_parameter("diff_dist_threshold_to_left_bound", 0.29); + diff_dist_threshold_to_right_bound_ = + declare_parameter("diff_dist_threshold_to_right_bound", -0.29); + reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5); + + path_generator_ = + std::make_shared(prediction_time_horizon_, prediction_sampling_time_interval_); + + sub_objects_ = this->create_subscription( + "/perception/object_recognition/tracking/objects", 1, + std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); + sub_map_ = this->create_subscription( + "/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); + + pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); + pub_debug_markers_ = + this->create_publisher("maneuver", rclcpp::QoS{1}); +} + +PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object) +{ + PredictedObjectKinematics output; + output.initial_pose_with_covariance = tracked_object.pose_with_covariance; + output.initial_twist_with_covariance = tracked_object.twist_with_covariance; + output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; + return output; +} + +PredictedObject MapBasedPredictionNode::convertToPredictedObject( + const TrackedObject & tracked_object) +{ + PredictedObject predicted_object; + predicted_object.kinematics = convertToPredictedKinematics(tracked_object.kinematics); + predicted_object.classification = tracked_object.classification; + predicted_object.object_id = tracked_object.object_id; + predicted_object.shape = tracked_object.shape; + predicted_object.existence_probability = tracked_object.existence_probability; + + return predicted_object; +} + +void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "[Map Based Prediction]: Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + RCLCPP_INFO(get_logger(), "[Map Based Prediction]: Map is loaded"); +} + +void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) +{ + // Guard for map pointer and frame transformation + if (!lanelet_map_ptr_) { + return; + } + + auto world2map_transform = transform_listener_.getTransform( + "map", // target + in_objects->header.frame_id, // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + auto map2world_transform = transform_listener_.getTransform( + in_objects->header.frame_id, // target + "map", // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + auto debug_map2lidar_transform = transform_listener_.getTransform( + "base_link", // target + "map", // src + rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); + + if (!world2map_transform || !map2world_transform || !debug_map2lidar_transform) { + return; + } + + // Remove old objects information in object history + const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); + removeOldObjectsHistory(objects_detected_time); + + // result output + PredictedObjects output; + output.header = in_objects->header; + output.header.frame_id = "map"; + + // result debug + visualization_msgs::msg::MarkerArray debug_markers; + + for (const auto & object : in_objects->objects) { + std::string object_id = toHexString(object.object_id); + TrackedObject transformed_object = object; + + // transform object frame if it's based on map frame + if (in_objects->header.frame_id != "map") { + geometry_msgs::msg::PoseStamped pose_in_map; + geometry_msgs::msg::PoseStamped pose_orig; + pose_orig.pose = object.kinematics.pose_with_covariance.pose; + tf2::doTransform(pose_orig, pose_in_map, *world2map_transform); + transformed_object.kinematics.pose_with_covariance.pose = pose_in_map.pose; + } + + // For non-vehicle object + if ( + transformed_object.classification.front().label != ObjectClassification::CAR && + transformed_object.classification.front().label != ObjectClassification::BUS && + transformed_object.classification.front().label != ObjectClassification::TRAILER && + transformed_object.classification.front().label != ObjectClassification::MOTORCYCLE && + transformed_object.classification.front().label != ObjectClassification::TRUCK) { + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(transformed_object); + predicted_path.confidence = 1.0; + + auto predicted_object = convertToPredictedObject(transformed_object); + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object); + continue; + } + + // Get Closest Lanelet + const auto current_lanelets = getCurrentLanelets(transformed_object); + + // Update Objects History + updateObjectsHistory(output.header, transformed_object, current_lanelets); + + // For off lane obstacles + if (current_lanelets.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForOffLaneVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) { + continue; + } + + auto predicted_object = convertToPredictedObject(transformed_object); + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object); + continue; + } + + // For too-slow vehicle + if ( + std::fabs(transformed_object.kinematics.twist_with_covariance.twist.linear.x) < + min_velocity_for_map_based_prediction_) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) { + continue; + } + + auto predicted_object = convertToPredictedObject(transformed_object); + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object); + continue; + } + + // Get Predicted Reference Path for Each Maneuver and current lanelets + // return: + const auto ref_paths = + getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + + // If predicted reference path is empty, assume this object is out of the lane + if (ref_paths.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) { + continue; + } + + auto predicted_object = convertToPredictedObject(transformed_object); + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object); + continue; + } + + // Get Debug Marker for On Lane Vehicles + const auto max_prob_path = std::max_element( + ref_paths.begin(), ref_paths.end(), + [](const PredictedRefPath & a, const PredictedRefPath & b) { + return a.probability < b.probability; + }); + const auto debug_marker = + getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); + debug_markers.markers.push_back(debug_marker); + + // Generate Predicted Path + std::vector predicted_paths; + for (const auto & ref_path : ref_paths) { + PredictedPath predicted_path = + path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + predicted_path.confidence = ref_path.probability; + + predicted_paths.push_back(predicted_path); + } + + // Normalize Path Confidence and output the predicted object + { + float sum_confidence = 0.0; + for (const auto & predicted_path : predicted_paths) { + sum_confidence += predicted_path.confidence; + } + const float min_sum_confidence_value = 1e-3; + sum_confidence = std::max(sum_confidence, min_sum_confidence_value); + + for (auto & predicted_path : predicted_paths) { + predicted_path.confidence = predicted_path.confidence / sum_confidence; + } + + auto predicted_object = convertToPredictedObject(transformed_object); + for (const auto & predicted_path : predicted_paths) { + predicted_object.kinematics.predicted_paths.push_back(predicted_path); + } + output.objects.push_back(predicted_object); + } + } + + // Publish Results + pub_objects_->publish(output); + pub_debug_markers_->publish(debug_markers); +} + +double MapBasedPredictionNode::getObjectYaw(const TrackedObject & object) +{ + if (object.kinematics.orientation_availability) { + return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + } + + const auto & object_pose = object.kinematics.pose_with_covariance.pose; + const auto & object_twist = object.kinematics.twist_with_covariance.twist; + const auto future_object_pose = tier4_autoware_utils::calcOffsetPose( + object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); + return tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); +} + +void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time) +{ + std::vector invalid_object_id; + for (auto iter = objects_history_.begin(); iter != objects_history_.end(); ++iter) { + const std::string object_id = iter->first; + std::deque & object_data = iter->second; + + // If object data is empty, we are going to delete the buffer for the obstacle + if (object_data.empty()) { + invalid_object_id.push_back(object_id); + continue; + } + + const double latest_object_time = rclcpp::Time(object_data.back().header.stamp).seconds(); + + // Delete Old Objects + if (current_time - latest_object_time > 2.0) { + invalid_object_id.push_back(object_id); + continue; + } + + // Delete old information + while (!object_data.empty()) { + const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds(); + if (current_time - post_object_time > 2.0) { + // Delete Old Position + object_data.pop_front(); + } else { + break; + } + } + + if (object_data.empty()) { + invalid_object_id.push_back(object_id); + continue; + } + } + + for (const auto & key : invalid_object_id) { + objects_history_.erase(key); + } +} + +LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) +{ + // obstacle point + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + // nearest lanelet + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10); + + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return {}; + } + + LaneletsData closest_lanelets; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets and + // Check if similar lanelet is inside the closest lanelet + if ( + !checkCloseLaneletCondition(lanelet, object, search_point) || + isDuplicated(lanelet, closest_lanelets)) { + continue; + } + + LaneletData closest_lanelet; + closest_lanelet.lanelet = lanelet.second; + closest_lanelet.probability = calculateLocalLikelihood(lanelet.second, object); + closest_lanelets.push_back(closest_lanelet); + } + + return closest_lanelets; +} + +bool MapBasedPredictionNode::checkCloseLaneletCondition( + const std::pair & lanelet, const TrackedObject & object, + const lanelet::BasicPoint2d & search_point) +{ + // Step1. If we only have one point in the centerline, we will ignore the lanelet + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + // Step2. Check if the obstacle is inside of this lanelet + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + return false; + } + + // If the object is in the objects history, we check if the target lanelet is + // inside the current lanelets id or following lanelets + const std::string object_id = toHexString(object.object_id); + if (objects_history_.count(object_id) != 0) { + const std::vector & possible_lanelet = + objects_history_.at(object_id).back().future_possible_lanelets; + + bool not_in_possible_lanelet = + std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) == + possible_lanelet.end(); + if (!possible_lanelet.empty() && not_in_possible_lanelet) { + return false; + } + } + + // Step3. Calculate the angle difference between the lane angle and obstacle angle + const double object_yaw = getObjectYaw(object); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + + // Step4. Check if the closest lanelet is valid, and add all + // of the lanelets that are below max_dist and max_delta_yaw + if ( + lanelet.first < dist_threshold_for_searching_lanelet_ && + abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_) { + return true; + } + + return false; +} + +float MapBasedPredictionNode::calculateLocalLikelihood( + const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const +{ + const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; + + // compute yaw difference between the object and lane + const double obj_yaw = getObjectYaw(object); + const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); + const double delta_yaw = obj_yaw - lane_yaw; + const double abs_norm_delta_yaw = std::fabs(tier4_autoware_utils::normalizeRadian(delta_yaw)); + + // compute lateral distance + const auto centerline = current_lanelet.centerline(); + std::vector converted_centerline; + for (const auto & p : centerline) { + const auto converted_p = lanelet::utils::conversion::toGeomMsgPt(p); + converted_centerline.push_back(converted_p); + } + const double lat_dist = + std::fabs(tier4_autoware_utils::calcLateralOffset(converted_centerline, obj_point)); + + // Compute Chi-squared distributed (Equation (8) in the paper) + const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position + const double sigma_yaw = M_PI * sigma_yaw_angle_deg_ / 180.0; // Standard Deviation for yaw angle + const Eigen::Vector2d delta(lat_dist, abs_norm_delta_yaw); + const Eigen::Matrix2d P_inv = + (Eigen::Matrix2d() << 1.0 / (sigma_d * sigma_d), 0.0, 0.0, 1.0 / (sigma_yaw * sigma_yaw)) + .finished(); + const double MINIMUM_DISTANCE = 1e-6; + const double dist = std::max(delta.dot(P_inv * delta), MINIMUM_DISTANCE); + + return static_cast(1.0 / dist); +} + +void MapBasedPredictionNode::updateObjectsHistory( + const std_msgs::msg::Header & header, const TrackedObject & object, + const LaneletsData & current_lanelets_data) +{ + std::string object_id = toHexString(object.object_id); + const auto current_lanelets = getLanelets(current_lanelets_data); + + ObjectData single_object_data; + single_object_data.header = header; + single_object_data.current_lanelets = current_lanelets; + single_object_data.future_possible_lanelets = current_lanelets; + single_object_data.pose = object.kinematics.pose_with_covariance.pose; + const double object_yaw = getObjectYaw(object); + single_object_data.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(object_yaw); + single_object_data.time_delay = std::fabs((this->get_clock()->now() - header.stamp).seconds()); + single_object_data.twist = object.kinematics.twist_with_covariance.twist; + + if (objects_history_.count(object_id) == 0) { + // New Object(Create a new object in object histories) + std::deque object_data = {single_object_data}; + objects_history_.emplace(object_id, object_data); + } else { + // Object that is already in the object buffer + std::deque & object_data = objects_history_.at(object_id); + object_data.push_back(single_object_data); + } +} + +std::vector MapBasedPredictionNode::getPredictedReferencePath( + const TrackedObject & object, const LaneletsData & current_lanelets_data, + const double object_detected_time) +{ + const double delta_horizon = 1.0; + const double obj_vel = object.kinematics.twist_with_covariance.twist.linear.x; + + std::vector all_ref_paths; + for (const auto & current_lanelet_data : current_lanelets_data) { + // Step1. Get the path + // Step1.1 Get the left lanelet + lanelet::routing::LaneletPaths left_paths; + auto opt_left = routing_graph_ptr_->left(current_lanelet_data.lanelet); + if (!!opt_left) { + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(*opt_left, search_dist, 0, false); + addValidPath(tmp_paths, left_paths); + } + } + + // Step1.2 Get the right lanelet + lanelet::routing::LaneletPaths right_paths; + auto opt_right = routing_graph_ptr_->right(current_lanelet_data.lanelet); + if (!!opt_right) { + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(*opt_right, search_dist, 0, false); + addValidPath(tmp_paths, right_paths); + } + } + + // Step1.3 Get the centerline + lanelet::routing::LaneletPaths center_paths; + for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { + const double search_dist = horizon * obj_vel + 10.0; + lanelet::routing::LaneletPaths tmp_paths = + routing_graph_ptr_->possiblePaths(current_lanelet_data.lanelet, search_dist, 0, false); + addValidPath(tmp_paths, center_paths); + } + + // Skip calculations if all paths are empty + if (left_paths.empty() && right_paths.empty() && center_paths.empty()) { + continue; + } + + // Step2. Predict Object Maneuver + const Maneuver predicted_maneuver = + predictObjectManeuver(object, current_lanelet_data, object_detected_time); + + // Step3. Allocate probability for each predicted maneuver + const auto maneuver_prob = + calculateManeuverProbability(predicted_maneuver, left_paths, right_paths, center_paths); + + // Step4. add candidate reference paths to the all_ref_paths + const float path_prob = current_lanelet_data.probability; + const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) { + addReferencePaths(object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths); + }; + addReferencePathsLocal(left_paths, Maneuver::LEFT_LANE_CHANGE); + addReferencePathsLocal(right_paths, Maneuver::RIGHT_LANE_CHANGE); + addReferencePathsLocal(center_paths, Maneuver::LANE_FOLLOW); + } + + return all_ref_paths; +} + +void MapBasedPredictionNode::addValidPath( + const lanelet::routing::LaneletPaths & candidate_paths, + lanelet::routing::LaneletPaths & valid_paths) +{ + // Check if candidate paths are already in the valid paths + for (const auto & candidate_path : candidate_paths) { + bool already_searched = false; + for (const auto & valid_path : valid_paths) { + for (const auto & llt : valid_path) { + if (candidate_path.back().id() == llt.id()) { + already_searched = true; + } + } + } + if (!already_searched) { + valid_paths.push_back(candidate_path); + } + } +} + +Maneuver MapBasedPredictionNode::predictObjectManeuver( + const TrackedObject & object, const LaneletData & current_lanelet_data, + const double object_detected_time) +{ + // Step1. Check if we have the object in the buffer + const std::string object_id = toHexString(object.object_id); + if (objects_history_.count(object_id) == 0) { + return Maneuver::LANE_FOLLOW; + } + + const std::deque & object_info = objects_history_.at(object_id); + const double current_time = (this->get_clock()->now()).seconds(); + + // Step2. Get the previous id + int prev_id = static_cast(object_info.size()) - 1; + while (prev_id >= 0) { + const double prev_time_delay = object_info.at(prev_id).time_delay; + const double prev_time = + rclcpp::Time(object_info.at(prev_id).header.stamp).seconds() + prev_time_delay; + // if (object_detected_time - prev_time > history_time_length_) { + if (current_time - prev_time > history_time_length_) { + break; + } + --prev_id; + } + + if (prev_id < 0) { + return Maneuver::LANE_FOLLOW; + } + + // Step3. Get closest previous lanelet ID + const auto & prev_info = object_info.at(static_cast(prev_id)); + const auto prev_pose = compensateTimeDelay(prev_info.pose, prev_info.twist, prev_info.time_delay); + const lanelet::ConstLanelets prev_lanelets = + object_info.at(static_cast(prev_id)).current_lanelets; + if (prev_lanelets.empty()) { + return Maneuver::LANE_FOLLOW; + } + lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); + double closest_prev_yaw = std::numeric_limits::max(); + for (const auto & lanelet : prev_lanelets) { + const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.position); + const double delta_yaw = tf2::getYaw(prev_pose.orientation) - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + if (normalized_delta_yaw < closest_prev_yaw) { + closest_prev_yaw = normalized_delta_yaw; + prev_lanelet = lanelet; + } + } + + // Step4. Check if the vehicle has changed lane + const auto current_lanelet = current_lanelet_data.lanelet; + const double current_time_delay = std::max(current_time - object_detected_time, 0.0); + const auto current_pose = compensateTimeDelay( + object.kinematics.pose_with_covariance.pose, object.kinematics.twist_with_covariance.twist, + current_time_delay); + const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose); + lanelet::routing::LaneletPaths possible_paths = + routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); + bool has_lane_changed = true; + for (const auto & path : possible_paths) { + for (const auto & lanelet : path) { + if (lanelet == current_lanelet) { + has_lane_changed = false; + break; + } + } + } + + if (has_lane_changed) { + return Maneuver::LANE_FOLLOW; + } + + // Step5. Lane Change Detection + const lanelet::ConstLineString2d prev_left_bound = prev_lanelet.leftBound2d(); + const lanelet::ConstLineString2d prev_right_bound = prev_lanelet.rightBound2d(); + const lanelet::ConstLineString2d current_left_bound = current_lanelet.leftBound2d(); + const lanelet::ConstLineString2d current_right_bound = current_lanelet.rightBound2d(); + const double prev_left_dist = calcLeftLateralOffset(prev_left_bound, prev_pose); + const double prev_right_dist = calcRightLateralOffset(prev_right_bound, prev_pose); + const double current_left_dist = calcLeftLateralOffset(current_left_bound, current_pose); + const double current_right_dist = calcRightLateralOffset(current_right_bound, current_pose); + const double prev_lane_width = std::fabs(prev_left_dist) + std::fabs(prev_right_dist); + const double current_lane_width = std::fabs(current_left_dist) + std::fabs(current_right_dist); + if (prev_lane_width < 1e-3 || current_lane_width < 1e-3) { + RCLCPP_ERROR(get_logger(), "[Map Based Prediction]: Lane Width is too small"); + return Maneuver::LANE_FOLLOW; + } + + const double current_left_dist_ratio = current_left_dist / current_lane_width; + const double current_right_dist_ratio = current_right_dist / current_lane_width; + const double diff_left_current_prev = current_left_dist - prev_left_dist; + const double diff_right_current_prev = current_right_dist - prev_right_dist; + + if ( + current_left_dist_ratio > dist_ratio_threshold_to_left_bound_ && + diff_left_current_prev > diff_dist_threshold_to_left_bound_) { + return Maneuver::LEFT_LANE_CHANGE; + } else if ( + current_right_dist_ratio < dist_ratio_threshold_to_right_bound_ && + diff_right_current_prev < diff_dist_threshold_to_right_bound_) { + return Maneuver::RIGHT_LANE_CHANGE; + } + + return Maneuver::LANE_FOLLOW; +} + +geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( + const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist, + const double dt) const +{ + if (!enable_delay_compensation_) { + return delayed_pose; + } + + /* == Nonlinear model == + * + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * + */ + + const double vx = twist.linear.x; + const double wz = twist.angular.z; + const double prev_yaw = tf2::getYaw(delayed_pose.orientation); + const double prev_x = delayed_pose.position.x; + const double prev_y = delayed_pose.position.y; + const double prev_z = delayed_pose.position.z; + + const double curr_x = prev_x + vx * std::cos(prev_yaw) * dt; + const double curr_y = prev_y + vx * std::sin(prev_yaw) * dt; + const double curr_z = prev_z; + const double curr_yaw = prev_yaw + wz * dt; + + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, curr_z); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + + return current_pose; +} + +double MapBasedPredictionNode::calcRightLateralOffset( + const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose) +{ + std::vector boundary_path(boundary_line.size()); + for (size_t i = 0; i < boundary_path.size(); ++i) { + const double x = boundary_line[i].x(); + const double y = boundary_line[i].y(); + boundary_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); + } + + return std::fabs(tier4_autoware_utils::calcLateralOffset(boundary_path, search_pose.position)); +} + +double MapBasedPredictionNode::calcLeftLateralOffset( + const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose) +{ + return -calcRightLateralOffset(boundary_line, search_pose); +} + +void MapBasedPredictionNode::updateFuturePossibleLanelets( + const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) +{ + std::string object_id = toHexString(object.object_id); + if (objects_history_.count(object_id) == 0) { + return; + } + + std::vector & possible_lanelets = + objects_history_.at(object_id).back().future_possible_lanelets; + for (const auto & path : paths) { + for (const auto & lanelet : path) { + bool not_in_buffer = std::find(possible_lanelets.begin(), possible_lanelets.end(), lanelet) == + possible_lanelets.end(); + if (not_in_buffer) { + possible_lanelets.push_back(lanelet); + } + } + } +} + +void MapBasedPredictionNode::addReferencePaths( + const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, + const float path_probability, const ManeuverProbability & maneuver_probability, + const Maneuver & maneuver, std::vector & reference_paths) +{ + if (!candidate_paths.empty()) { + updateFuturePossibleLanelets(object, candidate_paths); + const auto converted_paths = convertPathType(candidate_paths); + for (const auto & converted_path : converted_paths) { + PredictedRefPath predicted_path; + predicted_path.probability = maneuver_probability.at(maneuver) * path_probability; + predicted_path.path = converted_path; + predicted_path.maneuver = maneuver; + reference_paths.push_back(predicted_path); + } + } +} + +ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( + const Maneuver & predicted_maneuver, const lanelet::routing::LaneletPaths & left_paths, + const lanelet::routing::LaneletPaths & right_paths, + const lanelet::routing::LaneletPaths & center_paths) +{ + float left_lane_change_probability = 0.0; + float right_lane_change_probability = 0.0; + float lane_follow_probability = 0.0; + if (!left_paths.empty() && predicted_maneuver == Maneuver::LEFT_LANE_CHANGE) { + constexpr float LF_PROB_WHEN_LC = 0.9; // probability for lane follow during lane change + constexpr float LC_PROB_WHEN_LC = 1.0; // probability for left lane change + left_lane_change_probability = LC_PROB_WHEN_LC; + right_lane_change_probability = 0.0; + lane_follow_probability = LF_PROB_WHEN_LC; + } else if (!right_paths.empty() && predicted_maneuver == Maneuver::RIGHT_LANE_CHANGE) { + constexpr float LF_PROB_WHEN_LC = 0.9; // probability for lane follow during lane change + constexpr float RC_PROB_WHEN_LC = 1.0; // probability for right lane change + left_lane_change_probability = 0.0; + right_lane_change_probability = RC_PROB_WHEN_LC; + lane_follow_probability = LF_PROB_WHEN_LC; + } else if (!center_paths.empty()) { + constexpr float LF_PROB = 1.0; // probability for lane follow + constexpr float LC_PROB = 0.3; // probability for left lane change + constexpr float RC_PROB = 0.3; // probability for right lane change + if (predicted_maneuver == Maneuver::LEFT_LANE_CHANGE) { + // If prediction says left change, but left lane is empty, assume lane follow + left_lane_change_probability = 0.0; + right_lane_change_probability = (!right_paths.empty()) ? RC_PROB : 0.0; + } else if (predicted_maneuver == Maneuver::RIGHT_LANE_CHANGE) { + // If prediction says right change, but right lane is empty, assume lane follow + left_lane_change_probability = (!left_paths.empty()) ? LC_PROB : 0.0; + right_lane_change_probability = 0.0; + } else { + // Predicted Maneuver is Lane Follow + left_lane_change_probability = LC_PROB; + right_lane_change_probability = RC_PROB; + } + lane_follow_probability = LF_PROB; + } else { + // Center path is empty + constexpr float LC_PROB = 1.0; // probability for left lane change + constexpr float RC_PROB = 1.0; // probability for right lane change + lane_follow_probability = 0.0; + if (!left_paths.empty() && right_paths.empty()) { + left_lane_change_probability = LC_PROB; + right_lane_change_probability = 0.0; + } else if (left_paths.empty() && !right_paths.empty()) { + left_lane_change_probability = 0.0; + right_lane_change_probability = RC_PROB; + } else { + left_lane_change_probability = LC_PROB; + right_lane_change_probability = RC_PROB; + } + } + + const float MIN_PROBABILITY = 1e-3; + const float max_prob = std::max( + MIN_PROBABILITY, std::max( + lane_follow_probability, + std::max(left_lane_change_probability, right_lane_change_probability))); + + // Insert Normalized Probability + ManeuverProbability maneuver_prob; + maneuver_prob[Maneuver::LEFT_LANE_CHANGE] = left_lane_change_probability / max_prob; + maneuver_prob[Maneuver::RIGHT_LANE_CHANGE] = right_lane_change_probability / max_prob; + maneuver_prob[Maneuver::LANE_FOLLOW] = lane_follow_probability / max_prob; + + return maneuver_prob; +} + +std::vector MapBasedPredictionNode::convertPathType( + const lanelet::routing::LaneletPaths & paths) +{ + std::vector converted_paths; + for (const auto & path : paths) { + PosePath converted_path; + + // Insert Positions. Note that we start inserting points from previous lanelet + if (!path.empty()) { + lanelet::ConstLanelets prev_lanelets = routing_graph_ptr_->previous(path.front()); + if (!prev_lanelets.empty()) { + lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); + for (const auto & lanelet_p : prev_lanelet.centerline()) { + geometry_msgs::msg::Pose current_p; + current_p.position = + tier4_autoware_utils::createPoint(lanelet_p.x(), lanelet_p.y(), lanelet_p.z()); + const double lane_yaw = lanelet::utils::getLaneletAngle(prev_lanelet, current_p.position); + current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + converted_path.push_back(current_p); + } + } + } + + for (const auto & lanelet : path) { + for (const auto & lanelet_p : lanelet.centerline()) { + geometry_msgs::msg::Pose current_p; + current_p.position = + tier4_autoware_utils::createPoint(lanelet_p.x(), lanelet_p.y(), lanelet_p.z()); + const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, current_p.position); + current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + + // Prevent from inserting same points + if (!converted_path.empty()) { + const auto prev_p = converted_path.back(); + const double tmp_dist = tier4_autoware_utils::calcDistance2d(prev_p, current_p); + if (tmp_dist < 1e-6) { + continue; + } + } + + converted_path.push_back(current_p); + } + } + + // Resample Path + const auto resampled_converted_path = resamplePath(converted_path); + converted_paths.push_back(resampled_converted_path); + } + + return converted_paths; +} + +PosePath MapBasedPredictionNode::resamplePath(const PosePath & base_path) const +{ + std::vector base_s(base_path.size()); + std::vector base_x(base_path.size()); + std::vector base_y(base_path.size()); + std::vector base_z(base_path.size()); + for (size_t i = 0; i < base_path.size(); ++i) { + base_x.at(i) = base_path.at(i).position.x; + base_y.at(i) = base_path.at(i).position.y; + base_z.at(i) = base_path.at(i).position.z; + base_s.at(i) = tier4_autoware_utils::calcSignedArcLength(base_path, 0, i); + } + const double base_path_len = base_s.back(); + + std::vector resampled_s; + for (double s = 0.0; s <= base_path_len; s += reference_path_resolution_) { + resampled_s.push_back(s); + } + + if (resampled_s.empty()) { + return base_path; + } + + // Insert End Point + const double epsilon = 0.01; + if (std::fabs(resampled_s.back() - base_path_len) < epsilon) { + resampled_s.back() = base_path_len; + } else { + resampled_s.push_back(base_path_len); + } + + if (resampled_s.size() < 2) { + return base_path; + } + + const auto resampled_x = interpolation::lerp(base_s, base_x, resampled_s); + const auto resampled_y = interpolation::lerp(base_s, base_y, resampled_s); + const auto resampled_z = interpolation::lerp(base_s, base_z, resampled_s); + + PosePath resampled_path(resampled_x.size()); + // Position + for (size_t i = 0; i < resampled_path.size(); ++i) { + resampled_path.at(i).position = + tier4_autoware_utils::createPoint(resampled_x.at(i), resampled_y.at(i), resampled_z.at(i)); + } + // Orientation + for (size_t i = 0; i < resampled_path.size() - 1; ++i) { + const auto curr_p = resampled_path.at(i).position; + const auto next_p = resampled_path.at(i + 1).position; + const double yaw = std::atan2(next_p.y - curr_p.y, next_p.x - curr_p.x); + resampled_path.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + } + resampled_path.back().orientation = resampled_path.at(resampled_path.size() - 2).orientation; + + return resampled_path; +} + +bool MapBasedPredictionNode::isDuplicated( + const std::pair & target_lanelet, + const LaneletsData & lanelets_data) +{ + const double CLOSE_LANELET_THRESHOLD = 0.1; + for (const auto & lanelet_data : lanelets_data) { + const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); + const auto lanelet_end_p = lanelet_data.lanelet.centerline2d().back(); + const double dist = std::hypot( + target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); + if (dist < CLOSE_LANELET_THRESHOLD) { + return true; + } + } + + return false; +} + +bool MapBasedPredictionNode::isDuplicated( + const PredictedPath & predicted_path, const std::vector & predicted_paths) +{ + const double CLOSE_PATH_THRESHOLD = 0.1; + for (const auto & prev_predicted_path : predicted_paths) { + const auto prev_path_end = prev_predicted_path.path.back().position; + const auto current_path_end = predicted_path.path.back().position; + const double dist = tier4_autoware_utils::calcDistance2d(prev_path_end, current_path_end); + if (dist < CLOSE_PATH_THRESHOLD) { + return true; + } + } + + return false; +} +} // namespace map_based_prediction + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(map_based_prediction::MapBasedPredictionNode) diff --git a/perception/map_based_prediction/src/map_based_prediction_ros.cpp b/perception/map_based_prediction/src/map_based_prediction_ros.cpp deleted file mode 100644 index 19df2894e4087..0000000000000 --- a/perception/map_based_prediction/src/map_based_prediction_ros.cpp +++ /dev/null @@ -1,760 +0,0 @@ -// Copyright 2018-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. - -#include "map_based_prediction_ros.hpp" - -#include "map_based_prediction.hpp" - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace -{ -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} -} // namespace -namespace map_based_prediction -{ -MapBasedPredictionROS::MapBasedPredictionROS(const rclcpp::NodeOptions & node_options) -: Node("map_based_prediction", node_options), - interpolating_resolution_(0.5), - debug_accumulated_time_(0.0) -{ - [[maybe_unused]] auto ret = - rcutils_logging_set_logger_level(this->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); - tf_buffer_ptr_ = std::make_shared(clock); - tf_listener_ptr_ = std::make_shared(*tf_buffer_ptr_); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon", 10.0); - prediction_sampling_delta_time_ = declare_parameter("prediction_sampling_delta_time", 0.5); - min_velocity_for_map_based_prediction_ = - declare_parameter("min_velocity_for_map_based_prediction", 1.0); - dist_threshold_for_searching_lanelet_ = - declare_parameter("dist_threshold_for_searching_lanelet", 3.0); - delta_yaw_threshold_for_searching_lanelet_ = - declare_parameter("delta_yaw_threshold_for_searching_lanelet", 0.785); - sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset", 0.5); - sigma_yaw_angle_ = declare_parameter("sigma_yaw_angle", 5.0); - history_time_length_ = declare_parameter("history_time_length", 1.0); - dist_ratio_threshold_to_left_bound_ = - declare_parameter("dist_ratio_threshold_to_left_bound", -0.5); - dist_ratio_threshold_to_right_bound_ = - declare_parameter("dist_ratio_threshold_to_right_bound", 0.5); - diff_dist_threshold_to_left_bound_ = declare_parameter("diff_dist_threshold_to_left_bound", 0.29); - diff_dist_threshold_to_right_bound_ = - declare_parameter("diff_dist_threshold_to_right_bound", -0.29); - - map_based_prediction_ = std::make_shared( - interpolating_resolution_, prediction_time_horizon_, prediction_sampling_delta_time_); - - sub_objects_ = this->create_subscription( - "/perception/object_recognition/tracking/objects", 1, - std::bind(&MapBasedPredictionROS::objectsCallback, this, std::placeholders::_1)); - sub_map_ = this->create_subscription( - "/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MapBasedPredictionROS::mapCallback, this, std::placeholders::_1)); - - pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); - pub_markers_ = this->create_publisher( - "objects_path_markers", rclcpp::QoS{1}); -} - -double MapBasedPredictionROS::getObjectYaw(const TrackedObject & object) -{ - if (object.kinematics.orientation_availability) { - return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - } - - geometry_msgs::msg::Pose object_frame_pose; - geometry_msgs::msg::Pose map_frame_pose; - object_frame_pose.position.x = object.kinematics.twist_with_covariance.twist.linear.x * 0.1; - object_frame_pose.position.y = object.kinematics.twist_with_covariance.twist.linear.y * 0.1; - tf2::Transform tf_object2future; - tf2::Transform tf_map2object; - tf2::Transform tf_map2future; - - tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_map2object); - tf2::fromMsg(object_frame_pose, tf_object2future); - tf_map2future = tf_map2object * tf_object2future; - tf2::toMsg(tf_map2future, map_frame_pose); - double dx = map_frame_pose.position.x - object.kinematics.pose_with_covariance.pose.position.x; - double dy = map_frame_pose.position.y - object.kinematics.pose_with_covariance.pose.position.y; - return std::atan2(dy, dx); -} - -double MapBasedPredictionROS::calculateLikelihood( - const std::vector & path, const TrackedObject & object) -{ - // We compute the confidence value based on the object current position and angle - // Calculate path length - const double path_len = tier4_autoware_utils::calcArcLength(path); - const size_t nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( - path, object.kinematics.pose_with_covariance.pose.position); - const double l = tier4_autoware_utils::calcLongitudinalOffsetToSegment( - path, nearest_segment_idx, object.kinematics.pose_with_covariance.pose.position); - const double current_s_position = - tier4_autoware_utils::calcSignedArcLength(path, 0, nearest_segment_idx) + l; - // If the obstacle is ahead of this path, we assume the confidence for this path is 0 - if (current_s_position > path_len) { - return 0.0; - } - - // Euclid Lateral Distance - const double abs_d = std::fabs(tier4_autoware_utils::calcLateralOffset( - path, object.kinematics.pose_with_covariance.pose.position)); - - // Yaw Difference between obstacle and lane angle - const double lane_yaw = tf2::getYaw(path.at(nearest_segment_idx).orientation); - const double object_yaw = getObjectYaw(object); - const double delta_yaw = object_yaw - lane_yaw; - const double abs_norm_delta_yaw = std::fabs(tier4_autoware_utils::normalizeRadian(delta_yaw)); - - // Compute Chi-squared distributed (Equation (8) in the paper) - const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position - const double sigma_yaw = M_PI * sigma_yaw_angle_ / 180.0; // Standard Deviation for yaw angle - Eigen::Vector2d delta; - delta << abs_d, abs_norm_delta_yaw; - Eigen::Matrix2d P_inv; - P_inv << 1.0 / (sigma_d * sigma_d), 0.0, 0.0, 1.0 / (sigma_yaw * sigma_yaw); - const double MINIMUM_DISTANCE = 1e-6; - const double dist = std::max(delta.dot(P_inv * delta), MINIMUM_DISTANCE); - return 1.0 / dist; -} - -bool MapBasedPredictionROS::checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const lanelet::BasicPoint2d & search_point) -{ - // Step1. If we only have one point in the centerline, we will ignore the lanelet - if (lanelet.second.centerline().size() <= 1) { - return false; - } - - // Step2. Check if the obstacle is inside of this lanelet - if (!lanelet::geometry::inside(lanelet.second, search_point)) { - return false; - } - - // If the object is in the object buffer, we check if the target lanelet is - // inside the current lanelets id or following lanelets - const std::string object_id = toHexString(object.object_id); - if (object_buffer_.count(object_id) != 0) { - const std::vector & possible_lanelet = - object_buffer_.at(object_id).back().future_possible_lanelets; - - bool not_in_possible_lanelet = - std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) == - possible_lanelet.end(); - if (not_in_possible_lanelet) { - return false; - } - } - - // Step3. Calculate the angle difference between the lane angle and obstacle angle - double object_yaw = getObjectYaw(object); - const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet.second, object.kinematics.pose_with_covariance.pose.position); - const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = std::atan2(std::sin(delta_yaw), std::cos(delta_yaw)); - const double abs_norm_delta = std::fabs(normalized_delta_yaw); - - // Step4. Check if the closest lanelet is valid, and add all - // of the lanelets that are below max_dist and max_delta_yaw - if ( - lanelet.first < dist_threshold_for_searching_lanelet_ && - abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_) { - return true; - } - - return false; -} - -bool MapBasedPredictionROS::getClosestLanelets( - const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr_, - lanelet::ConstLanelets & closest_lanelets) -{ - std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); - - // obstacle point - lanelet::BasicPoint2d search_point( - object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y); - - // nearest lanelet - std::vector> surrounding_lanelets = - lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10); - - std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); - std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); - debug_accumulated_time_ += time.count() / (1000.0 * 1000.0); - - // No Closest Lanelets - if (surrounding_lanelets.empty()) { - return false; - } - - const double MIN_DIST = 1e-6; - bool found_target_closest_lanelet = false; - for (const auto & lanelet : surrounding_lanelets) { - // Check if the close lanelets meet the necessary condition for start lanelets - if (checkCloseLaneletCondition(lanelet, object, search_point)) { - // If the lanelet meets the condition, - // then check if similar lanelet is inside the closest lanelet - bool is_duplicate = false; - for (const auto & closest_lanelet : closest_lanelets) { - const auto lanelet_end_p = lanelet.second.centerline2d().back(); - const auto closest_lanelet_end_p = closest_lanelet.centerline2d().back(); - const double dist = std::hypot( - lanelet_end_p.x() - closest_lanelet_end_p.x(), - lanelet_end_p.y() - closest_lanelet_end_p.y()); - if (dist < MIN_DIST) { - is_duplicate = true; - break; - } - } - if (is_duplicate) { - continue; - } - - found_target_closest_lanelet = true; - closest_lanelets.push_back(lanelet.second); - } - } - - // If the closest lanelet is valid, return true - return found_target_closest_lanelet; -} - -void MapBasedPredictionROS::removeInvalidObject(const double current_time) -{ - std::vector invalid_object_id; - for (auto iter = object_buffer_.begin(); iter != object_buffer_.end(); ++iter) { - const std::string object_id = iter->first; - std::deque & object_data = iter->second; - - // If object data is empty, we are going to delete the buffer for the obstacle - if (object_data.empty()) { - invalid_object_id.push_back(object_id); - continue; - } - const double latest_object_time = rclcpp::Time(object_data.back().pose.header.stamp).seconds(); - - // Delete Old Objects - if (current_time - latest_object_time > 2.0) { - invalid_object_id.push_back(object_id); - continue; - } - - // Delete old information - while (!object_data.empty()) { - const double post_object_time = rclcpp::Time(object_data.front().pose.header.stamp).seconds(); - if (current_time - post_object_time > 2.0) { - // Delete Old Position - object_data.pop_front(); - } else { - break; - } - } - - if (object_data.empty()) { - invalid_object_id.push_back(object_id); - continue; - } - } - - for (const auto & key : invalid_object_id) { - object_buffer_.erase(key); - } -} - -bool MapBasedPredictionROS::updateObjectBuffer( - const std_msgs::msg::Header & header, const TrackedObject & object, - lanelet::ConstLanelets & current_lanelets) -{ - using Label = ObjectClassification; - // Ignore non-vehicle object - if ( - object.classification.front().label != Label::CAR && - object.classification.front().label != Label::BUS && - object.classification.front().label != Label::TRUCK) { - return false; - } - - // Get the current Lanelet - std::string object_id = toHexString(object.object_id); - - // Check whether the object is on the road - if (!getClosestLanelets(object, lanelet_map_ptr_, current_lanelets)) { - // This Object is not on the road - return false; - } - - // Get current Pose - geometry_msgs::msg::PoseStamped current_object_pose; - current_object_pose.header = header; - current_object_pose.pose = object.kinematics.pose_with_covariance.pose; - - // Update Object Buffer - if (object_buffer_.count(object_id) == 0) { - // New Object - ObjectData single_object_data; - single_object_data.current_lanelets = current_lanelets; - single_object_data.future_possible_lanelets = current_lanelets; - single_object_data.pose = current_object_pose; - const double object_yaw = getObjectYaw(object); - single_object_data.pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(object_yaw); - - std::deque object_data; - object_data.push_back(single_object_data); - - // Create new key, value pair in the buffer - object_buffer_.emplace(object_id, object_data); - } else { - // Object that is already in the object buffer - std::deque & object_data = object_buffer_.at(object_id); - - ObjectData single_object_data; - single_object_data.current_lanelets = current_lanelets; - single_object_data.future_possible_lanelets = current_lanelets; - single_object_data.pose = current_object_pose; - const double object_yaw = getObjectYaw(object); - single_object_data.pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(object_yaw); - - // push new object data - object_data.push_back(single_object_data); - } - - // If the obstacle is too slow, we do linear prediction - if ( - std::fabs(object.kinematics.twist_with_covariance.twist.linear.x) < - min_velocity_for_map_based_prediction_) { - return false; - } - - return true; -} - -void MapBasedPredictionROS::updatePossibleLanelets( - const std::string object_id, const lanelet::routing::LaneletPaths & paths) -{ - if (object_buffer_.count(object_id) != 0) { - std::vector & possible_lanelets = - object_buffer_.at(object_id).back().future_possible_lanelets; - for (const auto & path : paths) { - for (const auto & lanelet : path) { - bool not_in_buffer = - std::find(possible_lanelets.begin(), possible_lanelets.end(), lanelet) == - possible_lanelets.end(); - if (not_in_buffer) { - possible_lanelets.push_back(lanelet); - } - } - } - } -} - -void MapBasedPredictionROS::addValidPath( - const lanelet::routing::LaneletPaths & candidate_paths, - lanelet::routing::LaneletPaths & valid_paths) -{ - // Check if candidate paths are already in the valid paths - for (const auto & candidate_path : candidate_paths) { - bool already_searched = false; - for (const auto & valid_path : valid_paths) { - for (const auto & llt : valid_path) { - if (candidate_path.back().id() == llt.id()) { - already_searched = true; - } - } - } - if (!already_searched) { - valid_paths.push_back(candidate_path); - } - } -} - -double MapBasedPredictionROS::calcRightLateralOffset( - const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose) -{ - std::vector bound_path(bound_line.size()); - for (size_t i = 0; i < bound_path.size(); ++i) { - const double x = bound_line[i].x(); - const double y = bound_line[i].y(); - bound_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); - } - - return std::fabs(tier4_autoware_utils::calcLateralOffset(bound_path, search_pose.position)); -} - -double MapBasedPredictionROS::calcLeftLateralOffset( - const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose) -{ - return -calcRightLateralOffset(bound_line, search_pose); -} - -Maneuver MapBasedPredictionROS::detectLaneChange( - const TrackedObject & object, const lanelet::ConstLanelet & current_lanelet, - const double current_time) -{ - // Step1. Check if we have the object in the buffer - const std::string object_id = toHexString(object.object_id); - if (object_buffer_.count(object_id) == 0) { - return Maneuver::LANE_FOLLOW; - } - - // Object History Data - const std::deque object_info = object_buffer_.at(object_id); - - // Step2. Get the previous id - int prev_id = static_cast(object_info.size()) - 1; - while (prev_id >= 0) { - const double prev_time = rclcpp::Time(object_info.at(prev_id).pose.header.stamp).seconds(); - if (current_time - prev_time > history_time_length_) { - break; - } - --prev_id; - } - - if (prev_id < 0) { - return Maneuver::LANE_FOLLOW; - } - - // Step3. Get closest previous lanelet ID - const auto prev_pose = object_info.at(static_cast(prev_id)).pose; - const lanelet::ConstLanelets prev_lanelets = - object_info.at(static_cast(prev_id)).current_lanelets; - if (prev_lanelets.empty()) { - return Maneuver::LANE_FOLLOW; - } - lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); - double closest_prev_yaw = std::numeric_limits::max(); - for (const auto & lanelet : prev_lanelets) { - const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.pose.position); - const double delta_yaw = tf2::getYaw(prev_pose.pose.orientation) - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); - if (normalized_delta_yaw < closest_prev_yaw) { - closest_prev_yaw = normalized_delta_yaw; - prev_lanelet = lanelet; - } - } - - // Step4. Check if the vehicle has changed lane - const auto current_pose = object.kinematics.pose_with_covariance.pose; - const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose); - lanelet::routing::LaneletPaths possible_paths = - routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false); - bool has_lane_changed = true; - for (const auto & path : possible_paths) { - for (const auto & lanelet : path) { - if (lanelet == current_lanelet) { - has_lane_changed = false; - break; - } - } - } - - if (has_lane_changed) { - return Maneuver::LANE_FOLLOW; - } - - // Step5. Lane Change Detection - const lanelet::ConstLineString2d prev_left_bound = prev_lanelet.leftBound2d(); - const lanelet::ConstLineString2d prev_right_bound = prev_lanelet.rightBound2d(); - const lanelet::ConstLineString2d current_left_bound = current_lanelet.leftBound2d(); - const lanelet::ConstLineString2d current_right_bound = current_lanelet.rightBound2d(); - const double prev_left_dist = calcLeftLateralOffset(prev_left_bound, prev_pose.pose); - const double prev_right_dist = calcRightLateralOffset(prev_right_bound, prev_pose.pose); - const double current_left_dist = calcLeftLateralOffset(current_left_bound, current_pose); - const double current_right_dist = calcRightLateralOffset(current_right_bound, current_pose); - const double prev_lane_width = std::fabs(prev_left_dist) + std::fabs(prev_right_dist); - const double current_lane_width = std::fabs(current_left_dist) + std::fabs(current_right_dist); - if (prev_lane_width < 1e-3 || current_lane_width < 1e-3) { - RCLCPP_ERROR(get_logger(), "[Map Based Prediction]: Lane Width is too small"); - return Maneuver::LANE_FOLLOW; - } - - const double current_left_dist_ratio = current_left_dist / current_lane_width; - const double current_right_dist_ratio = current_right_dist / current_lane_width; - const double diff_left_current_prev = current_left_dist - prev_left_dist; - const double diff_right_current_prev = current_right_dist - prev_right_dist; - - if ( - current_left_dist_ratio > dist_ratio_threshold_to_left_bound_ && - diff_left_current_prev > diff_dist_threshold_to_left_bound_) { - return Maneuver::LEFT_LANE_CHANGE; - } else if ( - current_right_dist_ratio < dist_ratio_threshold_to_right_bound_ && - diff_right_current_prev < diff_dist_threshold_to_right_bound_) { - return Maneuver::RIGHT_LANE_CHANGE; - } - - return Maneuver::LANE_FOLLOW; -} - -void MapBasedPredictionROS::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) -{ - debug_accumulated_time_ = 0.0; - std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); - - if (!lanelet_map_ptr_) { - return; - } - - geometry_msgs::msg::TransformStamped world2map_transform; - geometry_msgs::msg::TransformStamped map2world_transform; - geometry_msgs::msg::TransformStamped debug_map2lidar_transform; - try { - world2map_transform = tf_buffer_ptr_->lookupTransform( - "map", // target - in_objects->header.frame_id, // src - in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); - map2world_transform = tf_buffer_ptr_->lookupTransform( - in_objects->header.frame_id, // target - "map", // src - in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); - debug_map2lidar_transform = tf_buffer_ptr_->lookupTransform( - "base_link", // target - "map", // src - rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(get_logger(), "%s", ex.what()); - return; - } - - /////////////////////////////////////////////////////////// - /////////////////// Update Object Buffer ////////////////// - ////////////////////////////////////////////////////////// - const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); - removeInvalidObject(objects_detected_time); - - ///////////////////////////////////////////////////////// - ///////////////////// Prediction /////////////////////// - /////////////////////////////////////////////////////// - PredictedObjects objects_without_map; - objects_without_map.header = in_objects->header; - DynamicObjectWithLanesArray prediction_input; - prediction_input.header = in_objects->header; - - for (const auto & object : in_objects->objects) { - std::string object_id = toHexString(object.object_id); - DynamicObjectWithLanes transformed_object; - transformed_object.object = object; - if (in_objects->header.frame_id != "map") { - geometry_msgs::msg::PoseStamped pose_in_map; - geometry_msgs::msg::PoseStamped pose_orig; - pose_orig.pose = object.kinematics.pose_with_covariance.pose; - tf2::doTransform(pose_orig, pose_in_map, world2map_transform); - transformed_object.object.kinematics.pose_with_covariance.pose = pose_in_map.pose; - } - - std_msgs::msg::Header transformed_header = in_objects->header; - transformed_header.frame_id = "map"; - lanelet::ConstLanelets start_lanelets; // current lanelet - if (!updateObjectBuffer(transformed_header, transformed_object.object, start_lanelets)) { - objects_without_map.objects.push_back( - map_based_prediction_->convertToPredictedObject(transformed_object.object)); - continue; - } - - // Obtain valid Paths - const double delta_horizon = 1.0; - const double obj_vel = object.kinematics.twist_with_covariance.twist.linear.x; - lanelet::routing::LaneletPaths paths; - for (const auto & start_lanelet : start_lanelets) { - // Step1. Lane Change Detection - // First: Right to Left Detection Result - // Second: Left to Right Detection Result - const Maneuver maneuver = detectLaneChange(object, start_lanelet, objects_detected_time); - - // Step2. Get the left lanelet - lanelet::routing::LaneletPaths left_paths; - auto opt_left = routing_graph_ptr_->left(start_lanelet); - if (!!opt_left) { - for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { - const double search_dist = horizon * obj_vel + 10.0; - lanelet::routing::LaneletPaths tmp_paths = - routing_graph_ptr_->possiblePaths(*opt_left, search_dist, 0, false); - addValidPath(tmp_paths, left_paths); - } - } - - // Step3. Get the right lanelet - lanelet::routing::LaneletPaths right_paths; - auto opt_right = routing_graph_ptr_->right(start_lanelet); - if (!!opt_right) { - for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { - const double search_dist = horizon * obj_vel + 10.0; - lanelet::routing::LaneletPaths tmp_paths = - routing_graph_ptr_->possiblePaths(*opt_right, search_dist, 0, false); - addValidPath(tmp_paths, right_paths); - } - } - - // Step4. Get the centerline - lanelet::routing::LaneletPaths center_paths; - for (double horizon = prediction_time_horizon_; horizon > 0; horizon -= delta_horizon) { - const double search_dist = horizon * obj_vel + 10.0; - lanelet::routing::LaneletPaths tmp_paths = - routing_graph_ptr_->possiblePaths(start_lanelet, search_dist, 0, false); - addValidPath(tmp_paths, center_paths); - } - - // Step5. Insert Valid Paths - if (!left_paths.empty() && maneuver == Maneuver::LEFT_LANE_CHANGE) { - paths.insert(paths.end(), left_paths.begin(), left_paths.end()); - } else if (!right_paths.empty() && maneuver == Maneuver::RIGHT_LANE_CHANGE) { - paths.insert(paths.end(), right_paths.begin(), right_paths.end()); - } else { - paths.insert(paths.end(), center_paths.begin(), center_paths.end()); - } - } - // If there is no valid path, we'll mark this object as map-less object - if (paths.empty()) { - objects_without_map.objects.push_back( - map_based_prediction_->convertToPredictedObject(transformed_object.object)); - continue; - } - - // Update Possible lanelet in the object buffer - updatePossibleLanelets(object_id, paths); - - std::vector> tmp_paths; - std::vector tmp_confidence; - for (const auto & path : paths) { - std::vector tmp_path; - - // Insert Positions. Note that we insert points from previous lanelet - if (!path.empty()) { - lanelet::ConstLanelets prev_lanelets = routing_graph_ptr_->previous(path.front()); - if (!prev_lanelets.empty()) { - lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); - for (const auto & point : prev_lanelet.centerline()) { - geometry_msgs::msg::Pose tmp_pose; - tmp_pose.position.x = point.x(); - tmp_pose.position.y = point.y(); - tmp_pose.position.z = point.z(); - tmp_path.push_back(tmp_pose); - } - } - } - - for (const auto & lanelet : path) { - for (const auto & point : lanelet.centerline()) { - geometry_msgs::msg::Pose tmp_pose; - tmp_pose.position.x = point.x(); - tmp_pose.position.y = point.y(); - tmp_pose.position.z = point.z(); - - // Prevent from inserting same points - if (!tmp_path.empty()) { - const auto prev_pose = tmp_path.back(); - const double tmp_dist = tier4_autoware_utils::calcDistance2d(prev_pose, tmp_pose); - if (tmp_dist < 1e-6) { - continue; - } - } - - tmp_path.push_back(tmp_pose); - } - } - - if (tmp_path.size() < 2) { - continue; - } - - // Compute yaw angles - for (size_t pose_id = 0; pose_id < tmp_path.size() - 1; ++pose_id) { - double tmp_yaw = std::atan2( - tmp_path.at(pose_id + 1).position.y - tmp_path.at(pose_id).position.y, - tmp_path.at(pose_id + 1).position.x - tmp_path.at(pose_id).position.x); - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, tmp_yaw); - tmp_path.at(pose_id).orientation = tf2::toMsg(quat); - } - tmp_path.back().orientation = tmp_path.at(tmp_path.size() - 2).orientation; - - ////////////////////////////////////////////////////////////////////// - // Calculate Confidence of each path(centerline) for this obstacle // - //////////////////////////////////////////////////////////////////// - const double confidence = calculateLikelihood(tmp_path, transformed_object.object); - // Ignore a path that has too low confidence - if (confidence < 1e-6) { - continue; - } - - tmp_paths.push_back(tmp_path); - tmp_confidence.push_back(confidence); - } - - transformed_object.lanes = tmp_paths; - transformed_object.confidence = tmp_confidence; - prediction_input.objects.push_back(transformed_object); - } - - std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); - std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); - - std::vector out_objects_in_map; - map_based_prediction_->doPrediction(prediction_input, out_objects_in_map); - PredictedObjects output; - output.header = in_objects->header; - output.header.frame_id = "map"; - output.objects = out_objects_in_map; - - std::vector out_objects_without_map; - map_based_prediction_->doLinearPrediction(objects_without_map, out_objects_without_map); - output.objects.insert( - output.objects.begin(), out_objects_without_map.begin(), out_objects_without_map.end()); - pub_objects_->publish(output); -} - -void MapBasedPredictionROS::mapCallback(const HADMapBin::ConstSharedPtr msg) -{ - RCLCPP_INFO(get_logger(), "Start loading lanelet"); - lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg( - *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - RCLCPP_INFO(get_logger(), "Map is loaded"); -} -} // namespace map_based_prediction - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(map_based_prediction::MapBasedPredictionROS) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp new file mode 100644 index 0000000000000..9f28f113b194b --- /dev/null +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -0,0 +1,305 @@ +// 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 "map_based_prediction/path_generator.hpp" + +#include +#include + +#include + +namespace map_based_prediction +{ +PathGenerator::PathGenerator(const double time_horizon, const double sampling_time_interval) +: time_horizon_(time_horizon), sampling_time_interval_(sampling_time_interval) +{ +} + +PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) +{ + return generateStraightPath(object); +} + +PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject & object) const +{ + PredictedPath path; + path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); + const double ep = 0.001; + for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + path.path.push_back(object.kinematics.pose_with_covariance.pose); + } + + return path; +} + +PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) +{ + return generateStraightPath(object); +} + +PredictedPath PathGenerator::generatePathForOnLaneVehicle( + const TrackedObject & object, const PosePath & ref_paths) +{ + if (ref_paths.size() < 2) { + const PredictedPath empty_path; + return empty_path; + } + + return generatePolynomialPath(object, ref_paths); +} + +PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const +{ + const auto & object_pose = object.kinematics.pose_with_covariance.pose; + const auto & object_twist = object.kinematics.twist_with_covariance.twist; + const double ep = 0.001; + const double duration = time_horizon_ + ep; + + PredictedPath path; + path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); + path.path.reserve(static_cast((duration) / sampling_time_interval_)); + for (double dt = 0.0; dt < duration; dt += sampling_time_interval_) { + const auto future_obj_pose = tier4_autoware_utils::calcOffsetPose( + object_pose, object_twist.linear.x * dt, object_twist.linear.y * dt, 0.0); + path.path.push_back(future_obj_pose); + } + + return path; +} + +PredictedPath PathGenerator::generatePolynomialPath( + const TrackedObject & object, const PosePath & ref_path) +{ + // Get current Frenet Point + const double ref_path_len = tier4_autoware_utils::calcArcLength(ref_path); + const auto current_point = getFrenetPoint(object, ref_path); + + // Step1. Set Target Frenet Point + // Note that we do not set position s, + // since we don't know the target longitudinal position + FrenetPoint terminal_point; + terminal_point.s_vel = current_point.s_vel; + terminal_point.s_acc = 0.0; + terminal_point.d = 0.0; + terminal_point.d_vel = 0.0; + terminal_point.d_acc = 0.0; + + // Step2. Generate Predicted Path on a Frenet coordinate + const auto frenet_predicted_path = + generateFrenetPath(current_point, terminal_point, ref_path_len); + + // Step3. Interpolate Reference Path for converting predicted path coordinate + const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); + + if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) { + const PredictedPath empty_path; + return empty_path; + } + + // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate + return convertToPredictedPath(object, frenet_predicted_path, interpolated_ref_path); +} + +FrenetPath PathGenerator::generateFrenetPath( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length) +{ + FrenetPath path; + const double duration = time_horizon_; + + // Compute Lateral and Longitudinal Coefficients to generate the trajectory + const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration); + const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration); + + path.reserve(static_cast(duration / sampling_time_interval_)); + for (double t = 0.0; t <= duration; t += sampling_time_interval_) { + const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) + + lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) + + lat_coeff(2) * std::pow(t, 5); + const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) + + lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4); + if (s_next > max_length) { + break; + } + + // We assume the object is traveling at a constant speed along s direction + FrenetPoint point; + point.s = std::max(s_next, 0.0); + point.s_vel = current_point.s_vel; + point.s_acc = current_point.s_acc; + point.d = d_next; + point.d_vel = current_point.d_vel; + point.d_acc = current_point.d_acc; + path.push_back(point); + } + + return path; +} + +Eigen::Vector3d PathGenerator::calcLatCoefficients( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) +{ + // Lateral Path Calculation + // Quintic polynomial for d + // A = np.array([[T**3, T**4, T**5], + // [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], + // [6 * T, 12 * T ** 2, 20 * T ** 3]]) + // A_inv = np.matrix([[10/(T**3), -4/(T**2), 1/(2*T)], + // [-15/(T**4), 7/(T**3), -1/(T**2)], + // [6/(T**5), -3/(T**4), 1/(2*T**3)]]) + // b = np.matrix([[xe - self.a0 - self.a1 * T - self.a2 * T**2], + // [vxe - self.a1 - 2 * self.a2 * T], + // [axe - 2 * self.a2]]) + Eigen::Matrix3d A_lat_inv; + A_lat_inv << 10 / std::pow(T, 3), -4 / std::pow(T, 2), 1 / (2 * T), -15 / std::pow(T, 4), + 7 / std::pow(T, 3), -1 / std::pow(T, 2), 6 / std::pow(T, 5), -3 / std::pow(T, 4), + 1 / (2 * std::pow(T, 3)); + Eigen::Vector3d b_lat; + b_lat[0] = target_point.d - current_point.d - current_point.d_vel * T; + b_lat[1] = target_point.d_vel - current_point.d_vel; + b_lat[2] = target_point.d_acc; + + return A_lat_inv * b_lat; +} + +Eigen::Vector2d PathGenerator::calcLonCoefficients( + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) +{ + // Longitudinal Path Calculation + // Quadric polynomial + // A_inv = np.matrix([[1/(T**2), -1/(3*T)], + // [-1/(2*T**3), 1/(4*T**2)]]) + // b = np.matrix([[vxe - self.a1 - 2 * self.a2 * T], + // [axe - 2 * self.a2]]) + Eigen::Matrix2d A_lon_inv; + A_lon_inv << 1 / std::pow(T, 2), -1 / (3 * T), -1 / (2 * std::pow(T, 3)), + 1 / (4 * std::pow(T, 2)); + Eigen::Vector2d b_lon; + b_lon[0] = target_point.s_vel - current_point.s_vel; + b_lon[1] = 0.0; + return A_lon_inv * b_lon; +} + +PosePath PathGenerator::interpolateReferencePath( + const PosePath & base_path, const FrenetPath & frenet_predicted_path) +{ + PosePath interpolated_path; + const size_t interpolate_num = frenet_predicted_path.size(); + if (interpolate_num < 2) { + interpolated_path.emplace_back(base_path.front()); + return interpolated_path; + } + + std::vector base_path_x; + std::vector base_path_y; + std::vector base_path_z; + std::vector base_path_s; + for (size_t i = 0; i < base_path.size(); ++i) { + base_path_x.push_back(base_path.at(i).position.x); + base_path_y.push_back(base_path.at(i).position.y); + base_path_z.push_back(base_path.at(i).position.z); + base_path_s.push_back(tier4_autoware_utils::calcSignedArcLength(base_path, 0, i)); + } + + std::vector resampled_s(frenet_predicted_path.size()); + for (size_t i = 0; i < frenet_predicted_path.size(); ++i) { + resampled_s.at(i) = frenet_predicted_path.at(i).s; + } + if (resampled_s.front() > resampled_s.back()) { + std::reverse(resampled_s.begin(), resampled_s.end()); + } + + // Spline Interpolation + std::vector slerp_ref_path_x = + interpolation::slerp(base_path_s, base_path_x, resampled_s); + std::vector slerp_ref_path_y = + interpolation::slerp(base_path_s, base_path_y, resampled_s); + std::vector slerp_ref_path_z = + interpolation::slerp(base_path_s, base_path_z, resampled_s); + + interpolated_path.resize(interpolate_num); + for (size_t i = 0; i < interpolate_num - 1; ++i) { + geometry_msgs::msg::Pose interpolated_pose; + const auto current_point = + tier4_autoware_utils::createPoint(slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), 0.0); + const auto next_point = tier4_autoware_utils::createPoint( + slerp_ref_path_x.at(i + 1), slerp_ref_path_y.at(i + 1), 0.0); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point); + interpolated_pose.position = tier4_autoware_utils::createPoint( + slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), slerp_ref_path_z.at(i)); + interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + interpolated_path.at(i) = interpolated_pose; + } + interpolated_path.back().position = tier4_autoware_utils::createPoint( + slerp_ref_path_x.back(), slerp_ref_path_y.back(), slerp_ref_path_z.back()); + interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; + + return interpolated_path; +} + +PredictedPath PathGenerator::convertToPredictedPath( + const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path) +{ + PredictedPath predicted_path; + predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); + predicted_path.path.resize(ref_path.size()); + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + // Reference Point from interpolated reference path + const auto & ref_pose = ref_path.at(i); + + // Frenet Point from frenet predicted path + const auto & frenet_point = frenet_predicted_path.at(i); + + // Converted Pose + auto predicted_pose = tier4_autoware_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); + predicted_pose.position.z = object.kinematics.pose_with_covariance.pose.position.z; + if (i == 0) { + predicted_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; + } else { + const double yaw = tier4_autoware_utils::calcAzimuthAngle( + predicted_path.path.at(i - 1).position, predicted_pose.position); + predicted_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + } + predicted_path.path.at(i) = predicted_pose; + } + + return predicted_path; +} + +FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const PosePath & ref_path) +{ + FrenetPoint frenet_point; + const auto obj_point = object.kinematics.pose_with_covariance.pose.position; + + const size_t nearest_segment_idx = + tier4_autoware_utils::findNearestSegmentIndex(ref_path, obj_point); + const double l = + tier4_autoware_utils::calcLongitudinalOffsetToSegment(ref_path, nearest_segment_idx, obj_point); + const float vx = static_cast(object.kinematics.twist_with_covariance.twist.linear.x); + const float vy = static_cast(object.kinematics.twist_with_covariance.twist.linear.y); + const float obj_yaw = + static_cast(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + const float lane_yaw = + static_cast(tf2::getYaw(ref_path.at(nearest_segment_idx).orientation)); + const float delta_yaw = obj_yaw - lane_yaw; + + frenet_point.s = tier4_autoware_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; + frenet_point.d = tier4_autoware_utils::calcLateralOffset(ref_path, obj_point); + frenet_point.s_vel = vx * std::cos(delta_yaw) - vy * std::sin(delta_yaw); + frenet_point.d_vel = vx * std::sin(delta_yaw) + vy * std::cos(delta_yaw); + frenet_point.s_acc = 0.0; + frenet_point.d_acc = 0.0; + + return frenet_point; +} +} // namespace map_based_prediction diff --git a/perception/multi_object_tracker/config/data_association_matrix.param.yaml b/perception/multi_object_tracker/config/data_association_matrix.param.yaml index 393bb949d048d..6e7ac3e9efe81 100644 --- a/perception/multi_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/multi_object_tracker/config/data_association_matrix.param.yaml @@ -1,52 +1,64 @@ /**: ros__parameters: can_assign_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN - 1, 1, 1, 1, 0, 0, 0, 0, #CAR - 1, 1, 1, 1, 0, 0, 0, 0, #TRUCK - 1, 1, 1, 1, 0, 0, 0, 0, #BUS - 0, 0, 0, 0, 1, 1, 1, 0, #BICYCLE - 0, 0, 0, 0, 1, 1, 1, 0, #MOTORBIKE - 0, 0, 0, 0, 1, 1, 1, 0, #PEDESTRIAN - 0, 0, 0, 0, 0, 0, 0, 1] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 1, 1, 1, 1, 1, 0, 0, 0, #CAR + 1, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 1, 1, 1, 1, 1, 0, 0, 0, #BUS + 1, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + max_dist_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN - 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #CAR - 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #TRUCK - 4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #BUS - 3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #BICYCLE - 3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #MOTORBIKE - 2.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #PEDESTRIAN - 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN max_area_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN - 12.10, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #CAR - 19.75, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #TRUCK - 32.40, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #BUS - 2.50, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #BICYCLE - 3.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #MOTORBIKE - 2.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00, 10000.00, #PEDESTRIAN - 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN + 12.10, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #CAR + 19.75, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #TRUCK + 32.40, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #BUS + 32.40, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #TRAILER + 3.00, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN min_area_matrix: - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN - 3.600, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #CAR - 6.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #TRUCK - 10.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #BUS - 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #BICYCLE - 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #MOTORBIKE - 0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #PEDESTRIAN - 0.500, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR + 6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN max_rad_matrix: # If value is greater than pi, it will be ignored. - #UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL - [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN - 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #CAR - 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #TRUCK - 3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #BUS - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #PEDESTRIAN - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #ANIMAL + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + min_iou_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index 23feca474fe4f..a29fcdaa4e1e5 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp @@ -41,6 +41,7 @@ class DataAssociation Eigen::MatrixXd max_area_matrix_; Eigen::MatrixXd min_area_matrix_; Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; const double score_threshold_; std::unique_ptr gnn_solver_ptr_; @@ -49,7 +50,7 @@ class DataAssociation DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, - std::vector max_rad_vector); + std::vector max_rad_vector, std::vector min_iou_vector); void assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 00ba413700901..58ff17c0c9d68 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -28,6 +28,7 @@ #include #include +#include #include namespace utils @@ -36,6 +37,11 @@ double getPolygonArea(const geometry_msgs::msg::Polygon & footprint); double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions); double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); +double get2dIoU( + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object1, + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object2); double get2dIoU( const autoware_auto_perception_msgs::msg::TrackedObject & object1, const autoware_auto_perception_msgs::msg::TrackedObject & object2); diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 42923a59c7f0b..4d3d83e20f3c0 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -79,7 +79,7 @@ double getFormedYawAngle( DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, - std::vector max_rad_vector) + std::vector max_rad_vector, std::vector min_iou_vector) : score_threshold_(0.01) { { @@ -112,6 +112,12 @@ DataAssociation::DataAssociation( max_rad_vector.data(), max_rad_label_num, max_rad_label_num); max_rad_matrix_ = max_rad_matrix_tmp.transpose(); } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } gnn_solver_ptr_ = std::make_unique(); } @@ -167,42 +173,57 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; - const geometry_msgs::msg::PoseWithCovariance tracker_pose_covariance = - (*tracker_itr)->getPoseWithCovariance(measurements.header.stamp); if (can_assign_matrix_(tracker_label, measurement_label)) { + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); + const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double max_area = max_area_matrix_(tracker_label, measurement_label); - const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double max_rad = max_rad_matrix_(tracker_label, measurement_label); const double dist = getDistance( measurement_object.kinematics.pose_with_covariance.pose.position, - tracker_pose_covariance.pose.position); - const double area = utils::getArea(measurement_object.shape); - score = (max_dist - std::min(dist, max_dist)) / max_dist; + tracked_object.kinematics.pose_with_covariance.pose.position); + bool passed_gate = true; // dist gate - if (max_dist < dist) { - score = 0.0; - // area gate - } else if (area < min_area || max_area < area) { - score = 0.0; - // angle gate - } else if (std::fabs(max_rad) < M_PI) { + if (passed_gate) { + if (max_dist < dist) passed_gate = false; + } + // area gate + if (passed_gate) { + const double max_area = max_area_matrix_(tracker_label, measurement_label); + const double min_area = min_area_matrix_(tracker_label, measurement_label); + const double area = utils::getArea(measurement_object.shape); + if (area < min_area || max_area < area) passed_gate = false; + } + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(tracker_label, measurement_label); const double angle = getFormedYawAngle( measurement_object.kinematics.pose_with_covariance.pose.orientation, - tracker_pose_covariance.pose.orientation, false); - if (std::fabs(max_rad) < std::fabs(angle)) { - score = 0.0; - } - // mahalanobis dist gate - } else if (score < score_threshold_) { - double mahalanobis_dist = getMahalanobisDistance( - measurements.objects.at(measurement_idx).kinematics.pose_with_covariance.pose.position, - tracker_pose_covariance.pose.position, getXYCovariance(tracker_pose_covariance)); - - if (2.448 /*95%*/ <= mahalanobis_dist) { - score = 0.0; - } + tracked_object.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) + passed_gate = false; + } + // mahalanobis dist gate + if (passed_gate) { + const double mahalanobis_dist = getMahalanobisDistance( + measurement_object.kinematics.pose_with_covariance.pose.position, + tracked_object.kinematics.pose_with_covariance.pose.position, + getXYCovariance(tracked_object.kinematics.pose_with_covariance)); + if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false; + } + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(tracker_label, measurement_label); + const double iou = utils::get2dIoU( + {measurement_object.kinematics.pose_with_covariance.pose, measurement_object.shape}, + {tracked_object.kinematics.pose_with_covariance.pose, tracked_object.shape}); + if (iou < min_iou) passed_gate = false; + } + + // all gate is passed + if (passed_gate) { + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; } } score_matrix(tracker_idx, measurement_idx) = score; diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 6ec99f44fa6ce..d5c2946843365 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -194,9 +194,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); + const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); data_association_ = std::make_unique( - can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix); + can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, + min_iou_matrix); } void MultiObjectTracker::onMeasurement( diff --git a/perception/multi_object_tracker/src/utils/utils.cpp b/perception/multi_object_tracker/src/utils/utils.cpp index 79dfeb92359ea..556ffb8fcdc5f 100644 --- a/perception/multi_object_tracker/src/utils/utils.cpp +++ b/perception/multi_object_tracker/src/utils/utils.cpp @@ -11,10 +11,6 @@ // 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. -// -// -// Author: v1.0 Yukihiro Saito -// #include "multi_object_tracker/utils/utils.hpp" @@ -29,7 +25,7 @@ namespace utils { void toPolygon2d( - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, tier4_autoware_utils::Polygon2d & output); bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon); tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon); @@ -51,8 +47,8 @@ double getPolygonArea(const geometry_msgs::msg::Polygon & footprint) { double area = 0.0; - for (int i = 0; i < static_cast(footprint.points.size()); ++i) { - int j = (i + 1) % static_cast(footprint.points.size()); + for (size_t i = 0; i < footprint.points.size(); ++i) { + size_t j = (i + 1) % footprint.points.size(); area += 0.5 * (footprint.points.at(i).x * footprint.points.at(j).y - footprint.points.at(j).x * footprint.points.at(i).y); } @@ -71,12 +67,14 @@ double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) } double get2dIoU( - const autoware_auto_perception_msgs::msg::TrackedObject & object1, - const autoware_auto_perception_msgs::msg::TrackedObject & object2) + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object1, + const std::tuple< + const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object2) { tier4_autoware_utils::Polygon2d polygon1, polygon2; - toPolygon2d(object1, polygon1); - toPolygon2d(object2, polygon2); + toPolygon2d(std::get<0>(object1), std::get<1>(object1), polygon1); + toPolygon2d(std::get<0>(object2), std::get<1>(object2), polygon2); std::vector union_polygons; std::vector intersection_polygons; @@ -91,14 +89,19 @@ double get2dIoU( for (const auto & intersection_polygon : intersection_polygons) { intersection_area += boost::geometry::area(intersection_polygon); } - double iou; - if (union_area < 0.01) { - iou = 0.0f; - } - iou = std::min(1.0, intersection_area / union_area); + const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area); return iou; } +double get2dIoU( + const autoware_auto_perception_msgs::msg::TrackedObject & object1, + const autoware_auto_perception_msgs::msg::TrackedObject & object2) +{ + return get2dIoU( + {object1.kinematics.pose_with_covariance.pose, object1.shape}, + {object2.kinematics.pose_with_covariance.pose, object2.shape}); +} + tier4_autoware_utils::Polygon2d inverseClockWise(const tier4_autoware_utils::Polygon2d & polygon) { tier4_autoware_utils::Polygon2d inverted_polygon; @@ -124,23 +127,18 @@ bool isClockWise(const tier4_autoware_utils::Polygon2d & polygon) } void toPolygon2d( - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, tier4_autoware_utils::Polygon2d & output) { - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - const auto & pose = object.kinematics.pose_with_covariance.pose; - double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); Eigen::Matrix2d rotation; rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); Eigen::Vector2d offset0, offset1, offset2, offset3; - offset0 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - offset1 = rotation * - Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); - offset2 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); - offset3 = rotation * - Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + offset0 = rotation * Eigen::Vector2d(shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); + offset1 = rotation * Eigen::Vector2d(shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); + offset2 = rotation * Eigen::Vector2d(-shape.dimensions.x * 0.5f, -shape.dimensions.y * 0.5f); + offset3 = rotation * Eigen::Vector2d(-shape.dimensions.x * 0.5f, shape.dimensions.y * 0.5f); output.outer().push_back(boost::geometry::make( pose.position.x + offset0.x(), pose.position.y + offset0.y())); output.outer().push_back(boost::geometry::make( @@ -150,9 +148,9 @@ void toPolygon2d( output.outer().push_back(boost::geometry::make( pose.position.x + offset3.x(), pose.position.y + offset3.y())); output.outer().push_back(output.outer().front()); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - const auto & center = object.kinematics.pose_with_covariance.pose.position; - const auto & radius = object.shape.dimensions.x * 0.5; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + const auto & center = pose.position; + const auto & radius = shape.dimensions.x * 0.5; constexpr int n = 6; for (int i = 0; i < n; ++i) { Eigen::Vector2d point; @@ -170,13 +168,8 @@ void toPolygon2d( boost::geometry::make(point.x(), point.y())); } output.outer().push_back(output.outer().front()); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - const auto & pose = object.kinematics.pose_with_covariance.pose; - // don't use yaw - // double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); - // Eigen::Matrix2d rotation; - // rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); - for (const auto & point : object.shape.footprint.points) { + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + for (const auto & point : shape.footprint.points) { output.outer().push_back(boost::geometry::make( pose.position.x + point.x, pose.position.y + point.y)); } diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 927793f713433..824f18706fcfe 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -220,10 +220,8 @@ target_link_libraries(scene_module_virtual_traffic_light scene_module_lib) # SceneModule OcclusionSpot ament_auto_add_library(scene_module_occlusion_spot SHARED src/scene_module/occlusion_spot/grid_utils.cpp - src/scene_module/occlusion_spot/geometry.cpp src/scene_module/occlusion_spot/manager.cpp src/scene_module/occlusion_spot/debug.cpp - src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp src/scene_module/occlusion_spot/scene_occlusion_spot.cpp src/scene_module/occlusion_spot/occlusion_spot_utils.cpp src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -313,7 +311,6 @@ if(BUILD_TESTING) ament_add_gtest(occlusion_spot-test test/src/test_occlusion_spot_utils.cpp test/src/test_risk_predictive_braking.cpp - test/src/test_geometry.cpp test/src/test_grid_utils.cpp ) target_link_libraries(occlusion_spot-test diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 5784cce657e0d..71bcf2694fa0d 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,7 +1,8 @@ /**: ros__parameters: occlusion_spot: - method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" + detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" + pass_judge: "current_velocity" # [-] candidate is "current_velocity"" debug: false # [-] whether to publish debug markers. Note Default should be false for performance use_partition_lanelet: true # [-] whether to use partition lanelet map data pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp deleted file mode 100644 index e022df839bccb..0000000000000 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// 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 SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ - -#include - -#include - -#include -#include - -#include - -namespace behavior_velocity_planner -{ -namespace occlusion_spot_utils -{ -namespace bg = boost::geometry; - -//!< @brief build slices all along the trajectory -// using the given range and desired slice length and width -void buildSlices( - std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const double offset, - const bool is_on_right, const PlannerParam & param); -//!< @brief build detection_area slice from path -void buildDetectionAreaPolygon( - std::vector & slices, const PathWithLaneId & path, const double offset, - const PlannerParam & param); -//!< @brief calculate interpolation between a and b at distance ratio t -template -T lerp(T a, T b, double t) -{ - return a + t * (b - a); -} - -} // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner - -#endif // SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp index 8d7edf060cf6f..b7f9df3371b66 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include @@ -67,7 +68,7 @@ bool isOcclusionSpotSquare( //!< @brief Find all occlusion spots inside the given lanelet void findOcclusionSpots( std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, - const lanelet::BasicPolygon2d & polygon, const double min_size); + const Polygon2d & polygon, const double min_size); //!< @brief Return true if the path between the two given points is free of occupied cells bool isCollisionFree( const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp index 6464226c788b0..831dca20f1655 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include @@ -49,6 +48,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface using PlannerParam = occlusion_spot_utils::PlannerParam; PlannerParam planner_param_; + int64_t module_id_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 65547fd0d763a..0146947be2039 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include @@ -42,6 +42,16 @@ #include #include +namespace tier4_autoware_utils +{ +template <> +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose; +} +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -67,7 +77,8 @@ using BasicPolygons2d = std::vector; namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; -enum METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT }; +enum DETECTION_METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT }; +enum PASS_JUDGE { SMOOTH_VELOCITY, CURRENT_VELOCITY }; struct DetectionArea { @@ -99,7 +110,8 @@ struct LatLon struct PlannerParam { - METHOD method; + DETECTION_METHOD detection_method; + PASS_JUDGE pass_judge; bool debug; // [-] bool use_partition_lanelet; // [-] // parameters in yaml @@ -127,22 +139,6 @@ struct SafeMotion double safe_velocity; }; -// @brief represent the range of a each polygon -struct SliceRange -{ - double min_length{}; - double max_length{}; - double min_distance{}; - double max_distance{}; -}; - -// @brief representation of a polygon along a path -struct Slice -{ - SliceRange range{}; - lanelet::BasicPolygon2d polygon{}; -}; - struct ObstacleInfo { SafeMotion safe_motion; // safe motion of velocity and stop point @@ -185,7 +181,7 @@ struct DebugData double z; std::string road_type = ""; std::string detection_type = ""; - std::vector detection_area_polygons; + Polygons2d detection_area_polygons; std::vector partition_lanelets; std::vector parked_vehicle_point; std::vector possible_collisions; @@ -200,22 +196,26 @@ struct DebugData occlusion_points.clear(); } }; - +// apply current velocity to path +PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0); +//!< @brief wrapper for detection area polygon generation +bool buildDetectionAreaPolygon( + Polygons2d & slices, const PathWithLaneId & path, const double offset, + const PlannerParam & param); lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); // Note : consider offset_from_start_to_ego and safety margin for collision here void handleCollisionOffset(std::vector & possible_collisions, double offset); void clipPathByLength( const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0); bool isStuckVehicle(PredictedObject obj, const double min_vel); -double offsetFromStartToEgo( - const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx); std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const std::vector & polys); + std::vector & objs, const Polygons2d & polys); std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point); -std::vector generatePossibleCollisionBehindParkedVehicle( - const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, +bool generatePossibleCollisionBehindParkedVehicle( + std::vector & possible_collisions, const PathWithLaneId & path, + const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects); ROAD_TYPE getCurrentRoadType( const lanelet::ConstLanelet & current_lanelet, const LaneletMapPtr & lanelet_map_ptr); @@ -236,10 +236,10 @@ void calcSlowDownPointsForPossibleCollision( //!< @brief convert a set of occlusion spots found on detection_area slice boost::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, - const double offset_from_start_to_ego, const BasicPoint2d base_point, + const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data); //!< @brief generate possible collisions coming from occlusion spots on the side of the path -void createPossibleCollisionsInDetectionArea( +bool createPossibleCollisionsInDetectionArea( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp index 9156d5d04ce07..3be1bf9c5f0fd 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp @@ -34,15 +34,6 @@ int insertSafeVelocityToPath( const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, PathWithLaneId * inout_path); -/** - * - * @param: longitudinal_distance: longitudinal distance to collision - * @param: param: planner param - * @return lateral distance - **/ -double calculateLateralDistanceFromTTC( - const double longitudinal_distance, const PlannerParam & param); - /** * @param: v: ego velocity config * @param: ttc: time to collision diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp deleted file mode 100644 index b194a4fb83d91..0000000000000 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// 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 SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include - -namespace behavior_velocity_planner -{ -class OcclusionSpotInPublicModule : public SceneModuleInterface -{ - using PlannerParam = occlusion_spot_utils::PlannerParam; - using DebugData = occlusion_spot_utils::DebugData; - -public: - OcclusionSpotInPublicModule( - const int64_t module_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); - - /** - * @brief plan occlusion spot velocity - */ - bool modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) override; - - visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - -private: - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects_array_; - - // Parameter - PlannerParam param_; - -protected: - int64_t module_id_; - - // Debug - mutable DebugData debug_data_; -}; -} // namespace behavior_velocity_planner - -#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index cd7b029cf1651..6632127a9e98e 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -63,18 +64,31 @@ struct SearchRangeIndex size_t min_idx; size_t max_idx; }; +struct DetectionRange +{ + bool use_right = true; + bool use_left = true; + double interval; + double min_longitudinal_distance; + double max_longitudinal_distance; + double min_lateral_distance; + double max_lateral_distance; +}; struct PointWithSearchRangeIndex { geometry_msgs::msg::Point point; SearchRangeIndex index; }; -using autoware_auto_planning_msgs::msg::PathWithLaneId; + using Point2d = boost::geometry::model::d2::point_xy; -using Polygons2d = std::vector; +using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using BasicPolygons2d = std::vector; +using Polygons2d = std::vector; namespace planning_utils { +using geometry_msgs::msg::Pose; inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; } inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Pose & p) { return p.position; } inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseStamped & p) @@ -110,7 +124,15 @@ inline geometry_msgs::msg::Pose getPose( { return traj.points.at(idx).pose; } -void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys); + +// create detection area from given range return false if creation failure +bool createDetectionAreaPolygons( + Polygons2d & slices, const PathWithLaneId & path, const DetectionRange da_range, + const double obstacle_vel_mps); +PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio); +Point2d calculateLateralOffsetPoint2d(const Pose & p, const double offset); + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys); void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input); void insertVelocity( PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index cb435400f1feb..b0749792956fd 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -29,7 +28,6 @@ namespace { using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; -using Slices = std::vector; visualization_msgs::msg::Marker makeArrowMarker( const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id) @@ -173,7 +171,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( } visualization_msgs::msg::MarkerArray makeSlicePolygonMarker( - const Slices & slices, const std::string ns, const int id, const double z) + const Polygons2d & slices, const std::string ns, const int id, const double z) { visualization_msgs::msg::MarkerArray debug_markers; visualization_msgs::msg::Marker debug_marker; @@ -189,7 +187,7 @@ visualization_msgs::msg::MarkerArray makeSlicePolygonMarker( debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); debug_marker.ns = ns; for (const auto & slice : slices) { - for (const auto & p : slice.polygon) { + for (const auto & p : slice.outer()) { geometry_msgs::msg::Point point = tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); debug_marker.points.push_back(point); @@ -283,24 +281,6 @@ visualization_msgs::msg::MarkerArray createOcclusionMarkerArray( } } // namespace -visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMarkerArray() -{ - const auto current_time = this->clock_->now(); - - visualization_msgs::msg::MarkerArray debug_marker_array; - if (!debug_data_.possible_collisions.empty()) { - appendMarkerArray( - createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array); - } - if (!debug_data_.detection_area_polygons.empty()) { - appendMarkerArray( - makeSlicePolygonMarker( - debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), - current_time, &debug_marker_array); - } - - return debug_marker_array; -} visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray() { const auto current_time = this->clock_->now(); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp deleted file mode 100644 index acb1827bc686b..0000000000000 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// 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 - -#include -#include - -namespace behavior_velocity_planner -{ -namespace occlusion_spot_utils -{ -using lanelet::BasicLineString2d; -using lanelet::BasicPoint2d; -using lanelet::BasicPolygon2d; -namespace bg = boost::geometry; -namespace lg = lanelet::geometry; - -BasicPoint2d calculateLateralOffsetPoint( - const BasicPoint2d & p0, const BasicPoint2d & p1, const double offset) -{ - // translation - const double dy = p1[1] - p0[1]; - const double dx = p1[0] - p0[0]; - // rotation (use inverse matrix of rotation) - const double yaw = std::atan2(dy, dx); - const double offset_x = p1[0] - std::sin(yaw) * offset; - const double offset_y = p1[1] + std::cos(yaw) * offset; - return BasicPoint2d{offset_x, offset_y}; -} - -void buildSlices( - std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const double offset, - const bool is_on_right, const PlannerParam & param) -{ - BasicLineString2d center_line = path_lanelet.centerline2d().basicLineString(); - const auto & p = param; - /** - * @brief relationships for interpolated polygon - * - * +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons - * | | - * +--------------------------+ - +---+(max_length,min_distance) = inner_polygons - */ - const double min_length = offset; // + p.baselink_to_front; - // Note: min_detection_area_length is for occlusion spot visualization but not effective for - // planning - const double min_detection_area_length = 10.0; - const double max_length = std::max( - min_detection_area_length, std::min(p.detection_area_max_length, p.detection_area_length)); - const double min_distance = (is_on_right) ? -p.half_vehicle_width : p.half_vehicle_width; - const double slice_length = p.detection_area.slice_length; - const int num_step = static_cast(slice_length); - //! max index is the last index of path point - const int max_index = static_cast(center_line.size() - 2); - int idx = 0; - /** - * Note: create polygon from path point is better than from ego offset to consider below - * - less computation cost and no need to recalculated interpolated point start from ego - * - less stable for localization noise - **/ - for (int s = 0; s < max_index; s += num_step) { - const double length = s * slice_length; - const double next_length = static_cast(s + num_step); - /// if (max_length < length) continue; - Slice slice; - BasicLineString2d inner_polygons; - BasicLineString2d outer_polygons; - // build connected polygon for lateral - for (int i = 0; i <= num_step; i++) { - idx = s + i; - const double arc_length_from_ego = std::max(0.0, static_cast(idx - min_length)); - if (arc_length_from_ego > max_length) break; - if (idx >= max_index) break; - const auto & c0 = center_line.at(idx); - const auto & c1 = center_line.at(idx + 1); - /** - * @brief points - * +--outer point (lateral distance obstacle can reach) - * | - * +--inner point(min distance) - */ - const BasicPoint2d inner_point = calculateLateralOffsetPoint(c0, c1, min_distance); - double lateral_distance = calculateLateralDistanceFromTTC(arc_length_from_ego, param); - if (is_on_right) lateral_distance *= -1; - const BasicPoint2d outer_point = calculateLateralOffsetPoint(c0, c1, lateral_distance); - inner_polygons.emplace_back(inner_point); - outer_polygons.emplace_back(outer_point); - } - if (inner_polygons.empty()) continue; - // connect invert point - inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); - slice.polygon = lanelet::BasicPolygon2d(inner_polygons); - // add range info - slice.range.min_length = length; - slice.range.max_length = next_length; - slices.emplace_back(slice); - } -} - -void buildDetectionAreaPolygon( - std::vector & slices, const PathWithLaneId & path, const double offset, - const PlannerParam & param) -{ - std::vector left_slices; - std::vector right_slices; - lanelet::ConstLanelet path_lanelet = toPathLanelet(path); - // in most case lateral distance is much more effective for velocity planning - buildSlices(left_slices, path_lanelet, offset, false /*is_on_right*/, param); - buildSlices(right_slices, path_lanelet, offset, true /*is_on_right*/, param); - // Properly order lanelets from closest to furthest - slices = left_slices; - slices.insert(slices.end(), right_slices.begin(), right_slices.end()); - return; -} -} // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp index 8e8b408082e78..8e91a5735b981 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp @@ -80,12 +80,12 @@ bool isOcclusionSpotSquare( void findOcclusionSpots( std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, - const lanelet::BasicPolygon2d & polygon, double min_size) + const Polygon2d & polygon, double min_size) { const grid_map::Matrix & grid_data = grid["layer"]; const int min_occlusion_spot_size = std::max(0.0, std::floor(min_size / grid.getResolution())); grid_map::Polygon grid_polygon; - for (const auto & point : polygon) { + for (const auto & point : polygon.outer()) { grid_polygon.addVertex({point.x(), point.y()}); } for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); ++iterator) { diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp index 44b30e7814e51..25ac665744861 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include @@ -25,7 +24,8 @@ namespace behavior_velocity_planner { -using occlusion_spot_utils::METHOD; +using occlusion_spot_utils::DETECTION_METHOD; +using occlusion_spot_utils::PASS_JUDGE; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -36,14 +36,31 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & pp = planner_param_; - // assume pedestrian coming out from occlusion spot with this velocity - const std::string method = node.declare_parameter(ns + ".method", "occupancy_grid"); - if (method == "occupancy_grid") { - pp.method = METHOD::OCCUPANCY_GRID; - } else if (method == "predicted_object") { - pp.method = METHOD::PREDICTED_OBJECT; - } else { - throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"}; + // for detection type + { + const std::string method = node.declare_parameter(ns + ".detection_method", "occupancy_grid"); + if (method == "occupancy_grid") { // module id 0 + pp.detection_method = DETECTION_METHOD::OCCUPANCY_GRID; + module_id_ = DETECTION_METHOD::OCCUPANCY_GRID; + } else if (method == "predicted_object") { // module id 1 + pp.detection_method = DETECTION_METHOD::PREDICTED_OBJECT; + module_id_ = DETECTION_METHOD::PREDICTED_OBJECT; + } else { + throw std::invalid_argument{ + "[behavior_velocity]: occlusion spot detection method has invalid argument"}; + } + } + // for passable judgement + { + const std::string pass_judge = node.declare_parameter(ns + ".pass_judge", "current_velocity"); + if (pass_judge == "current_velocity") { + pp.pass_judge = PASS_JUDGE::CURRENT_VELOCITY; + } else if (pass_judge == "smooth_velocity") { + pp.pass_judge = PASS_JUDGE::SMOOTH_VELOCITY; + } else { + throw std::invalid_argument{ + "[behavior_velocity]: occlusion spot pass judge method has invalid argument"}; + } } pp.debug = node.declare_parameter(ns + ".debug", false); pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", false); @@ -84,23 +101,11 @@ void OcclusionSpotModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; - const int64_t module_id = static_cast(ModuleID::OCCUPANCY); - const int64_t public_road_module_id = static_cast(ModuleID::OBJECT); // general - if (!isModuleRegistered(module_id)) { - if (planner_param_.method == METHOD::OCCUPANCY_GRID) { - registerModule(std::make_shared( - module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), - clock_, pub_debug_occupancy_grid_)); - } - } - // public - if (!isModuleRegistered(public_road_module_id)) { - if (planner_param_.method == METHOD::PREDICTED_OBJECT) { - registerModule(std::make_shared( - public_road_module_id, planner_data_, planner_param_, - logger_.get_child("occlusion_spot_in_public_module"), clock_)); - } + if (!isModuleRegistered(module_id_)) { + registerModule(std::make_shared( + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_, + pub_debug_occupancy_grid_)); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 87d351e0635fb..c0586aabff665 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -50,6 +49,32 @@ lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path) return lanelet::ConstLanelet(path_lanelet); } +PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0) +{ + PathWithLaneId out; + for (size_t i = 0; i < path.points.size(); i++) { + PathPointWithLaneId p = path.points.at(i); + p.point.longitudinal_velocity_mps = std::max(v0, 1.0); + out.points.emplace_back(p); + } + return out; +} + +bool buildDetectionAreaPolygon( + Polygons2d & slices, const PathWithLaneId & path, const double offset, const PlannerParam & param) +{ + const auto & p = param; + DetectionRange da_range; + da_range.interval = p.detection_area.slice_length; + da_range.min_longitudinal_distance = offset + p.baselink_to_front; + da_range.max_longitudinal_distance = + std::min(p.detection_area_max_length, p.detection_area_length) + + da_range.min_longitudinal_distance; + da_range.min_lateral_distance = p.half_vehicle_width; + da_range.max_lateral_distance = p.detection_area.max_lateral_distance; + return planning_utils::createDetectionAreaPolygons(slices, path, da_range, p.pedestrian_vel); +} + ROAD_TYPE getCurrentRoadType( const lanelet::ConstLanelet & current_lanelet, [[maybe_unused]] const lanelet::LaneletMapPtr & lanelet_map_ptr) @@ -178,21 +203,6 @@ bool isStuckVehicle(PredictedObject obj, const double min_vel) return false; } -double offsetFromStartToEgo( - const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx) -{ - double offset_from_ego_to_closest = 0; - for (int i = 0; i < closest_idx; i++) { - const auto & curr_p = path.points.at(i).point.pose.position; - const auto & next_p = path.points.at(i + 1).point.pose.position; - offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p); - } - const double offset_from_closest_to_target = - -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose) - .position.x; - return offset_from_ego_to_closest + offset_from_closest_to_target; -} - std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point) @@ -303,12 +313,12 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( return pc; } -std::vector generatePossibleCollisionBehindParkedVehicle( - const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, +bool generatePossibleCollisionBehindParkedVehicle( + std::vector & possible_collisions, const PathWithLaneId & path, + const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects) { lanelet::ConstLanelet path_lanelet = toPathLanelet(path); - std::vector possible_collisions; auto ll = path_lanelet.centerline2d(); for (const auto & dyn : dyn_objects) { ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll); @@ -334,11 +344,12 @@ std::vector generatePossibleCollisionBehindParkedVehicle( [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; }); - return possible_collisions; + if (possible_collisions.empty()) return false; + return true; } std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const std::vector & polys) + std::vector & objs, const Polygons2d & polys) { std::vector filtered_obj; // stuck points by predicted objects @@ -346,7 +357,7 @@ std::vector filterDynamicObjectByDetectionArea( // check if the footprint is in the stuck detect area const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object); for (const auto & p : polys) { - if (!bg::disjoint(obj_footprint, p.polygon)) { + if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); } } @@ -354,20 +365,21 @@ std::vector filterDynamicObjectByDetectionArea( return filtered_obj; } -void createPossibleCollisionsInDetectionArea( +bool createPossibleCollisionsInDetectionArea( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data) { lanelet::ConstLanelet path_lanelet = toPathLanelet(path); if (path_lanelet.centerline2d().empty()) { - return; + return true; } double distance_lower_bound = std::numeric_limits::max(); - for (const Slice & detection_area_slice : debug_data.detection_area_polygons) { + const Polygons2d & da_polygons = debug_data.detection_area_polygons; + for (const Polygon2d & detection_area_slice : da_polygons) { std::vector occlusion_spot_positions; grid_utils::findOcclusionSpots( - occlusion_spot_positions, grid, detection_area_slice.polygon, + occlusion_spot_positions, grid, detection_area_slice, param.detection_area.min_occlusion_spot_size); if (param.debug) { for (const auto & op : occlusion_spot_positions) { @@ -378,7 +390,7 @@ void createPossibleCollisionsInDetectionArea( } if (occlusion_spot_positions.empty()) continue; // for each partition find nearest occlusion spot from polygon's origin - BasicPoint2d base_point = detection_area_slice.polygon.at(0); + const Point2d base_point = detection_area_slice.outer().at(0); const auto pc = generateOneNotableCollisionFromOcclusionSpot( grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, debug_data); @@ -388,6 +400,14 @@ void createPossibleCollisionsInDetectionArea( distance_lower_bound = lateral_distance; possible_collisions.emplace_back(pc.get()); } + // sort by arc length + std::sort( + possible_collisions.begin(), possible_collisions.end(), + [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { + return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; + }); + if (possible_collisions.empty()) return false; + return true; } bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) @@ -400,7 +420,7 @@ bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons boost::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, - const double offset_from_start_to_ego, const BasicPoint2d base_point, + const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data) { const double baselink_to_front = param.baselink_to_front; @@ -414,7 +434,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS const lanelet::BasicPoint2d obstacle_point = { occlusion_spot_position[0], occlusion_spot_position[1]}; const double dist = - std::hypot(base_point[0] - obstacle_point[0], base_point[1] - obstacle_point[1]); + std::hypot(base_point.x() - obstacle_point[0], base_point.y() - obstacle_point[1]); // skip if absolute distance is larger if (distance_lower_bound < dist) continue; lanelet::ArcCoordinates arc_coord_occlusion_point = diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 94150d0a43fcd..38250d6d78255 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -98,25 +98,6 @@ int insertSafeVelocityToPath( return 0; } -double calculateLateralDistanceFromTTC( - const double longitudinal_distance, const PlannerParam & param) -{ - const auto & v = param.v; - const auto & p = param; - double v_min = 1.0; - const double lateral_buffer = 0.5; - const double min_distance = p.half_vehicle_width + lateral_buffer; - const double max_distance = p.detection_area.max_lateral_distance; - if (longitudinal_distance <= 0) return min_distance; - if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity; - // use min velocity if ego velocity is below min allowed - const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min; - // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ? - double t = longitudinal_distance / v0; - double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width; - return std::min(max_distance, std::max(min_distance, lateral_distance)); -} - SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) { SafeMotion sm; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 726c5e7bec799..9cd7f1851247f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -22,6 +21,8 @@ #include #include +#include + #include #include @@ -29,8 +30,6 @@ namespace behavior_velocity_planner { -namespace bg = boost::geometry; -namespace lg = lanelet::geometry; namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( @@ -38,10 +37,15 @@ OcclusionSpotModule::OcclusionSpotModule( const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const rclcpp::Publisher::SharedPtr publisher) -: SceneModuleInterface(module_id, logger, clock), publisher_(publisher) +: SceneModuleInterface(module_id, logger, clock), publisher_(publisher), param_(planner_param) { - param_ = planner_param; - debug_data_.detection_type = "occupancy"; + if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { + debug_data_.detection_type = "occupancy"; + //! occupancy grid limitation( 100 * 100 ) + param_.detection_area_length = std::min(50.0, param_.detection_area_length); + } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { + debug_data_.detection_type = "object"; + } if (param_.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets); @@ -62,41 +66,63 @@ bool OcclusionSpotModule::modifyPathVelocity( param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold; param_.v.v_ego = planner_data_->current_velocity->twist.linear.x; param_.v.a_ego = planner_data_->current_accel.get(); - param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( - param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_accel, param_.v.non_effective_jerk, - 0.0); + const double detection_area_offset = 5.0; // for visualization and stability + param_.detection_area_max_length = + planning_utils::calcJudgeLineDistWithJerkLimit( + param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_accel, param_.v.non_effective_jerk, + planner_data_->delay_response_time) + + detection_area_offset; } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; - const auto & occ_grid_ptr = planner_data_->occupancy_grid; - if (!occ_grid_ptr) { - return true; - } - - const double max_range = 50.0; PathWithLaneId clipped_path; - utils::clipPathByLength(*path, clipped_path, max_range); + utils::clipPathByLength(*path, clipped_path, param_.detection_area_length); PathWithLaneId interp_path; //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, &interp_path, logger_); - debug_data_.interp_path = interp_path; - int closest_idx = -1; - if (!planning_utils::calcClosestIndex( - interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { - return true; + if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { + interp_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); + } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { } - // return if ego is final point of interpolated path - if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; - grid_map::GridMap grid_map; - grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); - double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); + debug_data_.interp_path = interp_path; + const geometry_msgs::msg::Point start_point = interp_path.points.at(0).point.pose.position; + const auto offset = tier4_autoware_utils::calcSignedArcLength( + interp_path.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); + if (offset == boost::none) return true; + const double offset_from_start_to_ego = -offset.get(); auto & detection_area_polygons = debug_data_.detection_area_polygons; - utils::buildDetectionAreaPolygon( - detection_area_polygons, interp_path, offset_from_start_to_ego, param_); + if (!utils::buildDetectionAreaPolygon( + detection_area_polygons, interp_path, offset_from_start_to_ego, param_)) { + return true; // path point is not enough + } std::vector possible_collisions; - // Note: Don't consider offset from path start to ego here - utils::createPossibleCollisionsInDetectionArea( - possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_); + if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { + const auto & occ_grid_ptr = planner_data_->occupancy_grid; + if (!occ_grid_ptr) return true; // mo data + nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; + grid_map::GridMap grid_map; + grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); + if (param_.debug) publisher_->publish(occ_grid); // + // Note: Don't consider offset from path start to ego here + if (!utils::createPossibleCollisionsInDetectionArea( + possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, + debug_data_)) { + // no occlusion spot + return true; + } + } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { + const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; + if (!dynamic_obj_arr_ptr) return true; // mo data + std::vector obj = + utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); + const auto filtered_obj = + utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons); + // Note: Don't consider offset from path start to ego here + if (!utils::generatePossibleCollisionBehindParkedVehicle( + possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) { + // no occlusion spot + return true; + } + } RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); @@ -105,9 +131,7 @@ bool OcclusionSpotModule::modifyPathVelocity( // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); // these debug topics needs computation resource - if (param_.debug) { - publisher_->publish(occ_grid); - } + debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; debug_data_.path_raw = *path; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp deleted file mode 100644 index 7040c7c221af1..0000000000000 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// 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 -#include - -#include - -#include -#include -#include -#include - -namespace behavior_velocity_planner -{ -using occlusion_spot_utils::PossibleCollisionInfo; -namespace utils = occlusion_spot_utils; - -OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( - const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock) -{ - param_ = planner_param; - debug_data_.detection_type = "object"; - if (param_.use_partition_lanelet) { - const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); - planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets); - } -} - -bool OcclusionSpotInPublicModule::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) -{ - debug_data_.resetData(); - if (path->points.size() < 2) { - return true; - } - // set planner data - { - param_.v.max_stop_jerk = planner_data_->max_stop_jerk_threshold; - param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold; - param_.v.v_ego = planner_data_->current_velocity->twist.linear.x; - param_.v.a_ego = planner_data_->current_accel.get(); - param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( - param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_accel, param_.v.non_effective_jerk, - 0.0); - } - const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; - const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - - if (!dynamic_obj_arr_ptr) { - return true; - } - PathWithLaneId clipped_path; - utils::clipPathByLength(*path, clipped_path, param_.detection_area_length); - PathWithLaneId interp_path; - splineInterpolate(clipped_path, 1.0, &interp_path, logger_); - int closest_idx = -1; - if (!planning_utils::calcClosestIndex( - interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { - return true; - } - // return if ego is final point of interpolated path - if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - std::vector obj = - utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); - double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); - auto & detection_area_polygons = debug_data_.detection_area_polygons; - utils::buildDetectionAreaPolygon( - detection_area_polygons, interp_path, offset_from_start_to_ego, param_); - const auto filtered_obj = utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons); - // Note: Don't consider offset from path start to ego here - std::vector possible_collisions = - utils::generatePossibleCollisionBehindParkedVehicle( - interp_path, param_, offset_from_start_to_ego, filtered_obj); - utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); - // Note: Consider offset from path start to ego here - utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); - // apply safe velocity using ebs and pbs deceleration - utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); - debug_data_.possible_collisions = possible_collisions; - return true; -} - -} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 57906fe36c01f..5e1cf7e377654 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -23,7 +23,126 @@ namespace behavior_velocity_planner { namespace planning_utils { -void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys) +Point2d calculateLateralOffsetPoint2d(const Pose & pose, const double offset) +{ + using tier4_autoware_utils::calcOffsetPose; + return to_bg2d(calcOffsetPose(pose, 0.0, offset, 0.0)); +} + +PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) +{ + auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; + PathPoint p; + Pose pose; + const auto pp0 = p0.pose.position; + const auto pp1 = p1.pose.position; + pose.position.x = lerp(pp0.x, pp1.x, ratio); + pose.position.y = lerp(pp0.y, pp1.y, ratio); + pose.position.z = lerp(pp0.z, pp1.z, ratio); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(pp0, pp1); + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + p.pose = pose; + const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); + p.longitudinal_velocity_mps = v; + return p; +} + +bool createDetectionAreaPolygons( + Polygons2d & da_polys, const PathWithLaneId & path, const DetectionRange da_range, + const double obstacle_vel_mps) +{ + /** + * @brief relationships for interpolated polygon + * + * +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons + * | | + * +--------------------------+ - +---+(max_length,min_distance) = inner_polygons + */ + const double min_len = da_range.min_longitudinal_distance; + const double max_len = da_range.max_longitudinal_distance; + const double min_dst = da_range.min_lateral_distance; + const double max_dst = da_range.max_lateral_distance; + const double interval = da_range.interval; + const double min_velocity = 0.5; // min velocity that autoware can cruise stably + //! max index is the last index of path point + const size_t max_index = static_cast(path.points.size() - 1); + double dist_sum = 0; + size_t first_idx = 0; // first path point found in front of ego front bumper + offset + const auto & pp = path.points; + //! avoid bug with same point polygon + const double eps = 1e-3; + if (path.points.size() < 2) return false; // case of path point is only one + auto p0 = path.points.front().point; + // handle the first point + { + double current_dist = 0.0; + for (size_t i = 1; i <= max_index - 1; i++) { + dist_sum += tier4_autoware_utils::calcDistance2d(pp.at(i - 1), pp.at(i)); + if (dist_sum > min_len) { + first_idx = i; + break; + } + current_dist = dist_sum; + } + if (first_idx == 0) return false; // case of all path point is behind ego front bumper + offset + const double ds = dist_sum - current_dist; + if (std::abs(ds) < eps) { + p0 = pp.at(first_idx - 1).point; + } else { + const double ratio = (min_len - current_dist) / ds; + p0 = getLerpPathPointWithLaneId(pp.at(first_idx - 1).point, pp.at(first_idx).point, ratio); + } + } + double ttc = 0.0; + dist_sum = min_len; + double length = 0; + // initial point of detection area polygon + LineString2d left_inner_bound = {calculateLateralOffsetPoint2d(p0.pose, min_dst)}; + LineString2d left_outer_bound = {calculateLateralOffsetPoint2d(p0.pose, min_dst + eps)}; + LineString2d right_inner_bound = {calculateLateralOffsetPoint2d(p0.pose, -min_dst)}; + LineString2d right_outer_bound = {calculateLateralOffsetPoint2d(p0.pose, -min_dst - eps)}; + for (size_t s = first_idx; s <= max_index; s++) { + const auto p1 = path.points.at(s).point; + const double ds = tier4_autoware_utils::calcDistance2d(p0, p1); + dist_sum += ds; + length += ds; + // calculate the distance that obstacles can move until ego reach the trajectory point + const double v_average = 0.5 * (p0.longitudinal_velocity_mps + p1.longitudinal_velocity_mps); + const double v = std::max(v_average, min_velocity); + const double dt = ds / v; + ttc += dt; + // for offset calculation + const double max_lateral_distance = std::min(max_dst, min_dst + ttc * obstacle_vel_mps + eps); + // left bound + if (da_range.use_left) { + left_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, min_dst)); + left_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, max_lateral_distance)); + } + // right bound + if (da_range.use_right) { + right_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, -min_dst)); + right_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, -max_lateral_distance)); + } + // replace previous point with next point + p0 = p1; + // separate detection area polygon with fixed interval or at the end of detection max length + if (length > interval || max_len < dist_sum || s == max_index) { + if (left_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(left_inner_bound, left_outer_bound)); + if (right_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(right_outer_bound, right_inner_bound)); + left_inner_bound = {left_inner_bound.back()}; + left_outer_bound = {left_outer_bound.back()}; + right_inner_bound = {right_inner_bound.back()}; + right_outer_bound = {right_outer_bound.back()}; + length = 0; + if (max_len < dist_sum || s == max_index) return true; + } + } + return true; +} + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys) { const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); for (const auto & partition : partitions) { diff --git a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp index ff47f2414428a..7238dd971b02e 100644 --- a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp @@ -65,49 +65,6 @@ TEST(safeMotion, delay_jerk_acceleration) } } -TEST(detectionArea, calcLateralDistance) -{ - namespace utils = behavior_velocity_planner::occlusion_spot_utils; - using utils::calculateLateralDistanceFromTTC; - /** - * @brief check if calculation is correct in below parameter - * lateral distance is calculated from - * - ego velocity - * - min distance(safety margin) - * - max distance(ignore distance above this) - * - pedestrian velocity - * - min allowed velocity(not to stop) - */ - utils::PlannerParam p; - p.half_vehicle_width = 2.0; - p.baselink_to_front = 3.0; - p.pedestrian_vel = 1.5; - p.detection_area.max_lateral_distance = 5.0; - p.v.min_allowed_velocity = 1.5; - p.v.v_ego = 5.0; - const double offset_from_ego_to_start = 0.0; - { - for (size_t i = 0; i <= 15; i += 5) { - // arc length in path point - const double l = i * 1.0; - const double s = l - offset_from_ego_to_start; - const double d = utils::calculateLateralDistanceFromTTC(s, p); - const double eps = 1e-3; - std::cout << "s: " << l << " v: " << p.v.v_ego << " d: " << d << std::endl; - if (i == 0) - EXPECT_NEAR(d, 2.5, eps); - else if (i == 5) - EXPECT_NEAR(d, 3.5, eps); - else if (i == 10) - EXPECT_NEAR(d, 5.0, eps); - else if (i == 15) - EXPECT_NEAR(d, 5.0, eps); - else - break; - } - } -} - TEST(calculateInsertVelocity, min_max) { using behavior_velocity_planner::occlusion_spot_utils::calculateInsertVelocity; diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index 0c42f35d44206..bc694f79f46a1 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -19,12 +19,12 @@ find_package(OpenCV REQUIRED) ament_auto_add_library(obstacle_avoidance_planner SHARED src/vehicle_model/vehicle_model_interface.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp - src/util.cpp - src/debug.cpp + src/utils.cpp + src/costmap_generator.cpp + src/debug_visualization.cpp src/eb_path_optimizer.cpp src/mpt_optimizer.cpp - src/process_cv.cpp + src/cv_utils.cpp src/node.cpp ) diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/obstacle_avoidance_planner/README.md index f97e50aeea414..f2b3587274b06 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/obstacle_avoidance_planner/README.md @@ -1,9 +1,15 @@ -# Obstacle Avoidance Planner - ## Purpose This package generates a trajectory that is feasible to drive and collision free based on a reference path, drivable area, and static/dynamic obstacles. -Only position and orientation of trajectory are calculated in this module, and velocity or acceleration will be updated in the latter modules. +Only position and orientation of trajectory are calculated in this module (velocity is just aligned from the one in the path), and velocity or acceleration will be updated in the latter modules. + +## Feature + +This package is able to + +- follow the behavior path smoothly +- make the trajectory inside the drivable area as much as possible +- insert stop point if its trajectory point is outside the drivable area ## Inputs / Outputs @@ -13,34 +19,84 @@ Only position and orientation of trajectory are calculated in this module, and v | ---------------------------------------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | | `~/input/path` | autoware_auto_planning_msgs/Path | Reference path and the corresponding drivable area | | `~/input/objects` | autoware_auto_perception_msgs/PredictedObjects | Recognized objects around the vehicle | -| `/localization/kinematic_state` | nav_msgs/Odometry | Current Velocity of ego vehicle | +| `/localization/kinematic_kinematics` | nav_msgs/Odometry | Current Velocity of ego vehicle | | `/planning/scenario_planning/lane_driving/obstacle_avoidance_approval` | tier4_planning_msgs/EnableAvoidance | Approval to execute obstacle avoidance | ### output -| Name | Type | Description | -| --------------- | -------------------------------------- | ----------------------------------------------------------------- | -| `~/output/path` | autoware_auto_planning_msgs/Trajectory | Optimized trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | -------------------------------------- | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs/Trajectory | Optimized trajectory that is feasible to drive and collision-free | ## Flowchart -Each module is explained briefly here based on the flowchart. +Flowchart of functions is explained here. + +```plantuml +@startuml +title pathCallback +start + +group generateOptimizedTrajectory + :checkReplan; + if (replanning required?) then (yes) + group getMaps + :getDrivableArea; + :getRoadClearanceMap; + :drawObstacleOnImage; + :getObstacleClearanceMap; + end group + + group optimizeTrajectory + :getEBTrajectory; + :getModelPredictiveTrajectory; + + if (optimization failed?) then (no) + else (yes) + :send previous\n trajectory; + endif + end group + + :insertZeroVelocityOutsideDrivableArea; + + :publishDebugDataInOptimization; + else (no) + :send previous\n trajectory; + endif +end group + + +group generatePostProcessedTrajectory + :getExtendedOptimizedTrajectory; + :concatTrajectory; + :generateFineTrajectoryPoints; + :alignVelocity; +end group -![obstacle_avoidance_planner_flowchart](./media/obstacle_avoidance_planner_flowchart.drawio.svg) +:convertToTrajectory; -### Manage trajectory generation +:publishDebugDataInMain; -When one of the following conditions area realized, callback function to generate a trajectory is called and publish the trajectory. -Otherwise, previously generated trajectory is published. +stop +@enduml +``` -- Ego moves a certain distance compared to the previous ego pose (default: 10.0 [m]) +### checkReplan + +When one of the following conditions are realized, callback function to generate a trajectory is called and publish the trajectory. +Otherwise, previously generated (optimized) trajectory is published just with aligning the velocity from the latest behavior path. + +- Ego moves a certain distance compared to the previous ego pose (default: 3.0 [m]) - Time passes (default: 1.0 [s]) -- The path shape is changed (e.g. when starting planning lane change) - Ego is far from the previously generated trajectory -The previously generated trajectory is memorized, but it is not when the path shape is changed and ego is far from the previously generated trajectory. +### getRoadClearanceMap + +Cost map is generated according to the distance to the road boundaries. + +These cost maps are used in the optimization to generate a collision-free trajectory. -### Select objects to avoid +### drawObstacleOnImage Only obstacles that are static and locate in a shoulder lane is decided to avoid. In detail, this equals to the following three conditions at the same time, and the red obstacles in the figure (id: 3, 4, 5) is to be avoided. @@ -52,39 +108,37 @@ In detail, this equals to the following three conditions at the same time, and t ![obstacle_to_avoid](./media/obstacle_to_avoid.drawio.svg) -### Generate object costmap +### getObstacleClearanceMap Cost map is generated according to the distance to the target obstacles to be avoided. -### Generate road boundary costmap - -Cost map is generated according to the distance to the road boundaries. - -These cost maps area used in the optimization to generate a collision-free trajectory. +### getEBTrajectory -### Smooth path +The latter optimization (MPT) assumes that the reference path is smooth enough. +Therefore the path from behavior is made smooth here, and send to the optimization as a format of trajectory. +Obstacles are ignored in this function. -The latter optimization assumes that the reference path is smooth enough. -Therefore the path from behavior is smoothed here, and send to the optimization as a format of trajectory. -Obstacles are not considered. +More details can be seen in the Elastic Band section. -### Optimize trajectory +### getModelPredictiveTrajectory This module makes the trajectory kinematically-feasible and collision-free. We define vehicle pose in the frenet coordinate, and minimize tracking errors by optimization. -This optimization also considers vehicle kinematics and collision checking with road boundary and obstacles. -To decrease the computation cost, the optimization is applied to the shorter trajectory than the whole trajectory, and concatenate the remained trajectory with the optimized one at last. +This optimization considers vehicle kinematics and collision checking with road boundary and obstacles. +To decrease the computation cost, the optimization is applied to the shorter trajectory (default: 50 [m]) than the whole trajectory, and concatenate the remained trajectory with the optimized one at last. The trajectory just in front of the ego must not be changed a lot so that the steering wheel will be stable. Therefore, we use the previously generated trajectory in front of the ego. Optimization center on the vehicle, that tries to locate just on the trajectory, can be tuned along side the vehicle vertical axis. -This parameter `optimization center offset` is defined as the signed length from the back-wheel center to the optimization center. +This parameter `mpt.kinematics.optimization center offset` is defined as the signed length from the back-wheel center to the optimization center. Some examples are shown in the following figure, and it is shown that the trajectory of vehicle shape differs according to the optimization center even if the reference trajectory (green one) is the same. ![mpt_optimization_offset](./media/mpt_optimization_offset.svg) -### Check drivability, and extend trajectory +More details can be seen in the Model Predictive Trajectory section. + +### insertZeroVelocityOutsideDrivableArea Optimized trajectory is too short for velocity planning, therefore extend the trajectory by concatenating the optimized trajectory and the behavior path considering drivability. Generated trajectory is checked if it is inside the drivable area or not, and if outside drivable area, output a trajectory inside drivable area with the behavior path or the previously generated trajectory. @@ -96,9 +150,9 @@ As described above, the behavior path is separated into two paths: one is for op - In this case, we do not care if the remained trajectory is inside or outside the drivable area since generally it is outside the drivable area (especially in a narrow road), but we want to pass a trajectory as long as possible to the latter module. - If optimized trajectory is **outside the drivable area**, and the remained trajectory is inside/outside the drivable area, - and if the previously generated trajectory **is memorized**, - - the output trajectory will be a part of previously generated trajectory, whose end firstly goes outside the drivable area. + - the output trajectory will be the previously generated trajectory, where zero velocity is inserted to the point firstly going outside the drivable area. - and if the previously generated trajectory **is not memorized**, - - the output trajectory will be a part of trajectory just transformed from the behavior path, whose end firstly goes outside the drivable area. + - the output trajectory will be a part of trajectory just transformed from the behavior path, where zero velocity is inserted to the point firstly going outside the drivable area. Optimization failure is dealt with the same as if the optimized trajectory is outside the drivable area. The output trajectory is memorized as a previously generated trajectory for the next cycle. @@ -107,14 +161,14 @@ _Rationale_ In the current design, since there are some modelling errors, the constraints are considered to be soft constraints. Therefore, we have to make sure that the optimized trajectory is inside the drivable area or not after optimization. -### Assign trajectory velocity +### alignVelocity -Velocity is assigned in the result trajectory by the behavior path. +Velocity is assigned in the result trajectory from the velocity in the behavior path. The shapes of the trajectory and the path are different, therefore the each nearest trajectory point to the path is searched and interpolated linearly. ## Algorithms -In this section, Elastic band (to smooth the path) and Model Predictive Trajectory (to optimize the trajectory) will be explained in detail. +In this section, Elastic band (to make the path smooth) and Model Predictive Trajectory (to make the trajectory kinematically feasible and collision-free) will be explained in detail. ### Elastic band @@ -131,11 +185,68 @@ Therefore the output path may have a collision with road boundaries or obstacles We formulate a QP problem minimizing the distance between the previous point and the next point for each point. Conditions that each point can move to a certain extent are used so that the path will not changed a lot but will be smoother. -For $k$'th point ($\boldsymbol{p}_k$), the objective function is as follows. +For $k$'th point ($\boldsymbol{p}_k = (x_k, y_k)$), the objective function is as follows. The beginning and end point are fixed during the optimization. $$ -\min \sum_{k=1}^{n-1} |\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}| - |\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1}| +\begin{align} +\ J & = \min \sum_{k=1}^{n-1} ||(\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}) - (\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1})||^2 \\ +\ & = \min \sum_{k=1}^{n-1} ||\boldsymbol{p}_{k+1} - 2 \boldsymbol{p}_{k} + \boldsymbol{p}_{k-1}||^2 \\ +\ & = \min \sum_{k=1}^{n-1} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\ +\ & = \min + \begin{pmatrix} + \ x_0 \\ + \ x_1 \\ + \ x_2 \\ + \vdots \\ + \ x_{n-2}\\ + \ x_{n-1} \\ + \ x_{n} \\ + \ y_0 \\ + \ y_1 \\ + \ y_2 \\ + \vdots \\ + \ y_{n-2}\\ + \ y_{n-1} \\ + \ y_{n} \\ + \end{pmatrix}^T + \begin{pmatrix} + 1 & -2 & 1 & 0 & \dots& \\ + -2 & 5 & -4 & 1 & 0 &\dots \\ + 1 & -4 & 6 & -4 & 1 & \\ + 0 & 1 & -4 & 6 & -4 & \\ + \vdots & 0 & \ddots&\ddots& \ddots \\ + & \vdots & & & \\ + & & & 1 & -4 & 6 & -4 & 1 \\ + & & & & 1 & -4 & 5 & -2 \\ + & & & & & 1 & -2 & 1& \\ + & & & & & & & &1 & -2 & 1 & 0 & \dots& \\ + & & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\ + & & & & & & & &1 & -4 & 6 & -4 & 1 & \\ + & & & & & & & &0 & 1 & -4 & 6 & -4 & \\ + & & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\ + & & & & & & & & & \vdots & & & \\ + & & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\ + & & & & & & & & & & & & 1 & -4 & 5 & -2 \\ + & & & & & & & & & & & & & 1 & -2 & 1& \\ + \end{pmatrix} + \begin{pmatrix} + \ x_0 \\ + \ x_1 \\ + \ x_2 \\ + \vdots \\ + \ x_{n-2}\\ + \ x_{n-1} \\ + \ x_{n} \\ + \ y_0 \\ + \ y_1 \\ + \ y_2 \\ + \vdots \\ + \ y_{n-2}\\ + \ y_{n-1} \\ + \ y_{n} \\ + \end{pmatrix} +\end{align} $$ ### Model predictive trajectory @@ -152,7 +263,7 @@ When the optimization failed or the optimized trajectory is not collision free, Trajectory near the ego must be stable, therefore the condition where trajectory points near the ego are the same as previously generated trajectory is considered, and this is the only hard constraints in MPT. -#### Formulation +#### Vehicle kinematics As the following figure, we consider the bicycle kinematics model in the frenet frame to track the reference path. At time step $k$, we define lateral distance to the reference path, heading angle against the reference path, and steer angle as $y_k$, $\theta_k$, and $\delta_k$ respectively. @@ -170,6 +281,8 @@ y_{k+1} & = y_{k} + v \sin \theta_k dt \\ \end{align} $$ +##### Linearization + Then we linearize these equations. $y_k$ and $\theta_k$ are tracking errors, so we assume that those are small enough. Therefore $\sin \theta_k \approx \theta_k$. @@ -200,47 +313,197 @@ $$ \end{align} $$ -Based on the linearization, the error kinematics is formulated with the following linear equations. +##### One-step state equation + +Based on the linearization, the error kinematics is formulated with the following linear equations, $$ \begin{align} \begin{pmatrix} y_{k+1} \\ - \theta_{k+1} \\ - \delta_{k+1} + \theta_{k+1} \end{pmatrix} = \begin{pmatrix} - 1 & v dt & 0 \\ - 0 & 1 & \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ - 0 & 0 & 1 - \frac{dt}{\tau} + 1 & v dt \\ + 0 & 1 \\ \end{pmatrix} \begin{pmatrix} y_k \\ \theta_k \\ - \delta_k \end{pmatrix} + \begin{pmatrix} 0 \\ - 0 \\ - \frac{dt}{\tau} + \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ \end{pmatrix} - \delta_{des} + \delta_{k} + \begin{pmatrix} 0 \\ \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt\\ - 0 \end{pmatrix} \end{align} $$ -The objective function for smoothing and tracking is shown as follows. +which can be formulated as follows with the state $\boldsymbol{x}$, control input $u$ and some matrices, where $\boldsymbol{x} = (y_k, \theta_k)$ + +$$ +\begin{align} + \boldsymbol{x}_{k+1} = A_k \boldsymbol{x}_k + \boldsymbol{b}_k u_k + \boldsymbol{w}_k +\end{align} +$$ + +##### Time-series state equation + +Then, we formulate time-series state equation by concatenating states, control inputs and matrices respectively as + +$$ +\begin{align} + \boldsymbol{x} = A \boldsymbol{x}_0 + B \boldsymbol{u} + \boldsymbol{w} +\end{align} +$$ + +where + +$$ +\begin{align} +\boldsymbol{x} = (\boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \boldsymbol{x}^T_3, \dots, \boldsymbol{x}^T_{n-1})^T \\ +\boldsymbol{u} = (u_0, u_1, u_2, \dots, u_{n-2})^T \\ +\boldsymbol{w} = (\boldsymbol{w}^T_0, \boldsymbol{w}^T_1, \boldsymbol{w}^T_2, \dots, \boldsymbol{w}^T_{n-1})^T. \\ +\end{align} +$$ + +In detail, each matrices are constructed as follows. + +$$ +\begin{align} + \begin{pmatrix} + \boldsymbol{x}_1 \\ + \boldsymbol{x}_2 \\ + \boldsymbol{x}_3 \\ + \vdots \\ + \boldsymbol{x}_{n-1} + \end{pmatrix} + = + \begin{pmatrix} + A_0 \\ + A_1 A_0 \\ + A_2 A_1 A_0\\ + \vdots \\ + \prod\limits_{k=0}^{n-1} A_{k} + \end{pmatrix} + \boldsymbol{x}_0 + + + \begin{pmatrix} + B_0 & 0 & & \dots & 0 \\ + A_0 B_0 & B_1 & 0 & \dots & 0 \\ + A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} + \end{pmatrix} + \begin{pmatrix} + u_0 \\ + u_1 \\ + u_2 \\ + \vdots \\ + u_{n-2} + \end{pmatrix} + + + \begin{pmatrix} + I & 0 & & \dots & 0 \\ + A_0 & I & 0 & \dots & 0 \\ + A_1 A_0 & A_0 & I & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{w}_0 \\ + \boldsymbol{w}_1 \\ + \boldsymbol{w}_2 \\ + \vdots \\ + \boldsymbol{w}_{n-2} + \end{pmatrix} +\end{align} +$$ + +##### Free-boundary-conditioned time-series state equation + +For path planning which does not start from the current ego pose, $\boldsymbol{x}_0$ should be the design variable of optimization. +Therefore, we make $\boldsymbol{u}'$ by concatenating $\boldsymbol{x}_0$ and $\boldsymbol{u}$, and redefine $\boldsymbol{x}$ as follows. + +$$ +\begin{align} + \boldsymbol{u}' & = (\boldsymbol{x}^T_0, \boldsymbol{u}^T)^T \\ + \boldsymbol{x} & = (\boldsymbol{x}^T_0, \boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \dots, \boldsymbol{x}^T_{n-1})^T +\end{align} +$$ + +Then we get the following state equation + +$$ +\begin{align} + \boldsymbol{x}' = B \boldsymbol{u}' + \boldsymbol{w}, +\end{align} +$$ + +which is in detail $$ \begin{align} -J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) \\ & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 + \begin{pmatrix} + \boldsymbol{x}_0 \\ + \boldsymbol{x}_1 \\ + \boldsymbol{x}_2 \\ + \boldsymbol{x}_3 \\ + \vdots \\ + \boldsymbol{x}_{n-1} + \end{pmatrix} + = + \begin{pmatrix} + I & 0 & \dots & & & 0 \\ + A_0 & B_0 & 0 & & \dots & 0 \\ + A_1 A_0 & A_0 B_0 & B_1 & 0 & \dots & 0 \\ + A_2 A_1 A_0 & A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ + \vdots & \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-1} A_k & \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{x}_0 \\ + u_0 \\ + u_1 \\ + u_2 \\ + \vdots \\ + u_{n-2} + \end{pmatrix} + + + \begin{pmatrix} + 0 & \dots & & & 0 \\ + I & 0 & & \dots & 0 \\ + A_0 & I & 0 & \dots & 0 \\ + A_1 A_0 & A_0 & I & \dots & 0 \\ + \vdots & \vdots & & \ddots & 0 \\ + \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I + \end{pmatrix} + \begin{pmatrix} + \boldsymbol{w}_0 \\ + \boldsymbol{w}_1 \\ + \boldsymbol{w}_2 \\ + \vdots \\ + \boldsymbol{w}_{n-2} + \end{pmatrix}. +\end{align} +$$ + +#### Objective function + +The objective function for smoothing and tracking is shown as follows, which can be formulated with value function matrices $Q, R$. + +$$ +\begin{align} +J_1 (\boldsymbol{x}', \boldsymbol{u}') & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 \\ +& = \boldsymbol{x}'^T Q \boldsymbol{x}' + \boldsymbol{u}'^T R \boldsymbol{u}' \\ +& = \boldsymbol{u}'^T H \boldsymbol{u}' + \boldsymbol{u}'^T \boldsymbol{f} \end{align} $$ @@ -250,25 +513,29 @@ Assuming that the lateral distance to the road boundaries or obstacles from the $$ y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\ y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\ -y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} +y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} \\ +0 \leq \lambda_{\mathrm{base}, k} \\ +0 \leq \lambda_{\mathrm{top}, k} \\ +0 \leq \lambda_{\mathrm{mid}, k} $$ Since $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ is formulated as a linear function of $y_k$, the objective function for soft constraints is formulated as follows. $$ \begin{align} -J_2 & (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) \\ & = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k}^2 + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k}^2 + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k}^2 +J_2 & (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol {\lambda}_\mathrm{mid})\\ +& = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k} + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k} + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k} \end{align} $$ Slack variables are also design variables for optimization. -We define a vector $\boldsymbol{x}$, that concatenates all the design variables. +We define a vector $\boldsymbol{v}$, that concatenates all the design variables. $$ \begin{align} -\boldsymbol{x} = +\boldsymbol{v} = \begin{pmatrix} -... & y_k & \lambda_{\mathrm{base}, k} & \lambda_{\mathrm{top}, k} & \lambda_{\mathrm{mid}, k} & ... + \boldsymbol{u}'^T & \boldsymbol{\lambda}_\mathrm{base}^T & \boldsymbol{\lambda}_\mathrm{top}^T & \boldsymbol{\lambda}_\mathrm{mid}^T \end{pmatrix}^T \end{align} $$ @@ -277,7 +544,7 @@ The summation of these two objective functions is the objective function for the $$ \begin{align} -\min_{\boldsymbol{x}} J (\boldsymbol{x}) = \min_{\boldsymbol{x}} J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) + J_2 (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) +\min_{\boldsymbol{v}} J (\boldsymbol{v}) = \min_{\boldsymbol{v}} J_1 (\boldsymbol{u}') + J_2 (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol{\lambda}_\mathrm{mid}) \end{align} $$ @@ -294,11 +561,215 @@ Finally we transform those objective functions to the following QP problem, and $$ \begin{align} -\min_{\boldsymbol{x}} \ & \frac{1}{2} \boldsymbol{x}^T \boldsymbol{P} \boldsymbol{x} + \boldsymbol{q} \boldsymbol{x} \\ -\mathrm{s.t.} \ & \boldsymbol{b}_l \leq \boldsymbol{A} \boldsymbol{x} \leq \boldsymbol{b}_u +\min_{\boldsymbol{v}} \ & \frac{1}{2} \boldsymbol{v}^T \boldsymbol{H} \boldsymbol{v} + \boldsymbol{f} \boldsymbol{v} \\ +\mathrm{s.t.} \ & \boldsymbol{b}_{lower} \leq \boldsymbol{A} \boldsymbol{v} \leq \boldsymbol{b}_{upper} +\end{align} +$$ + +#### Constraints + +##### Steer angle limitation + +Steer angle has a certain limitation ($\delta_{max}$, $\delta_{min}$). +Therefore we add linear inequality equations. + +$$ +\begin{align} +\delta_{min} \leq \delta_i \leq \delta_{max} +\end{align} +$$ + +##### Collision free + +To realize collision-free path planning, we have to formulate constraints that the vehicle is inside the road (moreover, a certain meter far from the road boundary) and does not collide with obstacles in linear equations. +For linearity, we chose a method to approximate the vehicle shape with a set of circles, that is reliable and easy to implement. + +Now we formulate the linear constraints where a set of circles on each trajectory point is collision-free. +For collision checking, we have a drivable area in the format of an image where walls or obstacles are filled with a color. +By using this drivable area, we calculate upper (left) and lower (right) boundaries along reference points so that we can interpolate boundaries on any position on the trajectory. + +Assuming that upper and lower boundaries are $b_l$, $b_u$ respectively, and $r$ is a radius of a circle, lateral deviation of the circle center $y'$ has to be + +$$ +b_l + r \leq y' \leq b_u - r. +$$ + +Based on the following figure, $y'$ can be formulated as follows. + +$$ +\begin{align} +y' & = L \sin(\theta + \beta) + y \cos \beta - l \sin(\gamma - \phi_a) \\ +& = L \sin \theta \cos \beta + L \cos \theta \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \\ +& \approx L \theta \cos \beta + L \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) +\end{align} +$$ + +$$ +b_l + r - \lambda \leq y' \leq b_u - r + \lambda. +$$ + +$$ +\begin{align} +y' & = C_1 \boldsymbol{x} + C_2 \\ +& = C_1 (B \boldsymbol{v} + \boldsymbol{w}) + C_2 \\ +& = C_1 B \boldsymbol{v} + \boldsymbol{w} + C_2 \end{align} $$ +Note that longitudinal position of the circle center and the trajectory point to calculate boundaries are different. +But each boundaries are vertical against the trajectory, resulting in less distortion by the longitudinal position difference since road boundaries does not change so much. +For example, if the boundaries are not vertical against the trajectory and there is a certain difference of longitudinal position between the circe center and the trajectory point, we can easily guess that there is much more distortion when comparing lateral deviation and boundaries. + +$$ +\begin{align} + A_{blk} & = + \begin{pmatrix} + C_1 B & O & \dots & O & I_{N_{ref} \times N_{ref}} & O \dots & O\\ + -C_1 B & O & \dots & O & I & O \dots & O\\ + O & O & \dots & O & I & O \dots & O + \end{pmatrix} + \in \boldsymbol{R}^{3 N_{ref} \times D_v + N_{circle} N_{ref}} \\ + \boldsymbol{b}_{lower, blk} & = + \begin{pmatrix} + \boldsymbol{b}_{lower} - C_1 \boldsymbol{w} - C_2 \\ + -\boldsymbol{b}_{upper} + C_1 \boldsymbol{w} + C_2 \\ + O + \end{pmatrix} + \in \boldsymbol{R}^{3 N_{ref}} \\ + \boldsymbol{b}_{upper, blk} & = \boldsymbol{\infty} + \in \boldsymbol{R}^{3 N_{ref}} +\end{align} +$$ + +We will explain options for optimization. + +###### L-infinity optimization + +The above formulation is called L2 norm for slack variables. +Instead, if we use L-infinity norm where slack variables are shared by enabling `l_inf_norm`. + +$$ +\begin{align} + A_{blk} = + \begin{pmatrix} + C_1 B & I_{N_{ref} \times N_{ref}} \\ + -C_1 B & I \\ + O & I + \end{pmatrix} +\in \boldsymbol{R}^{3N_{ref} \times D_v + N_{ref}} +\end{align} +$$ + +###### Two-step soft constraints + +$$ +\begin{align} +\boldsymbol{v}' = + \begin{pmatrix} + \boldsymbol{v} \\ + \boldsymbol{\lambda}^{soft_1} \\ + \boldsymbol{\lambda}^{soft_2} \\ + \end{pmatrix} + \in \boldsymbol{R}^{D_v + 2N_{slack}} +\end{align} +$$ + +$*$ depends on whether to use L2 norm or L-infinity optimization. + +$$ +\begin{align} + A_{blk} & = + \begin{pmatrix} + A^{soft_1}_{blk} \\ + A^{soft_2}_{blk} \\ + \end{pmatrix}\\ + & = + \begin{pmatrix} + C_1^{soft_1} B & & \\ + -C_1^{soft_1} B & \Huge{*} & \Huge{O} \\ + O & & \\ + C_1^{soft_2} B & & \\ + -C_1^{soft_2} B & \Huge{O} & \Huge{*} \\ + O & & + \end{pmatrix} + \in \boldsymbol{R}^{6 N_{ref} \times D_v + 2 N_{slack}} +\end{align} +$$ + +$N_{slack}$ is $N_{circle}$ when L2 optimization, or $1$ when L-infinity optimization. +$N_{circle}$ is the number of circles to check collision. + +## Tuning + +### Vehicle + +- max steering wheel degree + - `mpt.kinematics.max_steer_deg` + +### Assumptions + +- EB optimized trajectory length should be longer than MPT optimized trajectory length + - since MPT result may be jerky because of non-fixed reference path (= EB optimized trajectory) + - At least, EB fixed optimized trajectory length must be longer than MPT fixed optimization trajectory length + - This causes the case that there is a large difference between MPT fixed optimized point and MPT optimized point just after the point. + +### Drivability in narrow roads + +- set `option.drivability_check.use_vehicle_circles` true + - use a set of circles as a shape of the vehicle when checking if the generated trajectory will be outside the drivable area. +- make `mpt.clearance.soft_clearance_from_road` smaller +- make `mpt.kinematics.optimization_center_offset` different + + - The point on the vehicle, offset forward from the base link` tries to follow the reference path. + + - This may cause the a part of generated trajectory will be outside the drivable area. + +### Computation time + +- Loose EB optimization + + - 1. make `eb.common.delta_arc_length_for_eb` large and `eb.common.num_sampling_points_for_eb` small + - This makes the number of design variables smaller + - Be careful about the trajectory length between MPT and EB as shown in Assumptions. + - However, empirically this causes large turn at the corner (e.g. The vehicle turns a steering wheel to the opposite side (=left) a bit just before the corner turning to right) + - 2. make `eb.qp.eps_abs` and `eb.qp.eps_rel` small + - This causes very unstable reference path generation for MPT, or turning a steering wheel a little bit larger + +- Enable computation reduction flag + + - 1. set l_inf_norm true (by default) + - use L-inf norm optimization for MPT w.r.t. slack variables, resulting in lower number of design variables + - 2. set enable_warm_start true + - 3. set enable_manual_warm_start true (by default) + - 4. set steer_limit_constraint false + - This causes no assumption for trajectory generation where steering angle will not exceeds its hardware limitation + - 5. make the number of collision-free constraints small + - How to change parameters depend on the type of collision-free constraints + - If + - This may cause the trajectory generation where a part of ego vehicle is out of drivable area + +- Disable publishing debug visualization markers + - set `option.is_publishing_*` false + +### Robustness + +- Check if the trajectory before EB, after EB, or after MPT is not robust + - if the trajectory before EB is not robust + - if the trajectory after EB is not robust + - if the trajectory after MPT is not robust + - make `mpt.weight.steer_input_weight` or `mpt.weight.steer_rate_weight` larger, which are stability of steering wheel along the trajectory. + +### Other options + +- `option.skip_optimization` skips EB and MPT optimization. +- `option.enable_pre_smoothing` enables EB which is smoothing the trajectory for MPT. + - EB is not required if the reference path for MPT is smooth enough and does not change its shape suddenly +- `option.is_showing_calculation_time` enables showing each calculation time for functions and total calculation time on the terminal. +- `option.is_stopping_if_outside_drivable_area` enables stopping just before the generated trajectory point will be outside the drivable area. +- `mpt.option.plan_from_ego` enables planning from the ego pose when the ego's velocity is zero. +- `mpt.option.two_step_soft_constraint` enables two step of soft constraints for collision free + - `mpt.option.soft_clearance_from_road` and `mpt.option.soft_second_clearance_from_road` are the weight. + ## Limitation - When turning right or left in the intersection, the output trajectory is close to the outside road boundary. @@ -331,54 +802,51 @@ Although it has a cons to converge to the local minima, it can get a good soluti Topics for debugging will be explained in this section. -- **interpolated_points_marker** - - Trajectory points that is resampled from the input trajectory of obstacle avoidance planner. Whether this trajectory is inside the drivable area, smooth enough can be checked. - -![interpolated_points_marker](./media/interpolated_points_marker.png) - -- **smoothed_points_text** - - The output trajectory points from Elastic Band. Not points but small numbers are visualized. Whether these smoothed points are distorted or not can be checked. +- **Drivable area** + - Drivable area to cover the road. Whether this area is continuous and covers the road can be checked. + - `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area`, whose type is `nav_msgs/msg/OccupancyGrid` -![smoothed_points_text](./media/smoothed_points_text.png) +![drivable_area](./media/drivable_area.png) -- **(base/top/mid)\_bounds_line** - - Lateral Distance to the road boundaries to check collision in MPT (More precisely, - vehicle_width / 2.0). - - This collision check is done with three points on the vehicle (base = back wheel center, top, mid), therefore three line markers are visualized for each trajectory point. - - If the distance between the edge of line markers and the road boundaries is not about the half width of the vehicle, collision check will fail. +- **Path from behavior** + - The input path of obstacle_avoidance_planner. Whether this path is continuous and the curvature is not so high can be checked. + - `Path` or `PathFootprint` rviz plugin. -![bounds_line](./media/bounds_line.png) +![behavior_path](./media/behavior_path.png) -- **optimized_points_marker** - - The output trajectory points from MPT. Whether the trajectory is outside the drivable area can be checked. +- **EB trajectory** + - The output trajectory of elastic band. Whether this trajectory is very smooth and a sampling width is constant can be checked. + - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![optimized_points_marker](./media/optimized_points_marker.png) +![eb_traj](./media/eb_traj.png) -- **Trajectory with footprint** - - Trajectory footprints can be visualized by TrajectoryFootprint of rviz_plugin. Whether trajectory footprints of input/output of obstacle_avoidance_planner is inside the drivable area or not can be checked. +- **MPT reference trajectory** + - The reference trajectory of model predictive trajectory. Whether this trajectory is very smooth and a sampling width is constant can be checked. + - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![trajectory_with_footprint](./media/trajectory_with_footprint.png) +![mpt_ref_traj](./media/mpt_ref_traj.png) -- **Drivable Area** - - Drivable area. When drivable area generation failed, the drawn area may be distorted. - - `nav_msgs/msg/OccupancyGrid` - - topic name: `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area` +- **MPT fixed trajectory** + - The fixed trajectory around the ego of model predictive trajectory. + - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![drivable_area](./media/drivable_area.png) +![mpt_fixed_traj](./media/mpt_fixed_traj.png) -- **area_with_objects** - - Area where obstacles are removed from the drivable area - - `nav_msgs/msg/OccupancyGrid` +- **bounds** + - Lateral Distance to the road or object boundaries to check collision in model predictive trajectory. + - Whether these lines' ends align the road or obstacle boundaries can be checked. + - `bounds*` of `/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker` whose type is `visualization_msgs/msg/MarkerArray` -![area_with_objects](./media/area_with_objects.png) +![bounds](./media/bounds.png) -- **object_clearance_map** - - Cost map for obstacles (distance to the target obstacles to be avoided) - - `nav_msgs/msg/OccupancyGrid` +- **MPT trajectory** + - The output of model predictive trajectory. Whether this trajectory is smooth enough and inside the drivable area can be checked. + - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![object_clearance_map](./media/object_clearance_map.png) +![mpt_traj](./media/mpt_traj.png) -- **clearance_map** - - Cost map for drivable area (distance to the road boundaries) - - `nav_msgs/msg/OccupancyGrid` +- **Output trajectory** + - The output of obstacle_avoidance_planner. Whether this trajectory is smooth enough can be checked. + - `Trajectory` or `TrajectoryFootprint` rviz plugin. -![clearance_map](./media/clearance_map.png) +![output_traj](./media/output_traj.png) diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 11e1f8a905f12..40b64b7d87012 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -1,99 +1,173 @@ /**: ros__parameters: - # trajectory total/fixing length - trajectory_length: 200 # total trajectory length[m] - forward_fixing_distance: 20.0 # forward fixing length from base_link[m] - backward_fixing_distance: 5.0 # backward fixing length from base_link[m] - - # clearance(distance) when generating trajectory - clearance_from_road: 0.2 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road - - # clearance for unique points - clearance_for_straight_line: 0.05 # minimum optimizing range around straight points - clearance_for_joint: 0.1 # minimum optimizing range around joint points - clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing - range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending - clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line - - # avoiding param - max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects[m/s] - max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects[m/s] - center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. center line width around path points used for judging that object is required to avoid or not - acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range - avoiding_object_type: # avoiding object class - unknown: true - car: true - truck: true - bus: true - bicycle: true - motorbike: true - pedestrian: true - animal: true - - # solving quadratic programming - qp_max_iteration: 10000 # max iteration when solving QP - qp_eps_abs: 1.0e-8 # eps abs when solving OSQP - qp_eps_rel: 1.0e-11 # eps rel when solving OSQP - qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending - qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending - qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing - qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing - - # constrain space - is_getting_constraints_close2path_points: true # generate trajectory closer to path points - max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate - coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction - coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction - keep_space_shape_x: 0.2 # keep space for x direction from base_link[m] - keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. keep space for y direction from base_link[m] - max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m] - - is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid - enable_avoidance: true # enable avoidance function - is_using_vehicle_config: true # use vehicle config - num_sampling_points: 100 # number of optimizing points - num_joint_buffer_points: 3 # number of joint buffer points - num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - num_fix_points_for_extending: 50 # number of fixing points when extending - delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m] - delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point[m] - delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0. delta yaw thres for closest point - delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + option: + # publish + is_publishing_debug_visualization_marker: true + is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid + is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid + is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid + + is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area + + # show + is_showing_debug_info: false + is_showing_calculation_time: false + + # other + enable_avoidance: false # enable avoidance function + enable_pre_smoothing: true # enable EB + skip_optimization: false # skip MPT and EB + reset_prev_optimization: false + + common: + # sampling + num_sampling_points: 100 # number of optimizing points + + # trajectory total/fixing length + trajectory_length: 300.0 # total trajectory length[m] + + forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] + forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] + + backward_fixing_distance: 5.0 # backward fixing length from base_link [m] + delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] + + delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] + delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point + delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point + + num_fix_points_for_extending: 50 # number of fixing points when extending + max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] + + object: # avoiding object + max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] + max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] + + avoiding_object_type: + unknown: true + car: true + truck: true + bus: true + bicycle: true + motorbike: true + pedestrian: true + animal: true # mpt param - # vehicle param for mpt - max_steer_deg: 40.0 # max steering angle [deg] - steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] - - # trajectory param for mpt - num_curvature_sampling_points: 5 # number of sampling points when calculating curvature - delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m] - forward_fixing_mpt_distance: 10 # forward fixing distance for MPT - - # optimization param for mpt - is_hard_fixing_terminal_point: false # hard fixing terminal point - base_point_weight: 2000.0 # slack weight for lateral error around base_link - top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point - mid_point_weight: 1000.0 # slack weight for lateral error around the middle point - lat_error_weight: 10.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - steer_input_weight: 0.01 # weight for steering input - steer_rate_weight: 1.0 # weight for steering rate - steer_acc_weight: 0.000001 # weight for steering acceleration - terminal_lat_error_weight: 0.0 # weight for lateral error at terminal point - terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point - terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point - terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point - zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero + mpt: + option: + steer_limit_constraint: true + fix_points_around_ego: true + # plan_from_ego: false + visualize_sampling_num: 1 + enable_manual_warm_start: true + enable_warm_start: true # false + + common: + num_curvature_sampling_points: 5 # number of sampling points when calculating curvature + delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] + + kinematics: + max_steer_deg: 40.0 # max steering angle [deg] + + # If this parameter is commented out, the parameter is set as below by default. + # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8` + # The 0.8 scale is adopted as it performed the best. + # optimization_center_offset: 2.3 # optimization center offset from base link # replanning & trimming trajectory param outside algorithm - min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m] - min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second] - max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m] - distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change + replan: + max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] + max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] + max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] + + # advanced parameters to improve performance as much as possible + advanced: + option: + # check if planned trajectory is outside drivable area + drivability_check: + # true: vehicle shape is considered as a set of circles + # false: vehicle shape is considered as footprint (= rectangle) + use_vehicle_circles: false + + # parameters only when use_vehicle_circles is true + vehicle_circles: + use_manual_vehicle_circles: false + num_for_constraints: 4 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.9 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 + + eb: + common: + num_joint_buffer_points: 3 # number of joint buffer points + num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx + delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. + num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points + + clearance: + clearance_for_straight_line: 0.05 # minimum optimizing range around straight points + clearance_for_joint: 0.1 # minimum optimizing range around joint points + clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing + + qp: + max_iteration: 10000 # max iteration when solving QP + eps_abs: 1.0e-8 # eps abs when solving OSQP + eps_rel: 1.0e-10 # eps rel when solving OSQP + + mpt: + clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory + hard_clearance_from_road: 0.0 # clearance from road boundary[m] + soft_clearance_from_road: 0.1 # clearance from road boundary[m] + soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] + clearance_from_object: 1.0 # clearance from object[m] + extra_desired_clearance_from_road: 0.0 # extra desired clearance from road + + weight: + soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point + soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point + + lat_error_weight: 100.0 # weight for lateral error + yaw_error_weight: 0.0 # weight for yaw error + yaw_error_rate_weight: 0.0 # weight for yaw error rate + steer_input_weight: 10.0 # weight for steering input + steer_rate_weight: 10.0 # weight for steering rate + + obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error + obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error + obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error + near_objects_length: 30.0 # weight for yaw error + + terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point + terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point + terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point + terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point + + # check if planned trajectory is outside drivable area + collision_free_constraints: + option: + l_inf_norm: true + soft_constraint: true + hard_constraint: false + # two_step_soft_constraint: false + + vehicle_circles: + use_manual_vehicle_circles: false + num_for_constraints: 3 + + # parameters only when use_manual_vehicle_circles is true + longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] + radius: 0.95 + + # parameters only when use_manual_vehicle_circles is false + radius_ratio: 0.8 + # If this parameter is commented out, the parameter is calculated automatically + # based on the vehicle length and width + # num_for_radius: 4 diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp new file mode 100644 index 0000000000000..5eba99ae6dfef --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -0,0 +1,255 @@ +// 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_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ + +#include "opencv2/core.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" + +#include + +#include +#include +#include + +struct ReferencePoint; + +struct Bounds; +using VehicleBounds = std::vector; +using SequentialBounds = std::vector; + +using BoundsCandidates = std::vector; +using SequentialBoundsCandidates = std::vector; + +struct CVMaps +{ + cv::Mat drivable_area; + cv::Mat clearance_map; + cv::Mat only_objects_clearance_map; + cv::Mat area_with_objects_map; + nav_msgs::msg::MapMetaData map_info; +}; + +struct QPParam +{ + int max_iteration; + double eps_abs; + double eps_rel; +}; + +struct EBParam +{ + double clearance_for_fixing; + double clearance_for_straight_line; + double clearance_for_joint; + double clearance_for_only_smoothing; + QPParam qp_param; + + int num_joint_buffer_points; + int num_offset_for_begin_idx; + + double delta_arc_length_for_eb; + int num_sampling_points_for_eb; +}; + +struct VehicleParam +{ + double wheelbase; + double length; + double width; + double rear_overhang; + double front_overhang; + // double max_steer_rad; +}; + +struct ConstrainRectangle +{ + geometry_msgs::msg::Point top_left; + geometry_msgs::msg::Point top_right; + geometry_msgs::msg::Point bottom_left; + geometry_msgs::msg::Point bottom_right; + double velocity; + bool is_empty_driveable_area = false; + bool is_including_only_smooth_range = true; +}; + +struct DebugData +{ + struct StreamWithPrint + { + StreamWithPrint & operator<<(const std::string & s) + { + sstream << s; + if (s.back() == '\n') { + std::string tmp_str = sstream.str(); + debug_str += tmp_str; + + if (is_showing_calculation_time) { + tmp_str.pop_back(); // NOTE* remove '\n' which is unnecessary for RCLCPP_INFO_STREAM + RCLCPP_INFO_STREAM(rclcpp::get_logger("obstacle_avoidance_planner.time"), tmp_str); + } + sstream.str(""); + } + return *this; + } + + StreamWithPrint & operator<<(const double d) + { + sstream << d; + return *this; + } + + std::string getString() const { return debug_str; } + + bool is_showing_calculation_time; + std::string debug_str = "\n"; + std::stringstream sstream; + }; + + void init( + const bool local_is_showing_calculation_time, const int local_mpt_visualize_sampling_num, + const geometry_msgs::msg::Pose & local_current_ego_pose, + const double local_vehicle_circle_radius, + const std::vector & local_vehicle_circle_longitudinal_offsets) + { + msg_stream.is_showing_calculation_time = local_is_showing_calculation_time; + mpt_visualize_sampling_num = local_mpt_visualize_sampling_num; + current_ego_pose = local_current_ego_pose; + vehicle_circle_radius = local_vehicle_circle_radius; + vehicle_circle_longitudinal_offsets = local_vehicle_circle_longitudinal_offsets; + } + + StreamWithPrint msg_stream; + size_t mpt_visualize_sampling_num; + geometry_msgs::msg::Pose current_ego_pose; + double vehicle_circle_radius; + std::vector vehicle_circle_longitudinal_offsets; + + boost::optional stop_pose_by_drivable_area = boost::none; + std::vector interpolated_points; + std::vector straight_points; + std::vector fixed_points; + std::vector non_fixed_points; + std::vector constrain_rectangles; + std::vector avoiding_traj_points; + std::vector avoiding_objects; + + cv::Mat clearance_map; + cv::Mat only_object_clearance_map; + cv::Mat area_with_objects_map; + + std::vector> vehicle_circles_pose; + std::vector ref_points; + + std::vector mpt_ref_poses; + std::vector lateral_errors; + + std::vector eb_traj; + std::vector mpt_fixed_traj; + std::vector mpt_ref_traj; + std::vector mpt_traj; + std::vector extended_fixed_traj; + std::vector extended_non_fixed_traj; +}; + +struct Trajectories +{ + std::vector smoothed_trajectory; + std::vector mpt_ref_points; + std::vector model_predictive_trajectory; +}; + +struct TrajectoryParam +{ + bool is_avoiding_unknown; + bool is_avoiding_car; + bool is_avoiding_truck; + bool is_avoiding_bus; + bool is_avoiding_bicycle; + bool is_avoiding_motorbike; + bool is_avoiding_pedestrian; + bool is_avoiding_animal; + int num_sampling_points; + double delta_arc_length_for_trajectory; + double delta_dist_threshold_for_closest_point; + double delta_yaw_threshold_for_closest_point; + double delta_yaw_threshold_for_straight; + double trajectory_length; + double forward_fixing_min_distance; + double forward_fixing_min_time; + double backward_fixing_distance; + double max_avoiding_ego_velocity_ms; + double max_avoiding_objects_velocity_ms; + double center_line_width; + double acceleration_for_non_deceleration_range; + int num_fix_points_for_extending; + double max_dist_for_extending_end_point; +}; + +struct MPTParam +{ + bool enable_warm_start; + bool enable_manual_warm_start; + bool steer_limit_constraint; + bool fix_points_around_ego; + int num_curvature_sampling_points; + + std::vector vehicle_circle_longitudinal_offsets; // from base_link + double vehicle_circle_radius; + + double delta_arc_length_for_mpt_points; + + double hard_clearance_from_road; + double soft_clearance_from_road; + double soft_second_clearance_from_road; + double extra_desired_clearance_from_road; + double clearance_from_object; + double soft_avoidance_weight; + double soft_second_avoidance_weight; + + double lat_error_weight; + double yaw_error_weight; + double yaw_error_rate_weight; + + double near_objects_length; + + double terminal_lat_error_weight; + double terminal_yaw_error_weight; + double terminal_path_lat_error_weight; + double terminal_path_yaw_error_weight; + + double steer_input_weight; + double steer_rate_weight; + + double obstacle_avoid_lat_error_weight; + double obstacle_avoid_yaw_error_weight; + double obstacle_avoid_steer_input_weight; + + double optimization_center_offset; + double max_steer_rad; + + bool soft_constraint; + bool hard_constraint; + bool l_inf_norm; + bool two_step_soft_constraint; + bool plan_from_ego; +}; + +#endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp new file mode 100644 index 0000000000000..8165b523ca0d2 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp @@ -0,0 +1,66 @@ +// 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_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include +#include + +class CostmapGenerator +{ +public: + CVMaps getMaps( + const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & objects, + const TrajectoryParam & traj_param, std::shared_ptr debug_data_ptr); + +private: + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + + cv::Mat getAreaWithObjects( + const cv::Mat & drivable_area, const cv::Mat & objects_image, + std::shared_ptr debug_data_ptr) const; + + cv::Mat getClearanceMap( + const cv::Mat & drivable_area, std::shared_ptr debug_data_ptr) const; + + cv::Mat drawObstaclesOnImage( + const bool enable_avoidance, + const std::vector & objects, + const std::vector & path_points, + const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, + const cv::Mat & clearance_map, const TrajectoryParam & traj_param, + std::vector * debug_avoiding_objects, + std::shared_ptr debug_data_ptr) const; + + cv::Mat getDrivableAreaInCV( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + std::shared_ptr debug_data_ptr) const; +}; +#endif // OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp new file mode 100644 index 0000000000000..b05a6edeb94d7 --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp @@ -0,0 +1,121 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" +#include "opencv2/core.hpp" +#include "opencv2/imgproc/imgproc_c.h" +#include "opencv2/opencv.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "geometry_msgs/msg/point32.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include + +namespace util +{ +struct Footprint; +} + +struct Edges +{ + int front_idx; + int back_idx; + geometry_msgs::msg::Point extended_front; + geometry_msgs::msg::Point extended_back; + geometry_msgs::msg::Point origin; +}; + +struct PolygonPoints +{ + std::vector points_in_image; + std::vector points_in_map; +}; + +namespace cv_utils +{ +void getOccupancyGridValue( + const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value); + +void putOccupancyGridValue( + nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value); +} // namespace cv_utils + +namespace cv_polygon_utils +{ +PolygonPoints getPolygonPoints( + const std::vector & points, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPoints( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPointsFromBB( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPointsFromCircle( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +PolygonPoints getPolygonPointsFromPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info); + +std::vector getCVPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const PolygonPoints & polygon_points, + const std::vector & path_points, + const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info); + +std::vector getDefaultCVPolygon( + const std::vector & points_in_image); + +std::vector getExtendedCVPolygon( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info); + +boost::optional getEdges( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info); +} // namespace cv_polygon_utils + +namespace cv_drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const VehicleParam & vehicle_param); + +bool isOutsideDrivableAreaFromCirclesFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const std::vector vehicle_circle_longitudinal_offsets, + const double vehicle_circle_radius); +} // namespace cv_drivable_area_utils + +#endif // OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug.hpp deleted file mode 100644 index 9baf1b686fa7d..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_ - -#include - -#include -#include -#include -#include - -#include -#include - -struct ConstrainRectangle; -struct Bounds; -struct DebugData; -struct VehicleParam; - -namespace util -{ -struct Footprint; -} - -visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - const DebugData & debug_data, - const std::vector & optimized_points, - const VehicleParam & vehicle_param); - -geometry_msgs::msg::Pose getVirtualWallPose( - const geometry_msgs::msg::Pose & target_pose, const VehicleParam & vehicle_param); - -visualization_msgs::msg::MarkerArray getDebugPointsMarkers( - const std::vector & interpolated_points, - const std::vector & optimized_points, - const std::vector & straight_points, - const std::vector & fixed_points, - const std::vector & non_fixed_points); - -visualization_msgs::msg::MarkerArray getDebugConstrainMarkers( - const std::vector & constrain_ranges, const std::string & ns); - -visualization_msgs::msg::MarkerArray getObjectsMarkerArray( - const std::vector & objects, - const std::string & ns, const double r, const double g, const double b); - -visualization_msgs::msg::MarkerArray getRectanglesMarkerArray( - const std::vector & rects, const std::string & ns, const double r, - const double g, const double b); - -visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray( - const std::vector & rects, const std::string & ns, const double r, - const double g, const double b); - -visualization_msgs::msg::MarkerArray getPointsMarkerArray( - const std::vector & points, const std::string & ns, const double r, - const double g, const double b); - -visualization_msgs::msg::MarkerArray getPointsMarkerArray( - const std::vector & points, const std::string & ns, const double r, - const double g, const double b); - -visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( - const std::vector & points, const std::string & ns, const double r, - const double g, const double b); - -visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( - const std::vector & points, - const std::string & ns, const double r, const double g, const double b); - -visualization_msgs::msg::MarkerArray getBaseBoundsLineMarkerArray( - const std::vector & bounds, const std::vector & candidate_p0, - const std::string & ns, const double r, const double g, const double b); - -visualization_msgs::msg::MarkerArray getTopBoundsLineMarkerArray( - const std::vector & bounds, const std::vector & candidate_p1, - const std::string & ns, const double r, const double g, const double b); - -visualization_msgs::msg::MarkerArray getMidBoundsLineMarkerArray( - const std::vector & bounds, const std::vector & candidate_top, - const std::string & ns, const double r, const double g, const double b); - -visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray( - const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, - const double b); - -visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray( - const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, - const double b); - -nav_msgs::msg::OccupancyGrid getDebugCostmap( - const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid); -#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp new file mode 100644 index 0000000000000..ce7bda4bd1c7f --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp @@ -0,0 +1,50 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_ + +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "opencv2/core.hpp" +#include "rclcpp/clock.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include + +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +namespace debug_visualization +{ +visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( + const std::shared_ptr debug_data_ptr, + const std::vector & optimized_points, + const VehicleParam & vehicle_param, const bool is_showing_debug_detail); + +visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( + const std::shared_ptr debug_data_ptr, const VehicleParam & vehicle_param); + +nav_msgs::msg::OccupancyGrid getDebugCostmap( + const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid); +} // namespace debug_visualization + +#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp index 7eb8d71adbacd..31c9943b4588b 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp @@ -11,266 +11,103 @@ // 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_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ -#include -#include -#include +#include "eigen3/Eigen/Core" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "osqp_interface/osqp_interface.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" -#include -#include -#include -#include -#include -#include -#include +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" -#include +#include "boost/optional.hpp" #include #include -struct Bounds; -struct MPTParam; -struct ReferencePoint; - -struct Anchor -{ - geometry_msgs::msg::Pose pose; - double velocity; -}; - -struct ConstrainLines -{ - double x_coef; - double y_coef; - double lower_bound; - double upper_bound; -}; - -struct Constrain -{ - ConstrainLines top_and_bottom; - ConstrainLines left_and_right; -}; - -struct Rectangle -{ - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; -}; - -struct OccupancyMaps -{ - std::vector> object_occupancy_map; - std::vector> road_occupancy_map; -}; - -namespace autoware::common::osqp -{ -class OSQPInterface; -} - -namespace util -{ -struct Rectangle; -struct Footprint; -} // namespace util - -class MPTOptimizer; - -struct ConstrainRectangle -{ - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; - double velocity; - bool is_empty_driveable_area = false; - bool is_including_only_smooth_range = true; -}; - -struct ConstrainRectangles -{ - ConstrainRectangle object_constrain_rectangle; - ConstrainRectangle road_constrain_rectangle; -}; - -struct CandidatePoints -{ - std::vector fixed_points; - std::vector non_fixed_points; - int begin_path_idx; - int end_path_idx; -}; - -struct QPParam -{ - int max_iteration; - double eps_abs; - double eps_rel; - double eps_abs_for_extending; - double eps_rel_for_extending; - double eps_abs_for_visualizing; - double eps_rel_for_visualizing; -}; - -struct TrajectoryParam -{ - bool is_avoiding_unknown; - bool is_avoiding_car; - bool is_avoiding_truck; - bool is_avoiding_bus; - bool is_avoiding_bicycle; - bool is_avoiding_motorbike; - bool is_avoiding_pedestrian; - bool is_avoiding_animal; - int num_sampling_points; - int num_joint_buffer_points; - int num_joint_buffer_points_for_extending; - int num_offset_for_begin_idx; - int num_fix_points_for_extending; - double delta_arc_length_for_optimization; - double delta_arc_length_for_mpt_points; - double delta_arc_length_for_trajectory; - double delta_dist_threshold_for_closest_point; - double delta_yaw_threshold_for_closest_point; - double delta_yaw_threshold_for_straight; - double trajectory_length; - double forward_fixing_distance; - double forward_fixing_mpt_distance; - double backward_fixing_distance; - double max_avoiding_ego_velocity_ms; - double max_avoiding_objects_velocity_ms; - double center_line_width; - double acceleration_for_non_deceleration_range; - double max_dist_for_extending_end_point; -}; - -struct Trajectories -{ - std::vector smoothed_trajectory; - std::vector mpt_ref_points; - std::vector model_predictive_trajectory; - std::vector extended_trajectory; -}; - -struct ConstrainParam -{ - bool is_getting_constraints_close2path_points; - double clearance_for_fixing; - double clearance_for_straight_line; - double clearance_for_joint; - double range_for_extend_joint; - double clearance_for_only_smoothing; - double clearance_from_object_for_straight; - double min_object_clearance_for_joint; - double min_object_clearance_for_deceleration; - double clearance_from_road; - double clearance_from_object; - double extra_desired_clearance_from_road; - double max_x_constrain_search_range; - double coef_x_constrain_search_resolution; - double coef_y_constrain_search_resolution; - double keep_space_shape_x; - double keep_space_shape_y; - double max_lon_space_for_driveable_constraint; -}; - -struct VehicleParam -{ - double wheelbase; - double length; - double width; - double rear_overhang; - double front_overhang; - double max_steer_rad; - double steer_tau; -}; - -struct FOAData -{ - bool is_avoidance_possible = true; - std::vector avoiding_traj_points; - std::vector constrain_rectangles; -}; - -struct DebugData -{ - bool is_expected_to_over_drivable_area = false; - std::vector interpolated_points; - std::vector straight_points; - std::vector fixed_points; - std::vector non_fixed_points; - std::vector constrain_rectangles; - std::vector fixed_points_for_extending; - std::vector non_fixed_points_for_extending; - std::vector constrain_rectangles_for_extending; - std::vector interpolated_points_for_extending; - std::vector vehicle_footprints; - std::vector current_vehicle_footprints; - std::vector avoiding_traj_points; - std::vector smoothed_points; - std::vector avoiding_objects; - std::vector bounds; - std::vector bounds_candidate_for_base_points; - std::vector bounds_candidate_for_top_points; - std::vector bounds_candidate_for_mid_points; - std::vector fixed_mpt_points; - cv::Mat clearance_map; - cv::Mat only_object_clearance_map; - cv::Mat area_with_objects_map; - FOAData foa_data; -}; - -enum class OptMode : int { - Normal = 0, - Extending = 1, - Visualizing = 2, -}; - class EBPathOptimizer { private: - rclcpp::Clock logger_ros_clock_; + struct CandidatePoints + { + std::vector fixed_points; + std::vector non_fixed_points; + int begin_path_idx; + int end_path_idx; + }; + + struct Anchor + { + geometry_msgs::msg::Pose pose; + double velocity; + }; + + struct ConstrainRectangles + { + ConstrainRectangle object_constrain_rectangle; + ConstrainRectangle road_constrain_rectangle; + }; + + struct ConstrainLines + { + double x_coef; + double y_coef; + double lower_bound; + double upper_bound; + }; + + struct Constrain + { + ConstrainLines top_and_bottom; + ConstrainLines left_and_right; + }; + + struct Rectangle + { + geometry_msgs::msg::Point top_left; + geometry_msgs::msg::Point top_right; + geometry_msgs::msg::Point bottom_left; + geometry_msgs::msg::Point bottom_right; + }; + + struct OccupancyMaps + { + std::vector> object_occupancy_map; + std::vector> road_occupancy_map; + }; const bool is_showing_debug_info_; const double epsilon_; const QPParam qp_param_; const TrajectoryParam traj_param_; - const ConstrainParam constrain_param_; + const EBParam eb_param_; const VehicleParam vehicle_param_; Eigen::MatrixXd default_a_matrix_; - std::unique_ptr keep_space_shape_ptr_; std::unique_ptr osqp_solver_ptr_; - std::unique_ptr ex_osqp_solver_ptr_; - std::unique_ptr vis_osqp_solver_ptr_; - std::unique_ptr mpt_optimizer_ptr_; + double current_ego_vel_; - void initializeSolver(); + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; Eigen::MatrixXd makePMatrix(); Eigen::MatrixXd makeAMatrix(); - geometry_msgs::msg::Pose getOriginPose( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points); - Anchor getAnchor( const std::vector & interpolated_points, const int interpolated_idx, const std::vector & path_points) const; - boost::optional>> getOccupancyPoints( - const geometry_msgs::msg::Pose & origin, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const; - Constrain getConstrainFromConstrainRectangle( const geometry_msgs::msg::Point & interpolated_point, const ConstrainRectangle & constrain_range); @@ -279,119 +116,21 @@ class EBPathOptimizer const double dx, const double dy, const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & opposite_point); - ConstrainRectangles getConstrainRectangles( - const Anchor & anchor, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info) const; - ConstrainRectangle getConstrainRectangle(const Anchor & anchor, const double clearance) const; ConstrainRectangle getConstrainRectangle( - const Anchor & anchor, const int & nearest_idx, - const std::vector & interpolated_points, - const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) const; - - ConstrainRectangle getConstrainRectangle( - const std::vector> & occupancy_map, - const std::vector> & occupancy_points, - const Anchor & anchor, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map) const; - - ConstrainRectangle getConstrainRectangle( - const std::vector & path_points, - const Anchor & anchor, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const; - - ConstrainRectangle getConstrainRectangle( - const std::vector> & occupancy_points, - const util::Rectangle & util_rect, const Anchor & anchor) const; - - ConstrainRectangle getUpdatedConstrainRectangle( - const ConstrainRectangle & rectangle, const geometry_msgs::msg::Point & candidate_point, - const nav_msgs::msg::MapMetaData & map_info, const cv::Mat & only_objects_clearance_map) const; - - OccupancyMaps getOccupancyMaps( - const std::vector> & occupancy_points, - const geometry_msgs::msg::Pose & origin_pose, - const geometry_msgs::msg::Point & origin_point_in_image, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info) const; + const Anchor & anchor, const double min_x, const double max_x, const double min_y, + const double max_y) const; int getStraightLineIdx( const std::vector & interpolated_points, - const int farthest_point_idx, const cv::Mat & only_objects_clearance, - const nav_msgs::msg::MapMetaData & map_info, + const int farthest_point_idx, std::vector & debug_detected_straight_points); - int getEndPathIdx( - const std::vector & path_points, - const int begin_path_idx, const double required_trajectory_length); - - int getEndPathIdxInsideArea( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const int begin_path_idx, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info); - - boost::optional> getPostProcessedConstrainRectangles( - const bool enable_avoidance, const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - const int farthest_point_idx, const int num_fixed_points, const int straight_idx, - DebugData * debug_data) const; - - boost::optional> getValidConstrainRectangles( - const std::vector & constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const; - - boost::optional> getConstrainRectanglesClose2PathPoints( - const bool is_using_only_smooth_constrain, const bool is_using_road_constrain, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const; - - boost::optional> getConstrainRectanglesWithinArea( - const bool is_using_only_smooth_constrain, const bool is_using_road_constrain, - const int farthest_point_idx, const int num_fixed_points, const int straight_idx, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - DebugData * debug_data) const; - - bool isPreFixIdx( - const int target_idx, const int farthest_point_idx, const int num_fixed, - const int straight_idx) const; - - bool isClose2Object( - const geometry_msgs::msg::Point & point, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map, const double distance_threshold) const; - - boost::optional> getConstrainRectangleVec( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data); - boost::optional> getConstrainRectangleVec( const autoware_auto_planning_msgs::msg::Path & path, const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map); - - std::vector getConstrainRectangleVec( - const std::vector & input_path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx); - - Rectangle getRelShapeRectangle( - const geometry_msgs::msg::Vector3 & vehicle_shape, - const geometry_msgs::msg::Pose & origin) const; - - Rectangle getAbsShapeRectangle( - const Rectangle & rel_shape_rectangle_points, const geometry_msgs::msg::Point & offset_point, - const geometry_msgs::msg::Pose & origin) const; + const int farthest_point_idx, const int straight_idx); std::vector getPaddedInterpolatedPoints( const std::vector & interpolated_points, const int farthest_idx); @@ -402,18 +141,12 @@ class EBPathOptimizer boost::optional> getOptimizedTrajectory( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const CandidatePoints & candidate_points, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data); - - std::vector getExtendedOptimizedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points, - DebugData * debug_data); + const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, + std::shared_ptr debug_data_ptr); void updateConstrain( const std::vector & interpolated_points, - const std::vector & rectangle_points, const OptMode & opt_mode); + const std::vector & rectangle_points); std::vector convertOptimizedPointsToTrajectory( const std::vector optimized_points, const std::vector & constraint, @@ -422,50 +155,32 @@ class EBPathOptimizer std::vector getFixedPoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_optimized_points, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info); + const std::unique_ptr & prev_optimized_points); CandidatePoints getCandidatePoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info, DebugData * debug_data); - - bool isPointInsideDrivableArea( - const geometry_msgs::msg::Point & point, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info); + const std::unique_ptr & prev_trajs, std::shared_ptr debug_data_ptr); CandidatePoints getDefaultCandidatePoints( const std::vector & path_points); - std::vector solveQP(const OptMode & opt_mode); - - bool isFixingPathPoint( - const std::vector & path_points) const; + std::vector solveQP(); std::vector calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, - const OptMode & opt_mode); - - FOAData getFOAData( - const std::vector & rectangles, - const std::vector & interpolated_points, const int farthest_idx); + std::shared_ptr debug_data_ptr); public: EBPathOptimizer( - const bool is_showing_debug_info, const QPParam qp_param, const TrajectoryParam traj_param, - const ConstrainParam constrain_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param); - - ~EBPathOptimizer(); + const bool is_showing_debug_info, const TrajectoryParam & traj_param, const EBParam & eb_param, + const VehicleParam & vehicle_param); - boost::optional generateOptimizedTrajectory( - const bool enable_avoidance, const geometry_msgs::msg::Pose & ego_pose, - const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, - const std::vector & objects, - DebugData * debug_data); + boost::optional> getEBTrajectory( + const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, + const std::unique_ptr & prev_trajs, const double current_ego_vel, + std::shared_ptr debug_data_ptr); }; #endif // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index b8e760771d6f9..f7ff8c9f6c1e6 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -40,37 +40,77 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Sparse" +#include "interpolation/linear_interpolation.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "osqp_interface/osqp_interface.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" -#include -#include -#include - -#include +#include "boost/optional.hpp" #include #include -namespace cv -{ -class Mat; -} +enum class CollisionType { NO_COLLISION = 0, OUT_OF_SIGHT = 1, OUT_OF_ROAD = 2, OBJECT = 3 }; -namespace autoware::common::osqp +struct Bounds { -class OSQPInterface; -} - -struct DebugData; -struct TrajectoryParam; -struct QPParam; -struct ConstrainParam; -struct VehicleParam; -struct Rectangle; -struct CVMaps; -struct Trajectories; + Bounds() = default; + Bounds( + const double lower_bound_, const double upper_bound_, CollisionType lower_collision_type_, + CollisionType upper_collision_type_) + : lower_bound(lower_bound_), + upper_bound(upper_bound_), + lower_collision_type(lower_collision_type_), + upper_collision_type(upper_collision_type_) + { + } + + double lower_bound; + double upper_bound; + + CollisionType lower_collision_type; + CollisionType upper_collision_type; + + bool hasCollisionWithRightObject() const { return lower_collision_type == CollisionType::OBJECT; } + + bool hasCollisionWithLeftObject() const { return upper_collision_type == CollisionType::OBJECT; } + + bool hasCollisionWithObject() const + { + return hasCollisionWithRightObject() || hasCollisionWithLeftObject(); + } + + static Bounds lerp(Bounds prev_bounds, Bounds next_bounds, double ratio) + { + const double lower_bound = + interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); + const double upper_bound = + interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); + + if (ratio < 0.5) { + return Bounds{ + lower_bound, upper_bound, prev_bounds.lower_collision_type, + prev_bounds.upper_collision_type}; + } + + return Bounds{ + lower_bound, upper_bound, next_bounds.lower_collision_type, next_bounds.upper_collision_type}; + } + + void translate(const double offset) + { + lower_bound -= offset; + upper_bound -= offset; + } +}; struct ReferencePoint { @@ -78,220 +118,199 @@ struct ReferencePoint double k = 0; double v = 0; double yaw = 0; - geometry_msgs::msg::Quaternion q; double s = 0; - geometry_msgs::msg::Pose top_pose; - geometry_msgs::msg::Pose mid_pose; - double delta_yaw_from_p1 = 0; - double delta_yaw_from_p2 = 0; - boost::optional fix_state = boost::none; - Eigen::VectorXd optimized_state; + double alpha = 0.0; + Bounds bounds; + bool near_objects; + + // NOTE: fix_kinematic_state is used for two purposes + // one is fixing points around ego for stability + // second is fixing current ego pose when no velocity for planning from ego pose + boost::optional fix_kinematic_state = boost::none; + bool plan_from_ego = false; + Eigen::Vector2d optimized_kinematic_state; + double optimized_input; + + // + std::vector> beta; + VehicleBounds vehicle_bounds; + + // SequentialBoundsCandidates sequential_bounds_candidates; + std::vector vehicle_bounds_poses; // for debug visualization }; -struct Bounds +class MPTOptimizer { - struct SingleBounds +private: + struct MPTMatrix { - SingleBounds & operator=(std::vector & bounds) - { - ub = bounds[0]; - lb = bounds[1]; - return *this; - } - double ub; // left - double lb; // right - } c0, c1, c2; -}; - -struct MPTTrajs -{ - std::vector mpt; - std::vector ref_points; -}; + // Eigen::MatrixXd Aex; + Eigen::MatrixXd Bex; + Eigen::VectorXd Wex; + // Eigen::SparseMatrix Cex; + // Eigen::SparseMatrix Qex; + // Eigen::SparseMatrix Rex; + // Eigen::MatrixXd R1ex; + // Eigen::MatrixXd R2ex; + // Eigen::MatrixXd Uref_ex; + }; + + struct ValueMatrix + { + Eigen::SparseMatrix Qex; + Eigen::SparseMatrix Rex; + }; -struct MPTMatrix -{ - Eigen::MatrixXd Aex; - Eigen::MatrixXd Bex; - Eigen::MatrixXd Wex; - Eigen::MatrixXd Cex; - Eigen::MatrixXd Qex; - Eigen::MatrixXd R1ex; - Eigen::MatrixXd R2ex; - Eigen::MatrixXd Uref_ex; -}; + struct ObjectiveMatrix + { + Eigen::MatrixXd hessian; + Eigen::VectorXd gradient; + }; -struct ObjectiveMatrix -{ - Eigen::MatrixXd hessian; - std::vector gradient; -}; + struct ConstraintMatrix + { + Eigen::MatrixXd linear; + Eigen::VectorXd lower_bound; + Eigen::VectorXd upper_bound; + }; -struct ConstraintMatrix -{ - Eigen::MatrixXd linear; - std::vector lower_bound; - std::vector upper_bound; -}; + struct MPTTrajs + { + std::vector mpt; + std::vector ref_points; + }; -struct MPTParam -{ - bool is_hard_fixing_terminal_point; - int num_curvature_sampling_points; - double base_point_dist_from_base_link; - double top_point_dist_from_base_link; - double mid_point_dist_from_base_link; - double clearance_from_road; - double clearance_from_object; - double base_point_weight; - double top_point_weight; - double mid_point_weight; - double lat_error_weight; - double yaw_error_weight; - double steer_input_weight; - double steer_rate_weight; - double steer_acc_weight; - double terminal_lat_error_weight; - double terminal_yaw_error_weight; - double terminal_path_lat_error_weight; - double terminal_path_yaw_error_weight; - double zero_ff_steer_angle; -}; + const double osqp_epsilon_ = 1.0e-3; -class MPTOptimizer -{ -private: bool is_showing_debug_info_; - - std::unique_ptr osqp_solver_ptr_; - std::unique_ptr mpt_param_ptr_; - std::unique_ptr traj_param_ptr_; - std::unique_ptr qp_param_ptr_; - std::unique_ptr constraint_param_ptr_; - std::unique_ptr vehicle_param_ptr_; + TrajectoryParam traj_param_; + VehicleParam vehicle_param_; + MPTParam mpt_param_; std::unique_ptr vehicle_model_ptr_; + std::unique_ptr osqp_solver_ptr_; - std::vector convertToReferencePoints( - const std::vector & points, - const std::vector & path_points, - const std::unique_ptr & prev_mpt_points, - const geometry_msgs::msg::Pose & ego_pose, const CVMaps & maps, DebugData * debug_data) const; + geometry_msgs::msg::Pose current_ego_pose_; + double current_ego_vel_; + + int prev_mat_n = 0; + int prev_mat_m = 0; + + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; std::vector getReferencePoints( - const geometry_msgs::msg::Pose & origin_pose, const geometry_msgs::msg::Pose & ego_pose, const std::vector & points, - const std::vector & path_points, - const std::unique_ptr & prev_mpt_points, const CVMaps & maps, - DebugData * debug_data) const; + const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const; - std::vector getBaseReferencePoints( - const std::vector & interpolated_points, - const std::unique_ptr & prev_trajs, DebugData * debug_data) const; + void calcPlanningFromEgo(std::vector & ref_points) const; - void calcOrientation(std::vector * ref_points) const; + /* + std::vector convertToReferencePoints( + const std::vector & points, + const std::unique_ptr & prev_mpt_points, const bool enable_avoidance, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const; + */ - void calcVelocity(std::vector * ref_points) const; + std::vector getFixedReferencePoints( + const std::vector & points, + const std::unique_ptr & prev_trajs) const; - void calcVelocity( - std::vector * ref_points, - const std::vector & points) const; + void calcBounds( + std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, + std::shared_ptr debug_data_ptr) const; - void calcCurvature(std::vector * ref_points) const; + void calcVehicleBounds( + std::vector & ref_points, const CVMaps & maps, + std::shared_ptr debug_data_ptr, const bool enable_avoidance) const; - void calcArcLength(std::vector * ref_points) const; + // void calcFixState( + // std::vector & ref_points, + // const std::unique_ptr & prev_trajs) const; - void calcExtraPoints(std::vector * ref_points) const; + void calcOrientation(std::vector & ref_points) const; - void calcFixPoints( - const std::unique_ptr & prev_trajs, const geometry_msgs::msg::Pose & ego_pose, - std::vector * ref_points, DebugData * debug_data) const; + void calcVelocity(std::vector & ref_points) const; - void calcInitialState( - std::vector * ref_points, const geometry_msgs::msg::Pose & origin_pose) const; + void calcVelocity( + std::vector & ref_points, + const std::vector & points) const; + + void calcCurvature(std::vector & ref_points) const; - void setOptimizedState(ReferencePoint * ref_point, const Eigen::Vector3d & optimized_state) const; + void calcArcLength(std::vector & ref_points) const; + + void calcExtraPoints( + std::vector & ref_points, + const std::unique_ptr & prev_trajs) const; /* * predict equation: Xec = Aex * x0 + Bex * Uex + Wex * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * * Uex Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) */ - boost::optional generateMPTMatrix( + MPTMatrix generateMPTMatrix( const std::vector & reference_points, - const std::vector & path_points, - const std::unique_ptr & prev_trajs) const; + std::shared_ptr debug_data_ptr) const; - void addSteerWeightR(Eigen::MatrixXd * R, const std::vector & ref_points) const; + ValueMatrix generateValueMatrix( + const std::vector & reference_points, + const std::vector & path_points, + std::shared_ptr debug_data_ptr) const; - void addSteerWeightF(Eigen::VectorXd * f) const; + void addSteerWeightR( + std::vector> & Rex_triplet_vec, + const std::vector & ref_points) const; boost::optional executeOptimization( - const bool enable_avoidance, const MPTMatrix & m, - const std::vector & ref_points, - const std::vector & path_points, - const CVMaps & maps, DebugData * debug_data); + const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, + const std::vector & ref_points, std::shared_ptr debug_data_ptr); std::vector getMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpc_matrix, - const std::vector & optimized_points) const; - - double calcLateralError( - const geometry_msgs::msg::Point & target_point, const ReferencePoint & ref_point) const; - - Eigen::VectorXd getState( - const geometry_msgs::msg::Pose & ego_pose, const ReferencePoint & nearest_ref_point) const; - - std::vector getReferenceBounds( - const bool enable_avoidance, const std::vector & ref_points, - const CVMaps & maps, DebugData * debug_data) const; + std::vector & fixed_ref_points, + std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, + const MPTMatrix & mpt_matrix, std::shared_ptr debug_data_ptr); - boost::optional> getBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const; + std::vector getMPTFixedPoints( + const std::vector & ref_points) const; - boost::optional> getBoundCandidate( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps, - const double min_road_clearance, const double min_obj_clearance, const double rel_initial_lat, - const std::vector & rough_road_bound) const; + BoundsCandidates getBoundsCandidates( + const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const; - boost::optional> getRoughBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const; + CollisionType getCollisionType( + const CVMaps & maps, const bool enable_avoidance, + const geometry_msgs::msg::Pose & avoiding_point, const double traversed_dist, + const double bound_angle) const; - double getTraversedDistance( - const bool enable_avoidance, const ReferencePoint & ref_point, const double traverse_angle, - const double initial_value, const CVMaps & maps, const double min_road_clearance, - const double min_obj_clearance, const bool search_expanding_side = false) const; - - boost::optional getValidLatError( - const bool enable_avoidance, const ReferencePoint & ref_point, const double initial_value, - const CVMaps & maps, const double min_road_clearance, const double min_obj_clearance, - const std::vector & rough_road_bound, const bool search_expanding_side = false) const; - - // TODO(unknown): refactor replace all relevant funcs boost::optional getClearance( const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, const nav_msgs::msg::MapMetaData & map_info) const; - ObjectiveMatrix getObjectiveMatrix(const Eigen::VectorXd & x0, const MPTMatrix & m) const; + ObjectiveMatrix getObjectiveMatrix( + const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat, + [[maybe_unused]] const std::vector & ref_points, + std::shared_ptr debug_data_ptr) const; ConstraintMatrix getConstraintMatrix( - const bool enable_avoidance, const Eigen::VectorXd & x0, const MPTMatrix & m, - const CVMaps & maps, const std::vector & ref_points, - const std::vector & path_points, - DebugData * debug_data) const; + const bool enable_avoidance, const MPTMatrix & mpt_mat, + const std::vector & ref_points, + std::shared_ptr debug_data_ptr) const; public: MPTOptimizer( - const bool is_showing_debug_info, const QPParam & qp_param, const TrajectoryParam & traj_param, - const ConstrainParam & constrain_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param); + const bool is_showing_debug_info, const TrajectoryParam & traj_param, + const VehicleParam & vehicle_param, const MPTParam & mpt_param); boost::optional getModelPredictiveTrajectory( const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, const std::unique_ptr & prev_trajs, const CVMaps & maps, - const geometry_msgs::msg::Pose & ego_pose, DebugData * debug_data); + const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, + std::shared_ptr debug_data_ptr); }; #endif // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index e184847cd7e7f..6f38883aa4790 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -14,207 +14,229 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "obstacle_avoidance_planner/costmap_generator.hpp" +#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "opencv2/core.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/ros/self_pose_listener.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/string_stamped.hpp" +#include "tier4_planning_msgs/msg/enable_avoidance.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "boost/optional.hpp" #include -#include #include -namespace ros +namespace { -class Time; -} - -namespace cv +template +double lerpTwistX( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { -class Mat; + constexpr double epsilon = 1e-6; + + const double closest_to_target_dist = + tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); + const double seg_dist = + tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); + + const double closest_vel = points[closest_seg_idx].longitudinal_velocity_mps; + const double next_vel = points[closest_seg_idx + 1].longitudinal_velocity_mps; + + return std::abs(seg_dist) < epsilon + ? next_vel + : interpolation::lerp(closest_vel, next_vel, closest_to_target_dist / seg_dist); } -namespace tf2_ros +template +double lerpPoseZ( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { -class Buffer; -class TransformListener; -} // namespace tf2_ros + constexpr double epsilon = 1e-6; -class EBPathOptimizer; + const double closest_to_target_dist = + tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); + const double seg_dist = + tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); -struct QPParam; -struct TrajectoryParam; -struct ConstrainParam; -struct VehicleParam; -struct MPTParam; -struct DebugData; -struct Trajectories; + const double closest_z = points[closest_seg_idx].pose.position.z; + const double next_z = points[closest_seg_idx + 1].pose.position.z; + + return std::abs(seg_dist) < epsilon + ? next_z + : interpolation::lerp(closest_z, next_z, closest_to_target_dist / seg_dist); +} +} // namespace class ObstacleAvoidancePlanner : public rclcpp::Node { private: OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rclcpp::Clock logger_ros_clock_; + int eb_solved_count_; + bool is_publishing_debug_visualization_marker_; bool is_publishing_area_with_objects_; + bool is_publishing_object_clearance_map_; bool is_publishing_clearance_map_; bool is_showing_debug_info_; - bool is_using_vehicle_config_; + bool is_showing_calculation_time_; bool is_stopping_if_outside_drivable_area_; bool enable_avoidance_; - const int min_num_points_for_getting_yaw_; - std::mutex mutex_; - - // params outside logic - double min_delta_dist_for_replan_; - double min_delta_time_sec_for_replan_; - double max_dist_for_extending_end_point_; - double distance_for_path_shape_change_detection_; + bool enable_pre_smoothing_; + bool skip_optimization_; + bool reset_prev_optimization_; + + // vehicle circles info for drivability check + bool use_vehicle_circles_for_drivability_; + bool use_manual_vehicle_circles_for_drivability_; + int vehicle_circle_constraints_num_for_drivability_; + int vehicle_circle_radius_num_for_drivability_; + double vehicle_circle_radius_ratio_for_drivability_; + double vehicle_circle_radius_for_drivability_; + std::vector vehicle_circle_longitudinal_offsets_for_drivability_; + + // vehicle circles info for for mpt constraints + bool use_manual_vehicle_circles_for_mpt_; + int vehicle_circle_constraints_num_for_mpt_; + int vehicle_circle_radius_num_for_mpt_; + double vehicle_circle_radius_ratio_for_mpt_; + + // params for replan + double max_path_shape_change_dist_for_replan_; + double max_ego_moving_dist_for_replan_; + double max_delta_time_sec_for_replan_; // logic + std::unique_ptr costmap_generator_ptr_; std::unique_ptr eb_path_optimizer_ptr_; + std::unique_ptr mpt_optimizer_ptr_; // params - std::unique_ptr qp_param_; - std::unique_ptr traj_param_; - std::unique_ptr constrain_param_; - std::unique_ptr vehicle_param_; - std::unique_ptr mpt_param_; - - std::unique_ptr current_ego_pose_ptr_; + TrajectoryParam traj_param_; + EBParam eb_param_; + VehicleParam vehicle_param_; + MPTParam mpt_param_; + int mpt_visualize_sampling_num_; + + // debug + mutable std::shared_ptr debug_data_ptr_; + mutable tier4_autoware_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + + geometry_msgs::msg::Pose current_ego_pose_; std::unique_ptr current_twist_ptr_; std::unique_ptr prev_ego_pose_ptr_; - std::unique_ptr prev_trajectories_ptr_; + std::unique_ptr prev_optimal_trajs_ptr_; std::unique_ptr> prev_path_points_ptr_; - std::unique_ptr in_objects_ptr_; + std::unique_ptr objects_ptr_; - // TF - std::unique_ptr tf_buffer_ptr_; - std::unique_ptr tf_listener_ptr_; - std::unique_ptr prev_replanned_time_ptr_; + std::unique_ptr latest_replanned_time_ptr_; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; // ROS - rclcpp::Publisher::SharedPtr trajectory_pub_; - rclcpp::Publisher::SharedPtr avoiding_traj_pub_; + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr + debug_extended_fixed_traj_pub_; rclcpp::Publisher::SharedPtr - debug_smoothed_points_pub_; - rclcpp::Publisher::SharedPtr - is_avoidance_possible_pub_; + debug_extended_non_fixed_traj_pub_; + rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; + rclcpp::Publisher::SharedPtr + debug_mpt_fixed_traj_pub_; + rclcpp::Publisher::SharedPtr + debug_mpt_ref_traj_pub_; + rclcpp::Publisher::SharedPtr debug_mpt_traj_pub_; + rclcpp::Publisher::SharedPtr debug_markers_pub_; + rclcpp::Publisher::SharedPtr debug_wall_markers_pub_; rclcpp::Publisher::SharedPtr debug_clearance_map_pub_; rclcpp::Publisher::SharedPtr debug_object_clearance_map_pub_; rclcpp::Publisher::SharedPtr debug_area_with_objects_pub_; + rclcpp::Publisher::SharedPtr debug_msg_pub_; + rclcpp::Subscription::SharedPtr path_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr is_avoidance_sub_; - // callback functions - void pathCallback(const autoware_auto_planning_msgs::msg::Path::SharedPtr); + // param callback function + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + // subscriber callback functions void odomCallback(const nav_msgs::msg::Odometry::SharedPtr); void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr); void enableAvoidanceCallback(const tier4_planning_msgs::msg::EnableAvoidance::SharedPtr); + void pathCallback(const autoware_auto_planning_msgs::msg::Path::SharedPtr); - void initialize(); - - // generate fine trajectory - std::vector generatePostProcessedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::vector & merged_optimized_points) - const; - - bool needReplan( - const geometry_msgs::msg::Pose & ego_pose, - const std::unique_ptr & prev_ego_pose, - const std::vector & path_points, - const std::unique_ptr & previous_replanned_time, - const std::unique_ptr> & - prev_path_points, - std::unique_ptr & prev_traj_points); + // functions + void resetPlanning(); + void resetPrevOptimization(); std::vector generateOptimizedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & input_path); - std::unique_ptr getCurrentEgoPose(); - - bool isPathShapeChanged( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr> & - prev_path_points); + bool checkReplan(const std::vector & path_points); - autoware_auto_planning_msgs::msg::Trajectory generateTrajectory( - const autoware_auto_planning_msgs::msg::Path & in_path); + Trajectories optimizeTrajectory( + const autoware_auto_planning_msgs::msg::Path & path, const CVMaps & cv_maps); - std::vector convertPointsToTrajectory( - const std::vector & path_points, - const std::vector & trajectory_points) const; + Trajectories getPrevTrajs( + const std::vector & path_points) const; - void publishingDebugData( - const DebugData & debug_data, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & traj_points, - const VehicleParam & vehicle_param); + void insertZeroVelocityOutsideDrivableArea( + std::vector & traj_points, + const CVMaps & cv_maps); - int calculateNonDecelerationRange( - const std::vector & traj_points, - const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Twist & ego_twist) const; + void publishDebugDataInOptimization( + const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & traj_points); - Trajectories getTrajectoryInsideArea( - const Trajectories & trajs, + Trajectories makePrevTrajectories( const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data) const; + const Trajectories & trajs); - boost::optional calcTrajectoryInsideArea( - const Trajectories & trajs, + std::vector generatePostProcessedTrajectory( const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data, const bool is_prev_traj = false) const; - - Trajectories getPrevTrajs( - const std::vector & path_points) const; - - std::vector getPrevTrajectory( - const std::vector & path_points) const; + const std::vector & merged_optimized_points); - Trajectories makePrevTrajectories( - const geometry_msgs::msg::Pose & ego_pose, + std::vector getExtendedTrajectory( const std::vector & path_points, - const Trajectories & trajs) const; + const std::vector & optimized_points); - Trajectories getBaseTrajectory( + std::vector generateFineTrajectoryPoints( const std::vector & path_points, - const Trajectories & current_trajs) const; + const std::vector & traj_points) const; - boost::optional getStopIdx( + std::vector alignVelocity( + const std::vector & fine_traj_points, const std::vector & path_points, - const Trajectories & trajs, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & road_clearance_map, DebugData * debug_data) const; - - void declareObstacleAvoidancePlannerParameters(); + const std::vector & traj_points) const; - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); + void publishDebugDataInMain(const autoware_auto_planning_msgs::msg::Path & path) const; public: explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); - ~ObstacleAvoidancePlanner(); }; #endif // OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp deleted file mode 100644 index 1ad7a77cf416a..0000000000000 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/util.hpp +++ /dev/null @@ -1,203 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTIL_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTIL_HPP_ - -#include -#include - -#include -#include - -#include - -#include -#include - -struct VehicleParam; -struct ReferencePoint; -struct Trajectories; -struct TrajectoryParam; - -namespace util -{ -template -geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const T & point, const geometry_msgs::msg::Pose & origin); - -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin); - -double calculate2DDistance( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b); - -double calculateSquaredDistance( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b); - -double getYawFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); - -double normalizeRadian(const double angle); - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); - -geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double yaw); - -template -geometry_msgs::msg::Point transformMapToImage( - const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info); - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info); - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point); - -bool interpolate2DPoints( - const std::vector & x, const std::vector & y, const double resolution, - std::vector & interpolated_points); - -std::vector getInterpolatedPoints( - const std::vector & first_points, - const std::vector & second_points, const double delta_arc_length); - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length); - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length); - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length); - -template -std::vector getInterpolatedPoints( - const T & points, const double delta_arc_length); - -template -int getNearestIdx( - const T & points, const geometry_msgs::msg::Pose & pose, const int default_idx, - const double delta_yaw_threshold); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const int default_idx, const double delta_yaw_threshold); - -int getNearestIdxOverPoint( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const int default_idx, const double delta_yaw_threshold); - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point, const int default_idx); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Point & point); - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point); - -int getNearestIdx( - const std::vector & points, const double target_s, const int begin_idx); - -template -int getNearestPointIdx(const T & points, const geometry_msgs::msg::Point & point); - -std::vector convertPathToTrajectory( - const std::vector & path_points); - -std::vector -convertPointsToTrajectoryPointsWithYaw(const std::vector & points); - -std::vector fillTrajectoryWithVelocity( - const std::vector & traj_points, - const double velocity); - -template -std::vector alignVelocityWithPoints( - const std::vector & traj_points, - const T & points, const int zero_velocity_traj_idx, const int max_skip_comparison_idx); - -struct Rectangle -{ - int min_x_idx = 0; - int min_y_idx = 0; - int max_x_idx = 0; - int max_y_idx = 0; - int area = 0; -}; - -std::vector> getHistogramTable(const std::vector> & input); - -Rectangle getLargestRectangleInRow( - const std::vector & histo, const int current_row, const int row_size); - -Rectangle getLargestRectangle(const std::vector> & input); - -boost::optional getLastExtendedPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold); - -boost::optional getLastExtendedTrajPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold); - -struct Footprint -{ - geometry_msgs::msg::Point p; - geometry_msgs::msg::Point top_left; - geometry_msgs::msg::Point top_right; - geometry_msgs::msg::Point bottom_left; - geometry_msgs::msg::Point bottom_right; -}; - -std::vector getVehicleFootprints( - const std::vector & optimized_points, - const VehicleParam & vehicle_param); - -std::vector calcEuclidDist(const std::vector & x, const std::vector & y); - -bool hasValidNearestPointFromEgo( - const geometry_msgs::msg::Pose & ego_pose, const Trajectories & trajs, - const TrajectoryParam & traj_param); - -std::vector concatTraj( - const Trajectories & trajs); - -int getZeroVelocityIdx( - const bool is_showing_debug_info, const std::vector & fine_points, - const std::vector & path_points, - const std::unique_ptr & opt_trajs, const TrajectoryParam & traj_param); - -template -int getZeroVelocityIdxFromPoints( - const T & points, const std::vector & fine_points, - const int /* default_idx */, const TrajectoryParam & traj_param); - -template -double getArcLength(const T & points, const int initial_idx = 0); - -double getArcLength( - const std::vector & points, const int initial_idx = 0); - -void logOSQPSolutionStatus(const int solution_status); - -} // namespace util - -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTIL_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp new file mode 100644 index 0000000000000..602c5db22e0ac --- /dev/null +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -0,0 +1,336 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ +#define OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ + +#include "eigen3/Eigen/Core" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "obstacle_avoidance_planner/common_structs.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include "boost/optional/optional_fwd.hpp" + +#include +#include +#include + +struct ReferencePoint; + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const ReferencePoint & p); + +template <> +geometry_msgs::msg::Pose getPose(const ReferencePoint & p); +} // namespace tier4_autoware_utils + +namespace geometry_utils +{ +template +geometry_msgs::msg::Point transformToRelativeCoordinate2D( + const T & point, const geometry_msgs::msg::Pose & origin) +{ + // NOTE: implement transformation without defining yaw variable + // but directly sin/cos of yaw for fast calculation + const auto & q = origin.orientation; + const double cos_yaw = 1 - 2 * q.z * q.z; + const double sin_yaw = 2 * q.w * q.z; + + geometry_msgs::msg::Point relative_p; + const double tmp_x = point.x - origin.position.x; + const double tmp_y = point.y - origin.position.y; + relative_p.x = tmp_x * cos_yaw + tmp_y * sin_yaw; + relative_p.y = -tmp_x * sin_yaw + tmp_y * cos_yaw; + relative_p.z = point.z; + + return relative_p; +} + +geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin); + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root); + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); + +template +geometry_msgs::msg::Point transformMapToImage( + const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info) +{ + geometry_msgs::msg::Point relative_p = + transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); + double resolution = occupancy_grid_info.resolution; + double map_y_height = occupancy_grid_info.height; + double map_x_width = occupancy_grid_info.width; + double map_x_in_image_resolution = relative_p.x / resolution; + double map_y_in_image_resolution = relative_p.y / resolution; + geometry_msgs::msg::Point image_point; + image_point.x = map_y_height - map_y_in_image_resolution; + image_point.y = map_x_width - map_x_in_image_resolution; + return image_point; +} + +boost::optional transformMapToOptionalImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info); + +bool transformMapToImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point); +} // namespace geometry_utils + +namespace interpolation_utils +{ +std::vector interpolate2DPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution, + const double offset); + +std::vector interpolateConnected2DPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution, + const double begin_yaw, const double end_yaw); + +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, + const std::vector & base_yaw, const double resolution); + +template +std::vector getInterpolatedPoints( + const T & points, const double delta_arc_length, const double offset = 0) +{ + constexpr double epsilon = 1e-6; + std::vector tmp_x; + std::vector tmp_y; + for (size_t i = 0; i < points.size(); i++) { + const auto & current_point = tier4_autoware_utils::getPoint(points.at(i)); + if (i > 0) { + const auto & prev_point = tier4_autoware_utils::getPoint(points.at(i - 1)); + if ( + std::fabs(current_point.x - prev_point.x) < epsilon && + std::fabs(current_point.y - prev_point.y) < epsilon) { + continue; + } + } + tmp_x.push_back(current_point.x); + tmp_y.push_back(current_point.y); + } + + return interpolation_utils::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, offset); +} + +std::vector getInterpolatedTrajectoryPoints( + const std::vector & points, + const double delta_arc_length); + +std::vector getConnectedInterpolatedPoints( + const std::vector & points, + const double delta_arc_length, const double begin_yaw, const double end_yaw); +} // namespace interpolation_utils + +namespace points_utils +{ +template +size_t findForwardIndex(const T & points, const size_t begin_idx, const double forward_length) +{ + double sum_length = 0.0; + for (size_t i = begin_idx; i < points.size() - 1; ++i) { + sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + if (sum_length > forward_length) { + return i; + } + } + return points.size() - 1; +} + +template +T clipForwardPoints(const T & points, const size_t begin_idx, const double forward_length) +{ + if (points.empty()) { + return T{}; + } + + const size_t end_idx = findForwardIndex(points, begin_idx, forward_length); + return T{points.begin() + begin_idx, points.begin() + end_idx}; +} + +template +T clipBackwardPoints( + const T & points, const size_t target_idx, const double backward_length, + const double delta_length) +{ + if (points.empty()) { + return T{}; + } + + const int begin_idx = + std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); + return T{points.begin() + begin_idx, points.end()}; +} + +template +T clipBackwardPoints( + const T & points, const geometry_msgs::msg::Point pos, const double backward_length, + const double delta_length) +{ + if (points.empty()) { + return T{}; + } + + const size_t target_idx = tier4_autoware_utils::findNearestIndex(points, pos); + + const int begin_idx = + std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); + return T{points.begin() + begin_idx, points.end()}; +} + +// NOTE: acceleration is not converted +template +std::vector convertToPoints(const std::vector & points) +{ + std::vector geom_points; + for (const auto & point : points) { + geom_points.push_back(tier4_autoware_utils::getPoint(point)); + } + return geom_points; +} + +template +autoware_auto_planning_msgs::msg::TrajectoryPoint convertToTrajectoryPoint(const T & point) +{ + autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + return traj_point; +} + +template <> +inline autoware_auto_planning_msgs::msg::TrajectoryPoint convertToTrajectoryPoint( + const ReferencePoint & point) +{ + autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(point); + return traj_point; +} + +// functions to convert to another type of points +template +std::vector convertToTrajectoryPoints( + const std::vector & points) +{ + std::vector traj_points; + for (const auto & point : points) { + const auto traj_point = convertToTrajectoryPoint(point); + traj_points.push_back(traj_point); + } + return traj_points; +} + +template +ReferencePoint convertToReferencePoint(const T & point); + +template +std::vector convertToReferencePoints(const std::vector & points) +{ + std::vector ref_points; + for (const auto & point : points) { + const auto ref_point = convertToReferencePoint(point); + ref_points.push_back(ref_point); + } + return ref_points; +} + +std::vector convertToPosesWithYawEstimation( + const std::vector points); + +std::vector concatTrajectory( + const std::vector & traj_points, + const std::vector & extended_traj_points); + +void compensateLastPose( + const autoware_auto_planning_msgs::msg::PathPoint & last_path_point, + std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold); + +template +std::vector calcCurvature(const T & points, const size_t num_sampling_points) +{ + std::vector res(points.size()); + const size_t num_points = static_cast(points.size()); + + /* calculate curvature by circle fitting from three points */ + geometry_msgs::msg::Point p1, p2, p3; + size_t max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); + size_t L = std::min(num_sampling_points, max_smoothing_num); + for (size_t i = L; i < num_points - L; ++i) { + p1 = tier4_autoware_utils::getPoint(points.at(i - L)); + p2 = tier4_autoware_utils::getPoint(points.at(i)); + p3 = tier4_autoware_utils::getPoint(points.at(i + L)); + double den = std::max( + tier4_autoware_utils::calcDistance2d(p1, p2) * tier4_autoware_utils::calcDistance2d(p2, p3) * + tier4_autoware_utils::calcDistance2d(p3, p1), + 0.0001); + const double curvature = + 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; + res.at(i) = curvature; + } + + /* first and last curvature is copied from next value */ + for (size_t i = 0; i < std::min(L, num_points); ++i) { + res.at(i) = res.at(std::min(L, num_points - 1)); + res.at(num_points - i - 1) = + res.at(std::max(static_cast(num_points) - static_cast(L) - 1, 0)); + } + return res; +} + +int getNearestIdx( + const std::vector & points, const double target_s, const int begin_idx); + +template +bool isNearLastPathPoint( + const T & ref_point, const std::vector & path_points, + const double delta_dist_threshold, const double delta_yaw_threshold) +{ + const geometry_msgs::msg::Pose last_ref_pose = tier4_autoware_utils::getPose(ref_point); + + if ( + tier4_autoware_utils::calcDistance2d(last_ref_pose, path_points.back().pose) > + delta_dist_threshold) { + return false; + } + if ( + std::fabs(tier4_autoware_utils::calcYawDeviation(last_ref_pose, path_points.back().pose)) > + delta_yaw_threshold) { + return false; + } + return true; +} +} // namespace points_utils + +namespace utils +{ +void logOSQPSolutionStatus(const int solution_status); +} // namespace utils + +#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp index a7e623bc099c3..6cce41c70f227 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -48,10 +48,11 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" -#include -#include +#include /** * @class vehicle model class of bicycle kinematics @@ -64,9 +65,8 @@ class KinematicsBicycleModel : public VehicleModelInterface * @brief constructor with parameter initialization * @param [in] wheelbase wheelbase length [m] * @param [in] steer_lim steering angle limit [rad] - * @param [in] steer_tau steering time constant for 1d-model */ - KinematicsBicycleModel(const double wheelbase, const double steer_lim, const double steer_tau); + KinematicsBicycleModel(const double wheel_base, const double steer_limit); /** * @brief destructor @@ -74,26 +74,31 @@ class KinematicsBicycleModel : public VehicleModelInterface virtual ~KinematicsBicycleModel() = default; /** - * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk + * @brief calculate discrete kinematics equation matrix of x_k+1 = Ad * x_k + Bd * uk + Wd * @param [out] Ad coefficient matrix * @param [out] Bd coefficient matrix - * @param [out] Cd coefficient matrix * @param [out] Wd coefficient matrix - * @param [in] dt Discretization arc length + * @param [in] ds discretization arc length + */ + void calculateStateEquationMatrix( + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) override; + + /** + * @brief calculate discrete observation matrix of y_k = Cd * x_k + * @param [out] Cd coefficient matrix */ - void calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) override; + void calculateObservationMatrix(Eigen::MatrixXd & Cd) override; + + /** + * @brief calculate discrete observation matrix of y_k = Cd * x_k + * @param [out] Cd_vec sparse matrix information of coefficient matrix + */ + void calculateObservationSparseMatrix(std::vector> & Cd_vec) override; /** * @brief calculate reference input * @param [out] reference input */ void calculateReferenceInput(Eigen::MatrixXd * Uref) override; - -private: - double wheelbase_; //!< @brief wheelbase length [m] - double steer_lim_; //!< @brief steering angle limit [rad] - double steer_tau_; //!< @brief steering time constant for 1d-model }; #endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp index 52f99e46aacc6..55de7dadc3d03 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp @@ -22,7 +22,10 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Sparse" + +#include /** * @class vehicle model class @@ -31,20 +34,23 @@ class VehicleModelInterface { protected: - const int dim_x_; //!< @brief dimension of state x - const int dim_u_; //!< @brief dimension of input u - const int dim_y_; //!< @brief dimension of output y - double velocity_; //!< @brief vehicle velocity - double curvature_; //!< @brief curvature on the linearized point on path + const int dim_x_; // !< @brief dimension of kinematics x + const int dim_u_; // !< @brief dimension of input u + const int dim_y_; // !< @brief dimension of output y + double velocity_; // !< @brief vehicle velocity + double curvature_; // !< @brief curvature on the linearized point on path + double wheel_base_; // !< @brief wheel base of vehicle + double steer_limit_; // !< @brief vehicle velocity + double center_offset_from_base_; // !< @brief length from base lin to optimization center [m] public: /** * @brief constructor - * @param [in] dim_x dimension of state x + * @param [in] dim_x dimension of kinematics x * @param [in] dim_u dimension of input u * @param [in] dim_y dimension of output y */ - VehicleModelInterface(int dim_x, int dim_u, int dim_y); + VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheel_base, double steer_limit); /** * @brief destructor @@ -52,8 +58,8 @@ class VehicleModelInterface virtual ~VehicleModelInterface() = default; /** - * @brief get state x dimension - * @return state dimension + * @brief get kinematics x dimension + * @return kinematics dimension */ int getDimX(); @@ -69,6 +75,8 @@ class VehicleModelInterface */ int getDimY(); + void updateCenterOffset(const double center_offset_from_base); + /** * @brief set curvature * @param [in] curvature curvature on the linearized point on path @@ -76,16 +84,26 @@ class VehicleModelInterface void setCurvature(const double curvature); /** - * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk + * @brief calculate discrete kinematics equation matrix of x_k+1 = Ad * x_k + Bd * uk + Wd * @param [out] Ad coefficient matrix * @param [out] Bd coefficient matrix - * @param [out] Cd coefficient matrix * @param [out] Wd coefficient matrix - * @param [in] ds Discretization arc length + * @param [in] ds discretization arc length + */ + virtual void calculateStateEquationMatrix( + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) = 0; + + /** + * @brief calculate discrete observation matrix of y_k = Cd * x_k + * @param [out] Cd coefficient matrix + */ + virtual void calculateObservationMatrix(Eigen::MatrixXd & Cd) = 0; + + /** + * @brief calculate discrete observation matrix of y_k = Cd * x_k + * @param [out] Cd_vec sparse matrix information of coefficient matrix */ - virtual void calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) = 0; + virtual void calculateObservationSparseMatrix(std::vector> & Cd_vec) = 0; /** * @brief calculate reference input diff --git a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml b/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml index e0ddc12072bd3..478c692989aea 100644 --- a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml +++ b/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml @@ -3,7 +3,6 @@ - @@ -13,6 +12,5 @@ - diff --git a/planning/obstacle_avoidance_planner/media/behavior_path.png b/planning/obstacle_avoidance_planner/media/behavior_path.png new file mode 100644 index 0000000000000..cc99ffe44e1ec Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/behavior_path.png differ diff --git a/planning/obstacle_avoidance_planner/media/bounds.png b/planning/obstacle_avoidance_planner/media/bounds.png new file mode 100644 index 0000000000000..54627b34f10ca Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/bounds.png differ diff --git a/planning/obstacle_avoidance_planner/media/drivable_area.png b/planning/obstacle_avoidance_planner/media/drivable_area.png index a1c855323e579..6606299705143 100644 Binary files a/planning/obstacle_avoidance_planner/media/drivable_area.png and b/planning/obstacle_avoidance_planner/media/drivable_area.png differ diff --git a/planning/obstacle_avoidance_planner/media/eb_traj.png b/planning/obstacle_avoidance_planner/media/eb_traj.png new file mode 100644 index 0000000000000..9910810920bff Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/eb_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png b/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png new file mode 100644 index 0000000000000..2344467811a83 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png b/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png new file mode 100644 index 0000000000000..6129ac23dc008 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_traj.png b/planning/obstacle_avoidance_planner/media/mpt_traj.png new file mode 100644 index 0000000000000..44a06bc1ef9f6 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/mpt_traj.png differ diff --git a/planning/obstacle_avoidance_planner/media/output_traj.png b/planning/obstacle_avoidance_planner/media/output_traj.png new file mode 100644 index 0000000000000..e4cfae92d0a8f Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/output_traj.png differ diff --git a/planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md b/planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md new file mode 100644 index 0000000000000..8610070c4871d --- /dev/null +++ b/planning/obstacle_avoidance_planner/obstacle_avoidance_planner-design.ja.md @@ -0,0 +1,330 @@ +# Obstacle Avoidance Planner + +## Purpose + +obstacle_avoidance_planner は入力された path と drivable area、および動物体情報をもとに、車両キネマティクスモデルを考慮して車が走行可能な軌道を生成する。trajectory 内の各経路点の位置姿勢のみ計画しており、速度や加速度の計算は後段の別モジュールで行われる。 + + + +## Inputs / Outputs + +### input + +- reference_path [`autoware_auto_planning_msgs/Path`] : Reference path and the corresponding drivable area. +- objects [`autoware_auto_perception_msgs/PredictedObjects`] : Recognized objects around the vehicle + +### output + +- optimized_trajectory [`autoware_auto_planning_msgs/Trajectory`] : Optimized trajectory that is feasible to drive and collision-free. + +## Flowchart + +フローチャートとともに、各機能の概要をおおまかに説明する。 + +![obstacle_avoidance_planner_flowchart](./media/obstacle_avoidance_planner_flowchart.drawio.svg) + +### Manage trajectory generation + +以下の条件のいずれかが満たされたときに、経路生成の関数を呼ぶ。それ以外の時は前回生成された経路を出力する。 + +- 前回経路生成時から一定距離走行 (default: 10.0 [m]) +- 一定時間経過 (default: 1.0 [s]) +- レーンチェンジなどで入力の path の形状が変わった時 +- 自車位置が前回経路から大きく外れた時 + +入力の path の形状が変わった時と自車が前回経路から大きく外れた時は、保持している前回経路を破棄するリセット機能もある。 + +### Select objects to avoid + +静的で路肩にある障害物のみ回避対象とする。 +具体的には、以下の 3 つの条件を満たすものであり、図で示すと赤い id: 3, 4, 5 の物体である。 + +- 指定された速度以下 (default: 0.1 [m/s]) +- 物体重心が center line 上に存在しない（前車追従の車を避けないようにするため） +- 少なくとも 1 つ以上の物体形状ポリゴン点が drivable area に存在する。 + +![obstacle_to_avoid](./media/obstacle_to_avoid.drawio.svg) + +### Generate object costmap + +回避対象である障害物からの距離に応じたコストマップを生成する。 + +### Generate road boundary costmap + +道路からの距離に応じたコストマップを生成する。 + +これらのコストマップは後段の Optimize trajectory の手法である Model predictive trajectory の障害物・道路境界に衝突しない制約条件を定式化する際に使用される。 + +### Smooth path + +後段の最適化処理で曲率のなめらかな参照経路が必要であるため、最適化前に path をなめらかにして trajectory の形式で出力する。 +この平滑化は障害物を考慮しない。 + +### Optimize trajectory + +参照経路に基づいたフレネ座標系で車両キネマティクス、及び追従誤差を定義し、二次計画法を用いて車両キネマティクスや障害物回避を考慮しながら追従誤差が小さくなるように経路生成する。 +自車近傍の経路の急な変化を防ぐため、直近の経路は前回の経路をそのまま使用する。 +計算コストを抑えるため、最終的に出力する経路長よりも短い距離に対してのみ計算を行う。 + +### Extend trajectory + +経路を規定の長さ（default: 200m）に拡張するため、最適化した経路の先を Reference 経路と接続する。 + +### Check drivable area + +生成された経路が走行可能領域を逸脱していた場合、前回生成された走行可能領域内にある経路を出力する。 + +_Rationale_ +現在の設計において、数値誤差による破綻を防ぐために障害物回避は全てソフト制約として考慮されており、生成された経路に置いて車両が走行可能領域内に入っているかの判定が必要。 + +### Apply path velocity + +入力の path に埋め込まれている速度を出力される trajectory に埋め込む。経路間隔が異なるため線形補間を用いる。 + +## Algorithms + +Smooth path で使われている Elastic band と、Optimized trajectory で使われている Model predictive trajectory の詳細な説明をする。 + +### Elastic band + +#### 概要 + +behavior_path_planner で計算された path は場合によってはなめらかではない可能性があるので、その path の平滑化をすることが目的である。 + +次の Model predictive trajectory でも平滑化項は入っているが、目標経路になるべく追従しようとする項も入っているため、目標経路がなめらかでなかった場合はこの 2 つの項が反発する。 +それを防ぐためにここで平滑化のみを行っている。 +また Model predictive trajectory では各点における曲率と法線を元に最適化しており、平滑化された目標経路を渡すことで最適化の結果を安定させる狙いもある。 + +平滑化の際、障害物や道路壁を考慮していないため障害物や道路壁に衝突した trajectory が計算されることもある。 + +この Elastic band と次の Model predictive trajectory は、計算負荷を抑えるためにある程度の長さでクリップした trajectory を出力するようになっている。 + +#### 数式 + +前後の点からの距離の差の二乗を目的関数とする二次計画。 + +各点は一定の範囲以内しか動かないという制約を設けることで、入力の軌道をよりなめらかにした軌道を得る。 + +$\boldsymbol{p}_k$を$k$番目の経路点の座標ととしたとき以下のコスト関数を最小化する二次計画を解く。ここでは始点と終点である$\boldsymbol{p}_0$と$\boldsymbol{p}_n$は固定である。 + +$$ +\min \sum_{k=1}^{n-1} |\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}| - |\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1}| +$$ + +### Model predictive trajectory + +#### 概要 + +Elastic band で平滑化された trajectory に対して、以下の条件を満たすように修正を行うことが目的である。 + +- 線形化された車両のキネマティクスモデルに基づき走行可能である +- 障害物や道路壁面に衝突しない + +障害物や道路壁面に衝突しない条件はハードではなくソフト制約として含まれている。車両の後輪位置、前輪位置、その中心位置において障害物・道路境界との距離から制約条件が計算されている。 +条件を満たさない解が出力された場合は、後段の後処理で弾かれ、前の周期で計画された trajectory を出力する。 + +自車付近の経路が振動しないように、自車近傍の経路点を前回の経路点と一致させる制約条件も含まれており、これが唯一の二次計画法のハード制約である。 + +#### 数式 + +以下のように、経路に対して車両が追従するときの bicycle kinematics model を考える。 +時刻$k$における、経路上の車両の最近傍点の座標($x$座標は経路の接線に平行)から見た追従誤差に関して、横偏差$y_k$、向きの偏差$\theta_k$、ステア角$\delta_k$と定める。 + +![vehicle_error_kinematics](./media/vehicle_error_kinematics.png) + +指令ステア角度を$\delta_{des, k}$とすると、ステア角の遅延を考慮した車両キネマティクスモデルは以下で表される。この時、ステア角$\delta_k$は一次遅れ系として指令ステア角に追従すると仮定する。 + +$$ +\begin{align} +y_{k+1} & = y_{k} + v \sin \theta_k dt \\ +\theta_{k+1} & = \theta_k + \frac{v \tan \delta_k}{L}dt - \kappa_k v \cos \theta_k dt \\ +\delta_{k+1} & = \delta_k - \frac{\delta_k - \delta_{des,k}}{\tau}dt +\end{align} +$$ + +次にこれらの式を線形化する。$y_k$, $\theta_k$は追従誤差であるため微小近似でき、$\sin \theta_k \approx \theta_k$となる。 + +$\delta_k$に関してはステア角であるため微小とみなせない。そこで、以下のように参照経路の曲率$\kappa_k$から計算されるステア角$\delta_{\mathrm{ref}, k}$を用いることにより、$\delta_k$を微小な値$\Delta \delta_k$で表す。 + +ここで注意すべきこととしては、$\delta_k$は最大ステア角度$\delta_{\max}$以内の値を取る。曲率$\kappa_k$から計算された$\delta_{\mathrm{ref}, k}$が最大ステア角度$\delta_{\max}$より大きいときに$\delta_{\mathrm{ref}, k}$をそのまま使用して線形化すると、$\Delta \delta_k = \delta - \delta_{\mathrm{ref}, k} = \delta_{\max} - \delta_{\mathrm{ref}, k}$となり、$\Delta \delta_k$の絶対値が大きくなる。すなわち、$\delta_{\mathrm{ref}, k}$にも最大ステア角度制限を適用する必要がある。 + +$$ +\begin{align} +\delta_{\mathrm{ref}, k} & = \mathrm{clamp}(\arctan(L \kappa_k), -\delta_{\max}, \delta_{\max}) \\ +\delta_k & = \delta_{\mathrm{ref}, k} + \Delta \delta_k, \ \Delta \delta_k \ll 1 \\ +\end{align} +$$ + +$\mathrm{clamp}(v, v_{\min}, v_{\max})$は$v$を$v_{\min}$から$v_{\max}$の範囲内に丸める関数である。 + +この$\delta_{\mathrm{ref}, k}$を介して$\tan \delta_k$を線形な式で近似する。 + +$$ +\begin{align} +\tan \delta_k & \approx \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} \Delta \delta_k \\ +& = \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} (\delta_{\mathrm{ref}, k} - \delta_k) \\ +& = \tan \delta_{\mathrm{ref}, k} - \frac{\delta_{\mathrm{ref}, k}}{\cos^2 \delta_{\mathrm{ref}, k}} + \frac{1}{\cos^2 \delta_{\mathrm{ref}, k}} \delta_k +\end{align} +$$ + +以上の線形化を踏まえ、誤差ダイナミクスは以下のように線形な行列演算で記述できる。 + +$$ +\begin{align} + \begin{pmatrix} + y_{k+1} \\ + \theta_{k+1} \\ + \delta_{k+1} + \end{pmatrix} + = + \begin{pmatrix} + 1 & v dt & 0 \\ + 0 & 1 & \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ + 0 & 0 & 1 - \frac{dt}{\tau} + \end{pmatrix} + \begin{pmatrix} + y_k \\ + \theta_k \\ + \delta_k + \end{pmatrix} + + + \begin{pmatrix} + 0 \\ + 0 \\ + \frac{dt}{\tau} + \end{pmatrix} + \delta_{des} + + + \begin{pmatrix} + 0 \\ + \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt\\ + 0 + \end{pmatrix} +\end{align} +$$ + +平滑化と経路追従のための目的関数は以下で表され、 + +$$ +\begin{align} +J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) \\ & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 +\end{align} +$$ + +前述の通り、車両が障害物・道路境界に侵入しない条件はスラック変数を用いてソフト制約として表されている。 +車両の後輪位置、前輪位置、およびその中心位置における障害物・道路境界までの距離をそれぞれ$y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$とする。 +ここでそれぞれに対するスラック変数 $\lambda_{\mathrm{base}}, \lambda_{\mathrm{top}}, \lambda_{\mathrm{mid}}$を定義し、 + +$$ +y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\ +y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\ +y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} +$$ + +$y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$は$y_k$の 1 次式として表現できるので、このソフト制約の目的関数は、以下のように記述できる。 + +$$ +\begin{align} +J_2 & (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) \\ & = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k}^2 + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k}^2 + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k}^2 +\end{align} +$$ + +スラック変数も二次計画法の設計変数となり、全ての設計変数をまとめたベクトル$\boldsymbol{x}$を定義する。 + +$$ +\begin{align} +\boldsymbol{x} = +\begin{pmatrix} +... & y_k & \lambda_{\mathrm{base}, k} & \lambda_{\mathrm{top}, k} & \lambda_{\mathrm{mid}, k} & ... +\end{pmatrix}^T +\end{align} +$$ + +これらの 2 つの目的関数の和を目的関数とする。 + +$$ +\begin{align} +\min_{\boldsymbol{x}} J (\boldsymbol{x}) = \min_{\boldsymbol{x}} J_1 & (y_{0...N-1}, \theta_{0...N-1}, \delta_{0...N-1}) + J_2 (\lambda_{\mathrm{base}, 0...N-1}, \lambda_{\mathrm{mid}, 0...N-1}, \lambda_{\mathrm{top}, 0...N-1}) +\end{align} +$$ + +前述の通り、真にハードな制約条件は車両前方ある程度の距離$N_{fix}$までの経路点の状態は前回値になるような条件であり、以下のように記述できる。 + +$$ +\begin{align} +\delta_k = \delta_{k}^{\mathrm{prev}} (0 \leq i \leq N_{\mathrm{fix}}) +\end{align} +$$ + +であり、これらを以下のような二次計画法の係数行列に変換して二次計画法を解く + +$$ +\begin{align} +\min_{\boldsymbol{x}} \ & \frac{1}{2} \boldsymbol{x}^T \boldsymbol{P} \boldsymbol{x} + \boldsymbol{q} \boldsymbol{x} \\ +\mathrm{s.t.} \ & \boldsymbol{b}_l \leq \boldsymbol{A} \boldsymbol{x} \leq \boldsymbol{b}_u +\end{align} +$$ + +## Limitation + +- カーブ時に外側に膨らんだ経路を返す +- behavior_path_planner と obstacle_avoidance_planner の経路計画の役割分担がはっきりと決まっていない + - behavior_path_planner 側で回避する場合と、obstacle_avoidance_planner で回避する場合がある +- behavior_path_planner から来る path が道路壁に衝突していると、大きく外側に膨れた trajectory を計画する (柏の葉のカーブで顕著) +- 計算負荷が高い + +## How to debug + +obstacle_avoidance_planner` から出力される debug 用の topic について説明する。 + +- **interpolated_points_marker** + - obstacle avoidance planner への入力経路をリサンプルしたもの。この経路が道路内に収まっているか（道路内にあることが必須ではない）、十分になめらかか（ある程度滑らかでないとロジックが破綻する）、などを確認する。 + +![interpolated_points_marker](./media/interpolated_points_marker.png) + +- **smoothed_points_text** + - Elastic Band の計算結果。点群ではなく小さい数字が描画される。平滑化されたこの経路が道路内からはみ出ていないか、歪んでいないかなどを確認。 + +![smoothed_points_text](./media/smoothed_points_text.png) + +- **(base/top/mid)\_bounds_line** + - 壁との衝突判定における横方向の道路境界までの距離（正確には - vehicle_width / 2.0）。 + - 車両の 3 箇所（base, top, mid）で衝突判定を行っており、3 つのマーカーが存在する。 + - 黄色い線の各端点から道路境界までの距離が車幅の半分くらいであれば異常なし（ここがおかしい場合はロジック異常）。 + +![bounds_line](./media/bounds_line.png) + +- **optimized_points_marker** + - MPT の計算結果。道路からはみ出ていないか、振動していないかなどを確認 + +![optimized_points_marker](./media/optimized_points_marker.png) + +- **Trajectory with footprint** + - TrajectoryFootprint の rviz_plugin を用いて経路上の footprint を描画することが可能。これを用いて obstacle_avoidance_planner への入出力の footprint、経路に収まっているかどうか等を確認する。 + +![trajectory_with_footprint](./media/trajectory_with_footprint.png) + +- **Drivable Area** + - obstacle avoidance への入力の走行可能領域を表示する。Drivable Area の生成に不具合があると生成経路が歪む可能性がある。 + - topic 名：`/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area` + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![drivable_area](./media/drivable_area.png) + +- **area_with_objects** + - 入力された走行可能領域から障害物を取り除いた後の、走行可能領域 + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![area_with_objects](./media/area_with_objects.png) + +- **object_clearance_map** + - 回避対象の障害物からの距離を可視化したもの。 + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![object_clearance_map](./media/object_clearance_map.png) + +- **clearance_map** + - 入力された走行可能領域からの距離を可視化したもの。 + - `nav_msgs/msg/OccupancyGrid` 型として出力される + +![clearance_map](./media/clearance_map.png) diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index b76c6db96f215..c58292c667327 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -21,6 +21,7 @@ std_msgs tf2_ros tier4_autoware_utils + tier4_debug_msgs tier4_planning_msgs vehicle_info_util visualization_msgs diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp new file mode 100644 index 0000000000000..dafc2d5881b3e --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp @@ -0,0 +1,342 @@ +// 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 "obstacle_avoidance_planner/costmap_generator.hpp" + +#include "obstacle_avoidance_planner/cv_utils.hpp" +#include "obstacle_avoidance_planner/utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace +{ +cv::Point toCVPoint(const geometry_msgs::msg::Point & p) +{ + cv::Point cv_point; + cv_point.x = p.x; + cv_point.y = p.y; + return cv_point; +} + +bool isAvoidingObjectType( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const TrajectoryParam & traj_param) +{ + if ( + (object.classification.at(0).label == object.classification.at(0).UNKNOWN && + traj_param.is_avoiding_unknown) || + (object.classification.at(0).label == object.classification.at(0).CAR && + traj_param.is_avoiding_car) || + (object.classification.at(0).label == object.classification.at(0).TRUCK && + traj_param.is_avoiding_truck) || + (object.classification.at(0).label == object.classification.at(0).BUS && + traj_param.is_avoiding_bus) || + (object.classification.at(0).label == object.classification.at(0).BICYCLE && + traj_param.is_avoiding_bicycle) || + (object.classification.at(0).label == object.classification.at(0).MOTORCYCLE && + traj_param.is_avoiding_motorbike) || + (object.classification.at(0).label == object.classification.at(0).PEDESTRIAN && + traj_param.is_avoiding_pedestrian)) { + return true; + } + return false; +} + +bool arePointsInsideDriveableArea( + const std::vector & image_points, const cv::Mat & clearance_map) +{ + bool points_inside_area = false; + for (const auto & image_point : image_points) { + const float clearance = + clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; + if (clearance > 0) { + points_inside_area = true; + } + } + return points_inside_area; +} + +bool isAvoidingObject( + const PolygonPoints & polygon_points, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info, + const std::vector & path_points, + const TrajectoryParam & traj_param) +{ + if (path_points.empty()) { + return false; + } + if (!isAvoidingObjectType(object, traj_param)) { + return false; + } + const auto image_point = geometry_utils::transformMapToOptionalImage( + object.kinematics.initial_pose_with_covariance.pose.position, map_info); + if (!image_point) { + return false; + } + + const int nearest_idx = tier4_autoware_utils::findNearestIndex( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + const auto nearest_path_point = path_points[nearest_idx]; + const auto rel_p = geometry_utils::transformToRelativeCoordinate2D( + object.kinematics.initial_pose_with_covariance.pose.position, nearest_path_point.pose); + // skip object located back the beginning of path points + if (nearest_idx == 0 && rel_p.x < 0) { + return false; + } + + /* + const float object_clearance_from_road = + clearance_map.ptr( + static_cast(image_point.get().y))[static_cast(image_point.get().x)] * + map_info.resolution; + */ + const geometry_msgs::msg::Vector3 twist = + object.kinematics.initial_twist_with_covariance.twist.linear; + const double vel = std::sqrt(twist.x * twist.x + twist.y * twist.y + twist.z * twist.z); + /* + const auto nearest_path_point_image = + geometry_utils::transformMapToOptionalImage(nearest_path_point.pose.position, map_info); + if (!nearest_path_point_image) { + return false; + } + const float nearest_path_point_clearance = + clearance_map.ptr(static_cast( + nearest_path_point_image.get().y))[static_cast(nearest_path_point_image.get().x)] * + map_info.resolution; + */ + const double lateral_offset_to_path = tier4_autoware_utils::calcLateralOffset( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + if ( + // nearest_path_point_clearance - traj_param.center_line_width * 0.5 < + // object_clearance_from_road || + std::abs(lateral_offset_to_path) < traj_param.center_line_width * 0.5 || + vel > traj_param.max_avoiding_objects_velocity_ms || + !arePointsInsideDriveableArea(polygon_points.points_in_image, clearance_map)) { + return false; + } + return true; +} +} // namespace + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const cv::Point & p) +{ + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x; + geom_p.y = p.y; + + return geom_p; +} +} // namespace tier4_autoware_utils + +CVMaps CostmapGenerator::getMaps( + const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & objects, + const TrajectoryParam & traj_param, std::shared_ptr debug_data_ptr) +{ + stop_watch_.tic(__func__); + + // make cv_maps + CVMaps cv_maps; + + cv_maps.drivable_area = getDrivableAreaInCV(path.drivable_area, debug_data_ptr); + cv_maps.clearance_map = getClearanceMap(cv_maps.drivable_area, debug_data_ptr); + + std::vector debug_avoiding_objects; + cv::Mat objects_image = drawObstaclesOnImage( + enable_avoidance, objects, path.points, path.drivable_area.info, cv_maps.drivable_area, + cv_maps.clearance_map, traj_param, &debug_avoiding_objects, debug_data_ptr); + + cv_maps.area_with_objects_map = + getAreaWithObjects(cv_maps.drivable_area, objects_image, debug_data_ptr); + cv_maps.only_objects_clearance_map = getClearanceMap(objects_image, debug_data_ptr); + cv_maps.map_info = path.drivable_area.info; + + // debug data + debug_data_ptr->clearance_map = cv_maps.clearance_map; + debug_data_ptr->only_object_clearance_map = cv_maps.only_objects_clearance_map; + debug_data_ptr->area_with_objects_map = cv_maps.area_with_objects_map; + debug_data_ptr->avoiding_objects = debug_avoiding_objects; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return cv_maps; +} + +cv::Mat CostmapGenerator::getDrivableAreaInCV( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + cv::Mat drivable_area = cv::Mat(occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1); + + drivable_area.forEach([&](unsigned char & value, const int * position) -> void { + cv_utils::getOccupancyGridValue(occupancy_grid, position[0], position[1], value); + }); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return drivable_area; +} + +cv::Mat CostmapGenerator::getClearanceMap( + const cv::Mat & drivable_area, std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + cv::Mat clearance_map; + cv::distanceTransform(drivable_area, clearance_map, cv::DIST_L2, 5); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return clearance_map; +} + +cv::Mat CostmapGenerator::drawObstaclesOnImage( + const bool enable_avoidance, + const std::vector & objects, + const std::vector & path_points, + const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area, + const cv::Mat & clearance_map, const TrajectoryParam & traj_param, + std::vector * debug_avoiding_objects, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + std::vector path_points_inside_area; + for (const auto & point : path_points) { + std::vector points; + geometry_msgs::msg::Point image_point; + if (!geometry_utils::transformMapToImage(point.pose.position, map_info, image_point)) { + continue; + } + const float clearance = + clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)]; + if (clearance < 1e-5) { + continue; + } + path_points_inside_area.push_back(point); + } + + // NOTE: objects image is too sparse so that creating cost map is heavy. + // Then, objects image is created by filling dilated drivable area, + // instead of "cv::Mat objects_image = cv::Mat::ones(clearance_map.rows, clearance_map.cols, + // CV_8UC1) * 255;". + constexpr double dilate_margin = 1.0; + cv::Mat objects_image; + const int dilate_size = static_cast( + (1.8 + dilate_margin) / + (std::sqrt(2) * map_info.resolution)); // TODO(murooka) use clearance_from_object + cv::dilate(drivable_area, objects_image, cv::Mat(), cv::Point(-1, -1), dilate_size); + + if (!enable_avoidance) { + return objects_image; + } + + // fill object + std::vector> cv_polygons; + std::vector> obj_cog_info; + std::vector obj_positions; + for (const auto & object : objects) { + const PolygonPoints polygon_points = cv_polygon_utils::getPolygonPoints(object, map_info); + if (isAvoidingObject( + polygon_points, object, clearance_map, map_info, path_points_inside_area, traj_param)) { + const double lon_dist_to_path = tier4_autoware_utils::calcSignedArcLength( + path_points, 0, object.kinematics.initial_pose_with_covariance.pose.position); + const double lat_dist_to_path = tier4_autoware_utils::calcLateralOffset( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + obj_cog_info.push_back({lon_dist_to_path, lat_dist_to_path}); + obj_positions.push_back(object.kinematics.initial_pose_with_covariance.pose.position); + + cv_polygons.push_back(cv_polygon_utils::getCVPolygon( + object, polygon_points, path_points_inside_area, clearance_map, map_info)); + debug_avoiding_objects->push_back(object); + } + } + for (const auto & polygon : cv_polygons) { + cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); + } + + // fill between objects in the same side + const auto get_closest_obj_point = [&](size_t idx) { + const auto & path_point = + path_points.at(tier4_autoware_utils::findNearestIndex(path_points, obj_positions.at(idx))); + double min_dist = std::numeric_limits::min(); + size_t min_idx = 0; + for (size_t p_idx = 0; p_idx < cv_polygons.at(idx).size(); ++p_idx) { + const double dist = + tier4_autoware_utils::calcDistance2d(cv_polygons.at(idx).at(p_idx), path_point); + if (dist < min_dist) { + min_dist = dist; + min_idx = p_idx; + } + } + + geometry_msgs::msg::Point geom_point; + geom_point.x = cv_polygons.at(idx).at(min_idx).x; + geom_point.y = cv_polygons.at(idx).at(min_idx).y; + return geom_point; + }; + + std::vector> cv_between_polygons; + for (size_t i = 0; i < obj_positions.size(); ++i) { + for (size_t j = i + 1; j < obj_positions.size(); ++j) { + const auto & obj_info1 = obj_cog_info.at(i); + const auto & obj_info2 = obj_cog_info.at(j); + + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lat"), obj_info1.at(1) << " " << obj_info2.at(1)); + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("lon"), obj_info1.at(0) << " " << obj_info2.at(0)); + + constexpr double max_lon_dist_to_convex_obstacles = 30.0; + if ( + obj_info1.at(1) * obj_info2.at(1) < 0 || + std::abs(obj_info1.at(0) - obj_info2.at(0)) > max_lon_dist_to_convex_obstacles) { + continue; + } + + std::vector cv_points; + cv_points.push_back(toCVPoint(obj_positions.at(i))); + cv_points.push_back(toCVPoint(get_closest_obj_point(i))); + cv_points.push_back(toCVPoint(get_closest_obj_point(j))); + cv_points.push_back(toCVPoint(obj_positions.at(j))); + + cv_between_polygons.push_back(cv_points); + } + } + /* + for (const auto & polygon : cv_between_polygons) { + cv::fillConvexPoly(objects_image, polygon, cv::Scalar(0)); + } + */ + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + + return objects_image; +} + +cv::Mat CostmapGenerator::getAreaWithObjects( + const cv::Mat & drivable_area, const cv::Mat & objects_image, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + cv::Mat area_with_objects = cv::min(objects_image, drivable_area); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return area_with_objects; +} diff --git a/planning/obstacle_avoidance_planner/src/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/cv_utils.cpp new file mode 100644 index 0000000000000..a73a5c70fdeb3 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/cv_utils.cpp @@ -0,0 +1,463 @@ +// 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 "obstacle_avoidance_planner/cv_utils.hpp" + +#include "obstacle_avoidance_planner/utils.hpp" +#include "tf2/utils.h" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include + +namespace +{ +boost::optional getDistance( + const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & map_info) +{ + const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); + if (!image_point) { + return boost::none; + } + const float clearance = clearance_map.ptr(static_cast( + image_point.get().y))[static_cast(image_point.get().x)] * + map_info.resolution; + return clearance; +} + +bool isOutsideDrivableArea( + const geometry_msgs::msg::Point & pos, const cv::Mat & road_clearance_map, + const nav_msgs::msg::MapMetaData & map_info, const double max_dist) +{ + const auto dist = getDistance(road_clearance_map, pos, map_info); + if (dist) { + return dist.get() < max_dist; + } + + return false; +} +} // namespace + +namespace cv_utils +{ +void getOccupancyGridValue( + const nav_msgs::msg::OccupancyGrid & og, const int i, const int j, unsigned char & value) +{ + int i_flip = og.info.width - i - 1; + int j_flip = og.info.height - j - 1; + if (og.data[i_flip + j_flip * og.info.width] > 0) { + value = 0; + } else { + value = 255; + } +} + +void putOccupancyGridValue( + nav_msgs::msg::OccupancyGrid & og, const int i, const int j, const unsigned char value) +{ + int i_flip = og.info.width - i - 1; + int j_flip = og.info.height - j - 1; + og.data[i_flip + j_flip * og.info.width] = value; +} +} // namespace cv_utils + +namespace cv_polygon_utils +{ +PolygonPoints getPolygonPoints( + const std::vector & points, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + for (const auto & point : points) { + const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); + if (image_point) { + points_in_image.push_back(image_point.get()); + points_in_map.push_back(point); + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +PolygonPoints getPolygonPoints( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + PolygonPoints polygon_points; + if (object.shape.type == object.shape.BOUNDING_BOX) { + polygon_points = getPolygonPointsFromBB(object, map_info); + } else if (object.shape.type == object.shape.CYLINDER) { + polygon_points = getPolygonPointsFromCircle(object, map_info); + } else if (object.shape.type == object.shape.POLYGON) { + polygon_points = getPolygonPointsFromPolygon(object, map_info); + } + return polygon_points; +} + +PolygonPoints getPolygonPointsFromBB( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + const double dim_x = object.shape.dimensions.x; + const double dim_y = object.shape.dimensions.y; + const std::vector rel_x = {0.5 * dim_x, 0.5 * dim_x, -0.5 * dim_x, -0.5 * dim_x}; + const std::vector rel_y = {0.5 * dim_y, -0.5 * dim_y, -0.5 * dim_y, 0.5 * dim_y}; + const geometry_msgs::msg::Pose object_pose = object.kinematics.initial_pose_with_covariance.pose; + for (size_t i = 0; i < rel_x.size(); i++) { + geometry_msgs::msg::Point rel_point; + rel_point.x = rel_x[i]; + rel_point.y = rel_y[i]; + auto abs_point = geometry_utils::transformToAbsoluteCoordinate2D(rel_point, object_pose); + geometry_msgs::msg::Point image_point; + if (geometry_utils::transformMapToImage(abs_point, map_info, image_point)) { + points_in_image.push_back(image_point); + points_in_map.push_back(abs_point); + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +PolygonPoints getPolygonPointsFromCircle( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + const double radius = object.shape.dimensions.x; + const geometry_msgs::msg::Point center = + object.kinematics.initial_pose_with_covariance.pose.position; + constexpr int num_sampling_points = 5; + for (int i = 0; i < num_sampling_points; ++i) { + std::vector deltas = {0, 1.0}; + for (const auto & delta : deltas) { + geometry_msgs::msg::Point point; + point.x = std::cos( + ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + + M_PI / static_cast(num_sampling_points)) * + (radius / 2.0) + + center.x; + point.y = std::sin( + ((i + delta) / static_cast(num_sampling_points)) * 2.0 * M_PI + + M_PI / static_cast(num_sampling_points)) * + (radius / 2.0) + + center.y; + point.z = center.z; + geometry_msgs::msg::Point image_point; + if (geometry_utils::transformMapToImage(point, map_info, image_point)) { + points_in_image.push_back(image_point); + points_in_map.push_back(point); + } + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +PolygonPoints getPolygonPointsFromPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const nav_msgs::msg::MapMetaData & map_info) +{ + std::vector points_in_image; + std::vector points_in_map; + for (const auto & polygon_p : object.shape.footprint.points) { + geometry_msgs::msg::Point rel_point; + rel_point.x = polygon_p.x; + rel_point.y = polygon_p.y; + geometry_msgs::msg::Point point = geometry_utils::transformToAbsoluteCoordinate2D( + rel_point, object.kinematics.initial_pose_with_covariance.pose); + const auto image_point = geometry_utils::transformMapToOptionalImage(point, map_info); + if (image_point) { + points_in_image.push_back(image_point.get()); + points_in_map.push_back(point); + } + } + PolygonPoints polygon_points; + polygon_points.points_in_image = points_in_image; + polygon_points.points_in_map = points_in_map; + return polygon_points; +} + +std::vector getCVPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const PolygonPoints & polygon_points, + const std::vector & path_points, + const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) +{ + const int nearest_idx = tier4_autoware_utils::findNearestIndex( + path_points, object.kinematics.initial_pose_with_covariance.pose.position); + const auto nearest_path_point = path_points[nearest_idx]; + if (path_points.empty()) { + return getDefaultCVPolygon(polygon_points.points_in_image); + } + return getExtendedCVPolygon( + polygon_points.points_in_image, polygon_points.points_in_map, nearest_path_point.pose, object, + clearance_map, map_info); +} + +std::vector getDefaultCVPolygon( + const std::vector & points_in_image) +{ + std::vector cv_polygon; + for (const auto & point : points_in_image) { + cv::Point image_point = cv::Point(point.x, point.y); + cv_polygon.push_back(image_point); + } + return cv_polygon; +} + +std::vector getExtendedCVPolygon( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info) +{ + const boost::optional optional_edges = getEdges( + points_in_image, points_in_map, nearest_path_point_pose, object, clearance_map, map_info); + if (!optional_edges) { + return getDefaultCVPolygon(points_in_image); + } + const Edges edges = optional_edges.get(); + + const int nearest_polygon_idx = + tier4_autoware_utils::findNearestIndex(points_in_image, edges.origin); + std::vector cv_polygon; + if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { + // make polygon only with edges and extended edges + } else if (edges.back_idx < nearest_polygon_idx) { + // back_idx -> nearest_idx -> frond_idx + if (edges.back_idx < edges.front_idx && nearest_polygon_idx < edges.front_idx) { + for (int i = edges.back_idx + 1; i < edges.front_idx; i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + // back_idx -> vector_front -> vector_back -> nearest_idx -> frond_idx + } else if (edges.back_idx < edges.front_idx && nearest_polygon_idx > edges.front_idx) { + for (int i = edges.back_idx - 1; i >= 0; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx + } else { + for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = 0; i < edges.front_idx; i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + } + } else { + // back_idx -> nearest_idx -> front_idx + if (edges.back_idx >= edges.front_idx && nearest_polygon_idx > edges.front_idx) { + for (int i = edges.back_idx - 1; i > edges.front_idx; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + // back_idx -> vector_back -> vector_front -> nearest_idx -> front_idx + } else { + if (edges.back_idx >= edges.front_idx && nearest_polygon_idx < edges.front_idx) { + for (size_t i = edges.back_idx + 1; i < points_in_image.size(); i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = 0; i < edges.front_idx; i++) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + } else { // back_idx -> vector_front -> vector_back -> nearest_idx -> front_idx + for (int i = edges.back_idx - 1; i >= 0; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + for (int i = points_in_image.size() - 1; i > edges.front_idx; i--) { + cv_polygon.push_back(cv::Point(points_in_image[i].x, points_in_image[i].y)); + } + } + } + } + cv_polygon.push_back( + cv::Point(points_in_image[edges.front_idx].x, points_in_image[edges.front_idx].y)); + cv_polygon.push_back(cv::Point(edges.extended_front.x, edges.extended_front.y)); + cv_polygon.push_back(cv::Point(edges.extended_back.x, edges.extended_back.y)); + cv_polygon.push_back( + cv::Point(points_in_image[edges.back_idx].x, points_in_image[edges.back_idx].y)); + return cv_polygon; +} + +boost::optional getEdges( + const std::vector & points_in_image, + const std::vector & points_in_map, + const geometry_msgs::msg::Pose & nearest_path_point_pose, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const cv::Mat & clearance_map, + const nav_msgs::msg::MapMetaData & map_info) +{ + // calculate perpendicular point to object along with path point orientation + const double yaw = tf2::getYaw(nearest_path_point_pose.orientation); + const Eigen::Vector2d rel_path_vec(std::cos(yaw), std::sin(yaw)); + const Eigen::Vector2d obj_vec( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + const double inner_product = rel_path_vec[0] * (obj_vec[0] - nearest_path_point_pose.position.x) + + rel_path_vec[1] * (obj_vec[1] - nearest_path_point_pose.position.y); + geometry_msgs::msg::Point origin; + origin.x = nearest_path_point_pose.position.x + rel_path_vec[0] * inner_product; + origin.y = nearest_path_point_pose.position.y + rel_path_vec[1] * inner_product; + const Eigen::Vector2d obj2origin(origin.x - obj_vec[0], origin.y - obj_vec[1]); + + // calculate origin for casting ray to edges + const auto path_point_image = + geometry_utils::transformMapToImage(nearest_path_point_pose.position, map_info); + constexpr double ray_origin_dist_scale = 1.0; + const float clearance = clearance_map.ptr(static_cast( + path_point_image.y))[static_cast(path_point_image.x)] * + map_info.resolution * ray_origin_dist_scale; + const Eigen::Vector2d obj2ray_origin = obj2origin.normalized() * (obj2origin.norm() + clearance); + geometry_msgs::msg::Point ray_origin; + ray_origin.x = obj_vec[0] + obj2ray_origin[0]; + ray_origin.y = obj_vec[1] + obj2ray_origin[1]; + geometry_msgs::msg::Point ray_origin_image; + ray_origin_image = geometry_utils::transformMapToImage(ray_origin, map_info); + + double min_cos = std::numeric_limits::max(); + double max_cos = std::numeric_limits::lowest(); + const double path_yaw = tf2::getYaw(nearest_path_point_pose.orientation); + const double dx1 = std::cos(path_yaw); + const double dy1 = std::sin(path_yaw); + const Eigen::Vector2d path_point_vec(dx1, dy1); + const double path_point_vec_norm = path_point_vec.norm(); + Edges edges; + for (size_t i = 0; i < points_in_image.size(); i++) { + const double dx2 = points_in_map[i].x - ray_origin.x; + const double dy2 = points_in_map[i].y - ray_origin.y; + const Eigen::Vector2d path_point2point(dx2, dy2); + const double inner_product = path_point_vec.dot(path_point2point); + const double cos = inner_product / (path_point_vec_norm * path_point2point.norm()); + if (cos > max_cos) { + max_cos = cos; + edges.front_idx = i; + } + if (cos < min_cos) { + min_cos = cos; + edges.back_idx = i; + } + } + + const double max_sin = std::sqrt(1 - max_cos * max_cos); + const double min_sin = std::sqrt(1 - min_cos * min_cos); + const Eigen::Vector2d point2front_edge( + points_in_image[edges.front_idx].x - ray_origin_image.x, + points_in_image[edges.front_idx].y - ray_origin_image.y); + const Eigen::Vector2d point2back_edge( + points_in_image[edges.back_idx].x - ray_origin_image.x, + points_in_image[edges.back_idx].y - ray_origin_image.y); + const Eigen::Vector2d point2extended_front_edge = + point2front_edge.normalized() * (clearance * 2 / max_sin) * (1 / map_info.resolution); + const Eigen::Vector2d point2extended_back_edge = + point2back_edge.normalized() * (clearance * 2 / min_sin) * (1 / map_info.resolution); + + const double dist2extended_front_edge = point2extended_front_edge.norm() * map_info.resolution; + const double dist2front_edge = point2front_edge.norm() * map_info.resolution; + const double dist2extended_back_edge = point2extended_back_edge.norm() * map_info.resolution; + const double dist2back_edge = point2back_edge.norm() * map_info.resolution; + if ( + dist2extended_front_edge < clearance * 2 || dist2extended_back_edge < clearance * 2 || + dist2front_edge > dist2extended_front_edge || dist2back_edge > dist2extended_back_edge) { + return boost::none; + } + geometry_msgs::msg::Point extended_front; + geometry_msgs::msg::Point extended_back; + extended_front.x = point2extended_front_edge(0) + ray_origin_image.x; + extended_front.y = point2extended_front_edge(1) + ray_origin_image.y; + extended_back.x = point2extended_back_edge(0) + ray_origin_image.x; + extended_back.y = point2extended_back_edge(1) + ray_origin_image.y; + edges.extended_front = extended_front; + edges.extended_back = extended_back; + edges.origin = ray_origin_image; + return edges; +} +} // namespace cv_polygon_utils + +namespace cv_drivable_area_utils +{ +bool isOutsideDrivableAreaFromRectangleFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const VehicleParam & vehicle_param) +{ + const double half_width = vehicle_param.width / 2.0; + const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; + const double base_to_rear = vehicle_param.rear_overhang; + + const auto top_left_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0).position; + const auto top_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0).position; + const auto bottom_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0).position; + const auto bottom_left_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0).position; + + constexpr double epsilon = 1e-8; + const bool out_top_left = + isOutsideDrivableArea(top_left_pos, road_clearance_map, map_info, epsilon); + const bool out_top_right = + isOutsideDrivableArea(top_right_pos, road_clearance_map, map_info, epsilon); + const bool out_bottom_left = + isOutsideDrivableArea(bottom_left_pos, road_clearance_map, map_info, epsilon); + const bool out_bottom_right = + isOutsideDrivableArea(bottom_right_pos, road_clearance_map, map_info, epsilon); + + if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { + return true; + } + + return false; +} + +bool isOutsideDrivableAreaFromCirclesFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, + const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, + const std::vector vehicle_circle_longitudinal_offsets, const double vehicle_circle_radius) +{ + for (const double offset : vehicle_circle_longitudinal_offsets) { + const auto avoiding_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, offset, 0.0, 0.0).position; + + const bool outside_drivable_area = + isOutsideDrivableArea(avoiding_pos, road_clearance_map, map_info, vehicle_circle_radius); + if (outside_drivable_area) { + return true; + } + } + + return false; +} + +} // namespace cv_drivable_area_utils diff --git a/planning/obstacle_avoidance_planner/src/debug.cpp b/planning/obstacle_avoidance_planner/src/debug_visualization.cpp similarity index 51% rename from planning/obstacle_avoidance_planner/src/debug.cpp rename to planning/obstacle_avoidance_planner/src/debug_visualization.cpp index 7c0036f0b03ea..2992ab4ccdffc 100644 --- a/planning/obstacle_avoidance_planner/src/debug.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_visualization.cpp @@ -12,122 +12,73 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/debug.hpp" +#include "obstacle_avoidance_planner/debug_visualization.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/marker_helper.hpp" +#include "obstacle_avoidance_planner/cv_utils.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" +#include "obstacle_avoidance_planner/utils.hpp" +#include "tf2/utils.h" -#include -#include - -#include -#include -#include -#include -#include -#include - -#include +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include #include -visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( - const DebugData & debug_data, - // const std::vector & interpolated_points, - // const std::vector & smoothed_points, - const std::vector & optimized_points, - const VehicleParam & vehicle_param) +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +namespace +{ +template +visualization_msgs::msg::MarkerArray getPointsMarkerArray( + const std::vector & points, const std::string & ns, const double r, const double g, + const double b) { - const auto points_marker_array = getDebugPointsMarkers( - debug_data.interpolated_points, optimized_points, debug_data.straight_points, - debug_data.fixed_points, debug_data.non_fixed_points); - const auto constrain_marker_array = - getDebugConstrainMarkers(debug_data.constrain_rectangles, "constrain_rect"); - const auto extending_constrain_marker_array = getDebugConstrainMarkers( - debug_data.constrain_rectangles_for_extending, "extending_constrain_rect"); + if (points.empty()) { + return visualization_msgs::msg::MarkerArray{}; + } - visualization_msgs::msg::MarkerArray vis_marker_array; - if (debug_data.is_expected_to_over_drivable_area && !optimized_points.empty()) { - const auto virtual_wall_pose = getVirtualWallPose(optimized_points.back().pose, vehicle_param); - appendMarkerArray( - getVirtualWallMarkerArray(virtual_wall_pose, "virtual_wall", 1.0, 0, 0), &vis_marker_array); - appendMarkerArray( - getVirtualWallTextMarkerArray(virtual_wall_pose, "virtual_wall_text", 1.0, 1.0, 1.0), - &vis_marker_array); + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(r, g, b, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + + for (const auto & point : points) { + marker.points.push_back(tier4_autoware_utils::getPoint(point)); } - appendMarkerArray(points_marker_array, &vis_marker_array); - appendMarkerArray(constrain_marker_array, &vis_marker_array); - appendMarkerArray(extending_constrain_marker_array, &vis_marker_array); - appendMarkerArray( - getPointsMarkerArray( - debug_data.fixed_points_for_extending, "fixed_points_for_extending", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsMarkerArray( - debug_data.non_fixed_points_for_extending, "non_fixed_points_for_extending", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsMarkerArray( - debug_data.interpolated_points_for_extending, "interpolated_points_for_extending", 0.99, 0.99, - 0.2), - &vis_marker_array); - appendMarkerArray( - getObjectsMarkerArray(debug_data.avoiding_objects, "avoiding_objects", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getRectanglesMarkerArray(debug_data.vehicle_footprints, "vehicle_footprint", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getRectanglesMarkerArray( - debug_data.current_vehicle_footprints, "current_vehicle_footprint", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getRectanglesNumMarkerArray( - debug_data.vehicle_footprints, "num_vehicle_footprint", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsMarkerArray( - debug_data.bounds_candidate_for_base_points, "bounds_candidate_for_base", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsMarkerArray( - debug_data.bounds_candidate_for_top_points, "bounds_candidate_for_top", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsMarkerArray(debug_data.fixed_mpt_points, "fixed_mpt_points", 0.99, 0.0, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsTextMarkerArray( - debug_data.bounds_candidate_for_base_points, "bounds_candidate_base_text", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsTextMarkerArray( - debug_data.bounds_candidate_for_top_points, "bounds_candidate_top_text", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getPointsTextMarkerArray(debug_data.smoothed_points, "smoothed_points_text", 0.99, 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getBaseBoundsLineMarkerArray( - debug_data.bounds, debug_data.bounds_candidate_for_base_points, "base_bounds_line", 0.99, - 0.99, 0.2), - &vis_marker_array); - appendMarkerArray( - getTopBoundsLineMarkerArray( - debug_data.bounds, debug_data.bounds_candidate_for_top_points, "top_bounds_line", 0.99, 0.99, - 0.2), - &vis_marker_array); - appendMarkerArray( - getMidBoundsLineMarkerArray( - debug_data.bounds, debug_data.bounds_candidate_for_mid_points, "mid_bounds_line", 0.99, 0.99, - 0.2), - &vis_marker_array); - return vis_marker_array; + visualization_msgs::msg::MarkerArray msg; + msg.markers.push_back(marker); + + return msg; +} + +template +visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( + const std::vector & points, const std::string & ns, const double r, const double g, + const double b) +{ + if (points.empty()) { + return visualization_msgs::msg::MarkerArray{}; + } + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.15), createMarkerColor(r, g, b, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + visualization_msgs::msg::MarkerArray msg; + for (size_t i = 0; i < points.size(); i++) { + marker.id = i; + marker.text = std::to_string(i); + marker.pose.position = tier4_autoware_utils::getPoint(points[i]); + msg.markers.push_back(marker); + } + + return msg; } geometry_msgs::msg::Pose getVirtualWallPose( @@ -195,7 +146,7 @@ visualization_msgs::msg::MarkerArray getDebugPointsMarkers( } unique_id = 0; - for (std::size_t i = 0; i < optimized_points.size(); i++) { + for (size_t i = 0; i < optimized_points.size(); i++) { visualization_msgs::msg::Marker optimized_points_text_marker; optimized_points_text_marker.lifetime = rclcpp::Duration::from_seconds(0); optimized_points_text_marker.header.frame_id = "map"; @@ -214,7 +165,7 @@ visualization_msgs::msg::MarkerArray getDebugPointsMarkers( } unique_id = 0; - for (std::size_t i = 0; i < interpolated_points.size(); i++) { + for (size_t i = 0; i < interpolated_points.size(); i++) { visualization_msgs::msg::Marker interpolated_points_text_marker; interpolated_points_text_marker.lifetime = rclcpp::Duration::from_seconds(0); interpolated_points_text_marker.header.frame_id = "map"; @@ -298,7 +249,7 @@ visualization_msgs::msg::MarkerArray getDebugConstrainMarkers( { visualization_msgs::msg::MarkerArray marker_array; int unique_id = 0; - for (std::size_t i = 0; i < constrain_ranges.size(); i++) { + for (size_t i = 0; i < constrain_ranges.size(); i++) { visualization_msgs::msg::Marker constrain_rect_marker; constrain_rect_marker.lifetime = rclcpp::Duration::from_seconds(0); constrain_rect_marker.header.frame_id = "map"; @@ -323,7 +274,7 @@ visualization_msgs::msg::MarkerArray getDebugConstrainMarkers( marker_array.markers.push_back(constrain_rect_marker); } - for (std::size_t i = 0; i < constrain_ranges.size(); i++) { + for (size_t i = 0; i < constrain_ranges.size(); i++) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Time(0); @@ -341,7 +292,7 @@ visualization_msgs::msg::MarkerArray getDebugConstrainMarkers( } unique_id = 0; - for (std::size_t i = 0; i < constrain_ranges.size(); i++) { + for (size_t i = 0; i < constrain_ranges.size(); i++) { visualization_msgs::msg::Marker constrain_range_text_marker; constrain_range_text_marker.lifetime = rclcpp::Duration::from_seconds(0); constrain_range_text_marker.header.frame_id = "map"; @@ -395,23 +346,17 @@ visualization_msgs::msg::MarkerArray getObjectsMarkerArray( const std::vector & objects, const std::string & ns, const double r, const double g, const double b) { - const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(3.0, 1.0, 1.0), createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); - int32_t i = 0; - for (const auto & object : objects) { - marker.id = i++; - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; + for (size_t i = 0; i < objects.size(); ++i) { + const auto & object = objects.at(i); + marker.id = i; marker.pose = object.kinematics.initial_pose_with_covariance.pose; - marker.scale = createMarkerScale(3.0, 1.0, 1.0); - marker.color = createMarkerColor(r, g, b, 0.8); msg.markers.push_back(marker); } @@ -419,305 +364,315 @@ visualization_msgs::msg::MarkerArray getObjectsMarkerArray( } visualization_msgs::msg::MarkerArray getRectanglesMarkerArray( - const std::vector & rects, const std::string & ns, const double r, - const double g, const double b) + const std::vector mpt_traj, + const VehicleParam & vehicle_param, const std::string & ns, const double r, const double g, + const double b, const size_t sampling_num) { - const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; + for (size_t i = 0; i < mpt_traj.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + const auto & traj_point = mpt_traj.at(i); + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + const double half_width = vehicle_param.width / 2.0; + const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; + const double base_to_rear = vehicle_param.rear_overhang; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0) + .position); + marker.points.push_back(marker.points.front()); - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; - - int unique_id = 0; - for (const auto & rect : rects) { - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.scale = createMarkerScale(0.05, 0, 0); - marker.color = createMarkerColor(r, g, b, 0.025); - marker.points.push_back(rect.top_left); - marker.points.push_back(rect.top_right); - marker.points.push_back(rect.bottom_right); - marker.points.push_back(rect.bottom_left); msg.markers.push_back(marker); } return msg; } visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray( - const std::vector & rects, const std::string & ns, const double r, - const double g, const double b) + const std::vector mpt_traj, + const VehicleParam & vehicle_param, const std::string & ns, const double r, const double g, + const double b) { - const auto current_time = rclcpp::Clock().now(); + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.125), createMarkerColor(r, g, b, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + visualization_msgs::msg::MarkerArray msg; + for (size_t i = 0; i < mpt_traj.size(); ++i) { + const auto & traj_point = mpt_traj.at(i); - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; + marker.text = std::to_string(i); - int unique_id = 0; - int number_of_rect = 0; - for (const auto & rect : rects) { - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.scale = createMarkerScale(0, 0, 0.125); - marker.color = createMarkerColor(r, g, b, 0.99); - marker.text = std::to_string(number_of_rect); - marker.pose.position = rect.top_left; + const double half_width = vehicle_param.width / 2.0; + const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang; + + const auto top_right_pos = + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + .position; + marker.id = i; + marker.pose.position = top_right_pos; msg.markers.push_back(marker); - marker.id = unique_id++; - marker.pose.position = rect.top_right; + + marker.id = i + mpt_traj.size(); + marker.pose.position = top_right_pos; msg.markers.push_back(marker); - marker.id = unique_id++; - number_of_rect++; } return msg; } -visualization_msgs::msg::MarkerArray getPointsMarkerArray( - const std::vector & points, const std::string & ns, const double r, - const double g, const double b) +/* +visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( + const std::vector & ref_points, + const SequentialBoundsCandidates & sequential_bounds_candidates, const double r, const double g, + const double b, const double vehicle_circle_radius, const size_t sampling_num) { - if (points.empty()) { - return visualization_msgs::msg::MarkerArray{}; - } const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; - - const int unique_id = 0; - marker.id = unique_id; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - marker.scale = createMarkerScale(0.5, 0.5, 0.5); - marker.color = createMarkerColor(r, g, b, 0.99); - for (const auto & p : points) { - marker.points.push_back(p.position); + int unique_id = 0; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "bounds_candidates", 0, + visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.05, 0, 0), + createMarkerColor(r + 0.5, g, b, 0.3)); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + + for (size_t p_idx = 0; p_idx < sequential_bounds_candidates.size(); ++p_idx) { + if (p_idx % sampling_num != 0) { + continue; + } + + const auto & bounds_candidates = sequential_bounds_candidates.at(p_idx); + for (size_t b_idx = 0; b_idx < bounds_candidates.size(); ++b_idx) { + const auto & bounds_candidate = bounds_candidates.at(b_idx); + + marker.id = unique_id++; + geometry_msgs::msg::Pose pose; + pose.position = ref_points.at(p_idx).p; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(p_idx).yaw); + + const double lb_y = bounds_candidate.lower_bound - vehicle_circle_radius; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + marker.points.push_back(lb); + + const double ub_y = bounds_candidate.upper_bound + vehicle_circle_radius; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + marker.points.push_back(ub); + } } + msg.markers.push_back(marker); return msg; } +*/ -visualization_msgs::msg::MarkerArray getPointsMarkerArray( - const std::vector & points, const std::string & ns, const double r, - const double g, const double b) +visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray( + const std::vector & ref_points, const double r, const double g, const double b, + const double vehicle_circle_radius, const size_t sampling_num) { - if (points.empty()) { - return visualization_msgs::msg::MarkerArray{}; - } const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; - - const int unique_id = 0; - marker.id = unique_id; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - marker.scale = createMarkerScale(0.7, 0.7, 0.7); - marker.color = createMarkerColor(r, g, b, 0.99); - for (const auto & p : points) { - marker.points.push_back(p); + if (ref_points.empty()) return msg; + + for (size_t bound_idx = 0; bound_idx < ref_points.at(0).vehicle_bounds.size(); ++bound_idx) { + const std::string ns = "base_bounds_" + std::to_string(bound_idx); + + { // lower bound + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + for (size_t i = 0; i < ref_points.size(); i++) { + if (i % sampling_num != 0) { + continue; + } + + const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx); + const double lb_y = + ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - vehicle_circle_radius; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + + marker.points.push_back(pose.position); + marker.points.push_back(lb); + } + msg.markers.push_back(marker); + } + + { // upper bound + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g + 0.5, b, 0.3)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + for (size_t i = 0; i < ref_points.size(); i++) { + if (i % sampling_num != 0) { + continue; + } + + const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx); + const double ub_y = + ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + vehicle_circle_radius; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + + marker.points.push_back(pose.position); + marker.points.push_back(ub); + } + msg.markers.push_back(marker); + } } - msg.markers.push_back(marker); + return msg; } -visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( - const std::vector & points, const std::string & ns, const double r, - const double g, const double b) +visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray( + const std::vector> & vehicle_circles_pose, + const double vehicle_circle_radius, const size_t sampling_num, const std::string & ns, + const double r, const double g, const double b) { - if (points.empty()) { - return visualization_msgs::msg::MarkerArray{}; - } const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; - - int unique_id = 0; - for (std::size_t i = 0; i < points.size(); i++) { - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.scale = createMarkerScale(0, 0, 0.15); - marker.color = createMarkerColor(r, g, b, 0.99); - marker.text = std::to_string(i); - // marker.text = std::to_string(i) + " "+ std::to_string(points[i].position.x)+ " "+ - // std::to_string(points[i].position.y); - marker.pose.position = points[i].position; + for (size_t i = 0; i < vehicle_circles_pose.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 0.25)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + for (size_t j = 0; j < vehicle_circles_pose.at(i).size(); ++j) { + const geometry_msgs::msg::Pose & pose = vehicle_circles_pose.at(i).at(j); + const auto ub = + tier4_autoware_utils::calcOffsetPose(pose, 0.0, vehicle_circle_radius, 0.0).position; + const auto lb = + tier4_autoware_utils::calcOffsetPose(pose, 0.0, -vehicle_circle_radius, 0.0).position; + + marker.points.push_back(ub); + marker.points.push_back(lb); + } msg.markers.push_back(marker); } + return msg; } -visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( - const std::vector & points, - const std::string & ns, const double r, const double g, const double b) +visualization_msgs::msg::MarkerArray getLateralErrorsLineMarkerArray( + const std::vector mpt_ref_poses, std::vector lateral_errors, + const size_t sampling_num, const std::string & ns, const double r, const double g, const double b) { - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 1.0)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + for (size_t i = 0; i < mpt_ref_poses.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + + const auto vehicle_pose = + tier4_autoware_utils::calcOffsetPose(mpt_ref_poses.at(i), 0.0, lateral_errors.at(i), 0.0); + marker.points.push_back(mpt_ref_poses.at(i).position); + marker.points.push_back(vehicle_pose.position); + } - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; + visualization_msgs::msg::MarkerArray msg; + msg.markers.push_back(marker); - int unique_id = 0; - for (std::size_t i = 0; i < points.size(); i++) { - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.scale = createMarkerScale(0, 0, 0.15); - marker.color = createMarkerColor(r, g, b, 0.99); - // marker.text = std::to_string(i) + " "+ std::to_string(points[i].pose.position.x)+ " "+ - // std::to_string(points[i].pose.position.y); - marker.text = std::to_string(i); - marker.pose.position = points[i].pose.position; - msg.markers.push_back(marker); - } return msg; } -visualization_msgs::msg::MarkerArray getBaseBoundsLineMarkerArray( - const std::vector & bounds, const std::vector & candidate_base, - const std::string & ns, const double r, const double g, const double b) +visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray( + const geometry_msgs::msg::Pose & current_ego_pose, + const std::vector & vehicle_circle_longitudinal_offsets, + const double vehicle_circle_radius, const std::string & ns, const double r, const double g, + const double b) { - const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; + size_t id = 0; + for (const double offset : vehicle_circle_longitudinal_offsets) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + marker.pose = tier4_autoware_utils::calcOffsetPose(current_ego_pose, offset, 0.0, 0.0); - int unique_id = 0; - for (std::size_t i = 0; i < bounds.size(); i++) { - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.scale = createMarkerScale(0.05, 0, 0); - marker.color = createMarkerColor(r, g, b, 0.6); - geometry_msgs::msg::Pose pose; - pose = candidate_base[i]; - geometry_msgs::msg::Point rel_lb; - rel_lb.x = 0; - rel_lb.y = bounds[i].c0.lb; - geometry_msgs::msg::Point abs_lb = util::transformToAbsoluteCoordinate2D(rel_lb, pose); - geometry_msgs::msg::Point rel_ub; - rel_ub.x = 0; - rel_ub.y = bounds[i].c0.ub; - geometry_msgs::msg::Point abs_ub = util::transformToAbsoluteCoordinate2D(rel_ub, pose); - marker.points.push_back(abs_lb); - marker.points.push_back(abs_ub); - msg.markers.push_back(marker); - marker.points.clear(); - } - return msg; -} + constexpr size_t circle_dividing_num = 16; + for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { + geometry_msgs::msg::Point edge_pos; -visualization_msgs::msg::MarkerArray getTopBoundsLineMarkerArray( - const std::vector & bounds, const std::vector & candidate_top, - const std::string & ns, const double r, const double g, const double b) -{ - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; + const double edge_angle = + static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; + edge_pos.x = vehicle_circle_radius * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radius * std::sin(edge_angle); - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; + marker.points.push_back(edge_pos); + } - int unique_id = 0; - for (std::size_t i = 0; i < bounds.size(); i++) { - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.scale = createMarkerScale(0.05, 0, 0); - marker.color = createMarkerColor(r, g, b, 0.6); - geometry_msgs::msg::Point rel_lb; - rel_lb.x = 0; - rel_lb.y = bounds[i].c1.lb; - geometry_msgs::msg::Point abs_lb = - util::transformToAbsoluteCoordinate2D(rel_lb, candidate_top[i]); - geometry_msgs::msg::Point rel_ub; - rel_ub.x = 0; - rel_ub.y = bounds[i].c1.ub; - geometry_msgs::msg::Point abs_ub = - util::transformToAbsoluteCoordinate2D(rel_ub, candidate_top[i]); - marker.points.push_back(abs_lb); - marker.points.push_back(abs_ub); msg.markers.push_back(marker); - marker.points.clear(); + ++id; } return msg; } -visualization_msgs::msg::MarkerArray getMidBoundsLineMarkerArray( - const std::vector & bounds, const std::vector & candidate_top, - const std::string & ns, const double r, const double g, const double b) +visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray( + const std::vector & mpt_traj_points, + const std::vector & vehicle_circle_longitudinal_offsets, + const double vehicle_circle_radius, const size_t sampling_num, const std::string & ns, + const double r, const double g, const double b) { - const auto current_time = rclcpp::Clock().now(); visualization_msgs::msg::MarkerArray msg; - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; - - int unique_id = 0; - for (std::size_t i = 0; i < bounds.size(); i++) { - marker.id = unique_id++; - marker.lifetime = rclcpp::Duration::from_seconds(0); - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.scale = createMarkerScale(0.05, 0, 0); - marker.color = createMarkerColor(r, g, b, 0.6); - geometry_msgs::msg::Point rel_lb; - rel_lb.x = 0; - rel_lb.y = bounds[i].c2.lb; - geometry_msgs::msg::Point abs_lb = - util::transformToAbsoluteCoordinate2D(rel_lb, candidate_top[i]); - geometry_msgs::msg::Point rel_ub; - rel_ub.x = 0; - rel_ub.y = bounds[i].c2.ub; - geometry_msgs::msg::Point abs_ub = - util::transformToAbsoluteCoordinate2D(rel_ub, candidate_top[i]); - marker.points.push_back(abs_lb); - marker.points.push_back(abs_ub); - msg.markers.push_back(marker); - marker.points.clear(); + size_t id = 0; + for (size_t i = 0; i < mpt_traj_points.size(); ++i) { + if (i % sampling_num != 0) { + continue; + } + const auto & mpt_traj_point = mpt_traj_points.at(i); + + for (const double offset : vehicle_circle_longitudinal_offsets) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0); + + constexpr size_t circle_dividing_num = 16; + for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) { + geometry_msgs::msg::Point edge_pos; + + const double edge_angle = + static_cast(e_idx) / static_cast(circle_dividing_num) * 2.0 * M_PI; + edge_pos.x = vehicle_circle_radius * std::cos(edge_angle); + edge_pos.y = vehicle_circle_radius * std::sin(edge_angle); + + marker.points.push_back(edge_pos); + } + + msg.markers.push_back(marker); + ++id; + } } return msg; } @@ -726,21 +681,15 @@ visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray( const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, const double b) { - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - - visualization_msgs::msg::Marker marker; - marker.id = 0; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(r, g, b, 0.5)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); marker.pose = pose; - marker.scale = createMarkerScale(0.1, 5.0, 2.0); - marker.color = createMarkerColor(r, g, b, 0.5); + + visualization_msgs::msg::MarkerArray msg; msg.markers.push_back(marker); + return msg; } @@ -748,34 +697,136 @@ visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray( const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g, const double b) { - const auto current_time = rclcpp::Clock().now(); - visualization_msgs::msg::MarkerArray msg; - - visualization_msgs::msg::Marker marker; - marker.id = 0; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = ns; - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::msg::Marker::ADD; + auto marker = createDefaultMarker( + "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(r, g, b, 0.99)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); marker.pose = pose; marker.text = "drivable area"; - marker.scale = createMarkerScale(0.0, 0.0, 1.0); - marker.color = createMarkerColor(r, g, b, 0.99); + + visualization_msgs::msg::MarkerArray msg; msg.markers.push_back(marker); + return msg; } +} // namespace + +namespace debug_visualization +{ +visualization_msgs::msg::MarkerArray getDebugVisualizationMarker( + const std::shared_ptr debug_data_ptr, + const std::vector & optimized_points, + const VehicleParam & vehicle_param, const bool is_showing_debug_detail) +{ + visualization_msgs::msg::MarkerArray vis_marker_array; + + if (is_showing_debug_detail) { + const auto points_marker_array = getDebugPointsMarkers( + debug_data_ptr->interpolated_points, optimized_points, debug_data_ptr->straight_points, + debug_data_ptr->fixed_points, debug_data_ptr->non_fixed_points); + + const auto constrain_marker_array = + getDebugConstrainMarkers(debug_data_ptr->constrain_rectangles, "constrain_rect"); + + appendMarkerArray(points_marker_array, &vis_marker_array); + appendMarkerArray(constrain_marker_array, &vis_marker_array); + + appendMarkerArray( + getRectanglesNumMarkerArray( + debug_data_ptr->mpt_traj, vehicle_param, "num_vehicle_footprint", 0.99, 0.99, 0.2), + &vis_marker_array); + + appendMarkerArray( + getPointsTextMarkerArray(debug_data_ptr->eb_traj, "eb_traj_text", 0.99, 0.99, 0.2), + &vis_marker_array); + } + + // avoiding objects + appendMarkerArray( + getObjectsMarkerArray(debug_data_ptr->avoiding_objects, "avoiding_objects", 0.99, 0.99, 0.2), + &vis_marker_array); + // mpt footprints + appendMarkerArray( + getRectanglesMarkerArray( + debug_data_ptr->mpt_traj, vehicle_param, "mpt_footprints", 0.99, 0.99, 0.2, + debug_data_ptr->mpt_visualize_sampling_num), + &vis_marker_array); + // bounds + appendMarkerArray( + getBoundsLineMarkerArray( + debug_data_ptr->ref_points, 0.99, 0.99, 0.2, debug_data_ptr->vehicle_circle_radius, + debug_data_ptr->mpt_visualize_sampling_num), + &vis_marker_array); + + /* + // bounds candidates + appendMarkerArray( + getBoundsCandidatesLineMarkerArray( + debug_data_ptr->ref_points, debug_data_ptr->sequential_bounds_candidates, 0.2, 0.99, 0.99, + debug_data_ptr->vehicle_circle_radius, debug_data_ptr->mpt_visualize_sampling_num), + &vis_marker_array); + */ + + // vehicle circle line + appendMarkerArray( + getVehicleCircleLineMarkerArray( + debug_data_ptr->vehicle_circles_pose, debug_data_ptr->vehicle_circle_radius, + debug_data_ptr->mpt_visualize_sampling_num, "vehicle_circle_lines", 0.99, 0.99, 0.2), + &vis_marker_array); + + // lateral error line + appendMarkerArray( + getLateralErrorsLineMarkerArray( + debug_data_ptr->mpt_ref_poses, debug_data_ptr->lateral_errors, + debug_data_ptr->mpt_visualize_sampling_num, "lateral_errors", 0.1, 0.1, 0.8), + &vis_marker_array); + + // current vehicle circles + appendMarkerArray( + getCurrentVehicleCirclesMarkerArray( + debug_data_ptr->current_ego_pose, debug_data_ptr->vehicle_circle_longitudinal_offsets, + debug_data_ptr->vehicle_circle_radius, "current_vehicle_circles", 1.0, 0.3, 0.3), + &vis_marker_array); + + // vehicle circles + appendMarkerArray( + getVehicleCirclesMarkerArray( + debug_data_ptr->mpt_traj, debug_data_ptr->vehicle_circle_longitudinal_offsets, + debug_data_ptr->vehicle_circle_radius, debug_data_ptr->mpt_visualize_sampling_num, + "vehicle_circles", 1.0, 0.3, 0.3), + &vis_marker_array); + + return vis_marker_array; +} + +visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker( + const std::shared_ptr debug_data_ptr, const VehicleParam & vehicle_param) +{ + visualization_msgs::msg::MarkerArray vis_marker_array; + if (debug_data_ptr->stop_pose_by_drivable_area) { + const auto virtual_wall_pose = + getVirtualWallPose(debug_data_ptr->stop_pose_by_drivable_area.get(), vehicle_param); + appendMarkerArray( + getVirtualWallMarkerArray(virtual_wall_pose, "virtual_wall", 1.0, 0, 0), &vis_marker_array); + appendMarkerArray( + getVirtualWallTextMarkerArray(virtual_wall_pose, "virtual_wall_text", 1.0, 1.0, 1.0), + &vis_marker_array); + } + return vis_marker_array; +} nav_msgs::msg::OccupancyGrid getDebugCostmap( const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid) { + if (clearance_map.empty()) return nav_msgs::msg::OccupancyGrid(); + cv::Mat tmp; clearance_map.copyTo(tmp); cv::normalize(tmp, tmp, 0, 255, cv::NORM_MINMAX, CV_8UC1); nav_msgs::msg::OccupancyGrid clearance_map_in_og = occupancy_grid; tmp.forEach([&](const unsigned char & value, const int * position) -> void { - process_cv::putOccupancyGridValue(clearance_map_in_og, position[0], position[1], value); + cv_utils::putOccupancyGridValue(clearance_map_in_og, position[0], position[1], value); }); return clearance_map_in_og; } +} // namespace debug_visualization diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index 57dcad29a7c84..6743bc7884e1a 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -14,25 +14,9 @@ #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" +#include "obstacle_avoidance_planner/utils.hpp" -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include +#include "geometry_msgs/msg/vector3.hpp" #include #include @@ -41,66 +25,46 @@ #include EBPathOptimizer::EBPathOptimizer( - const bool is_showing_debug_info, const QPParam qp_param, const TrajectoryParam traj_pram, - const ConstrainParam constrain_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param) -: logger_ros_clock_(RCL_ROS_TIME), - is_showing_debug_info_(is_showing_debug_info), + const bool is_showing_debug_info, const TrajectoryParam & traj_param, const EBParam & eb_param, + const VehicleParam & vehicle_param) +: is_showing_debug_info_(is_showing_debug_info), epsilon_(1e-8), - qp_param_(qp_param), - traj_param_(traj_pram), - constrain_param_(constrain_param), + qp_param_(eb_param.qp_param), + traj_param_(traj_param), + eb_param_(eb_param), vehicle_param_(vehicle_param) { - geometry_msgs::msg::Vector3 keep_space_shape; - keep_space_shape.x = constrain_param_.keep_space_shape_x; - keep_space_shape.y = constrain_param_.keep_space_shape_y; - keep_space_shape_ptr_ = std::make_unique(keep_space_shape); - mpt_optimizer_ptr_ = std::make_unique( - is_showing_debug_info, qp_param, traj_pram, constrain_param, vehicle_param, mpt_param); - initializeSolver(); -} - -EBPathOptimizer::~EBPathOptimizer() {} - -void EBPathOptimizer::initializeSolver() -{ - Eigen::MatrixXd p = makePMatrix(); + const Eigen::MatrixXd p = makePMatrix(); default_a_matrix_ = makeAMatrix(); - std::vector q(traj_param_.num_sampling_points * 2, 0.0); - std::vector lower_bound(traj_param_.num_sampling_points * 2, 0.0); - std::vector upper_bound(traj_param_.num_sampling_points * 2, 0.0); + const int num_points = eb_param_.num_sampling_points_for_eb; + const std::vector q(num_points * 2, 0.0); + const std::vector lower_bound(num_points * 2, 0.0); + const std::vector upper_bound(num_points * 2, 0.0); + osqp_solver_ptr_ = std::make_unique( p, default_a_matrix_, q, lower_bound, upper_bound, qp_param_.eps_abs); osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); osqp_solver_ptr_->updateMaxIter(qp_param_.max_iteration); - ex_osqp_solver_ptr_ = std::make_unique( - p, default_a_matrix_, q, lower_bound, upper_bound, qp_param_.eps_abs); - ex_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - ex_osqp_solver_ptr_->updateMaxIter(qp_param_.max_iteration); - vis_osqp_solver_ptr_ = std::make_unique( - p, default_a_matrix_, q, lower_bound, upper_bound, qp_param_.eps_abs); - vis_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - vis_osqp_solver_ptr_->updateMaxIter(qp_param_.max_iteration); } -/* make positive semidefinite matrix for objective function - reference: https://ieeexplore.ieee.org/document/7402333 */ +// make positive semidefinite matrix for objective function +// reference: https://ieeexplore.ieee.org/document/7402333 Eigen::MatrixXd EBPathOptimizer::makePMatrix() { - Eigen::MatrixXd P = - Eigen::MatrixXd::Zero(traj_param_.num_sampling_points * 2, traj_param_.num_sampling_points * 2); - for (int r = 0; r < traj_param_.num_sampling_points * 2; r++) { - for (int c = 0; c < traj_param_.num_sampling_points * 2; c++) { + const int num_points = eb_param_.num_sampling_points_for_eb; + + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(num_points * 2, num_points * 2); + for (int r = 0; r < num_points * 2; ++r) { + for (int c = 0; c < num_points * 2; ++c) { if (r == c) { - P(r, c) = 6; + P(r, c) = 6.0; } else if (std::abs(c - r) == 1) { - P(r, c) = -4; + P(r, c) = -4.0; } else if (std::abs(c - r) == 2) { - P(r, c) = 1; + P(r, c) = 1.0; } else { - P(r, c) = 0; + P(r, c) = 0.0; } } } @@ -110,37 +74,33 @@ Eigen::MatrixXd EBPathOptimizer::makePMatrix() // make default linear constrain matrix Eigen::MatrixXd EBPathOptimizer::makeAMatrix() { - Eigen::MatrixXd A = Eigen::MatrixXd::Identity( - traj_param_.num_sampling_points * 2, traj_param_.num_sampling_points * 2); - for (int i = 0; i < traj_param_.num_sampling_points * 2; i++) { - if (i < traj_param_.num_sampling_points) { - A(i, i + traj_param_.num_sampling_points) = 1; + const int num_points = eb_param_.num_sampling_points_for_eb; + + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(num_points * 2, num_points * 2); + for (int i = 0; i < num_points * 2; ++i) { + if (i < num_points) { + A(i, i + num_points) = 1.0; } else { - A(i, i - traj_param_.num_sampling_points) = 1; + A(i, i - num_points) = 1.0; } } return A; } -boost::optional EBPathOptimizer::generateOptimizedTrajectory( - const bool enable_avoidance, const geometry_msgs::msg::Pose & ego_pose, - const autoware_auto_planning_msgs::msg::Path & path, - const std::unique_ptr & prev_trajs, - const std::vector & objects, - DebugData * debug_data) +boost::optional> +EBPathOptimizer::getEBTrajectory( + const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path, + const std::unique_ptr & prev_trajs, const double current_ego_vel, + std::shared_ptr debug_data_ptr) { - // processing drivable area - auto t_start1 = std::chrono::high_resolution_clock::now(); - CVMaps cv_maps = process_cv::getMaps(enable_avoidance, path, objects, traj_param_, debug_data); - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Processing driveable area time: = %f [ms]", elapsed_ms1); + stop_watch_.tic(__func__); + + current_ego_vel_ = current_ego_vel; // get candidate points for optimization - CandidatePoints candidate_points = getCandidatePoints( - ego_pose, path.points, prev_trajs, cv_maps.drivable_area, path.drivable_area.info, debug_data); + // decide fix or non fix, might not required only for smoothing purpose + const CandidatePoints candidate_points = + getCandidatePoints(ego_pose, path.points, prev_trajs, debug_data_ptr); if (candidate_points.fixed_points.empty() && candidate_points.non_fixed_points.empty()) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, @@ -148,231 +108,115 @@ boost::optional EBPathOptimizer::generateOptimizedTrajectory( return boost::none; } - // get optimized smooth points - const auto opt_traj_points = getOptimizedTrajectory( - enable_avoidance, path, candidate_points, cv_maps.clearance_map, - cv_maps.only_objects_clearance_map, debug_data); - if (!opt_traj_points) { + // get optimized smooth points with elastic band + const auto eb_traj_points = getOptimizedTrajectory(path, candidate_points, debug_data_ptr); + if (!eb_traj_points) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, "return boost::none since smoothing failed"); return boost::none; } - const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( - enable_avoidance, opt_traj_points.get(), path.points, prev_trajs, cv_maps, ego_pose, - debug_data); - if (!mpt_trajs) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "return boost::none since mpt failed"); - return boost::none; - } - - auto t_start2 = std::chrono::high_resolution_clock::now(); - std::vector extended_traj_points = - getExtendedOptimizedTrajectory(path.points, mpt_trajs.get().mpt, debug_data); - auto t_end2 = std::chrono::high_resolution_clock::now(); - float elapsed_ms2 = std::chrono::duration(t_end2 - t_start2).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Extending trajectory time: = %f [ms]", elapsed_ms2); - - Trajectories traj; - traj.smoothed_trajectory = opt_traj_points.get(); - traj.mpt_ref_points = mpt_trajs.get().ref_points; - traj.model_predictive_trajectory = mpt_trajs.get().mpt; - traj.extended_trajectory = extended_traj_points; - debug_data->smoothed_points = opt_traj_points.get(); - return traj; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return eb_traj_points; } boost::optional> EBPathOptimizer::getOptimizedTrajectory( - [[maybe_unused]] const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const CandidatePoints & candidate_points, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data) + const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points, + std::shared_ptr debug_data_ptr) { + stop_watch_.tic(__func__); + // get constrain rectangles around each point - std::vector interpolated_points = util::getInterpolatedPoints( - candidate_points.fixed_points, candidate_points.non_fixed_points, - traj_param_.delta_arc_length_for_optimization); + auto full_points = candidate_points.fixed_points; + full_points.insert( + full_points.end(), candidate_points.non_fixed_points.begin(), + candidate_points.non_fixed_points.end()); + + // interpolate points for logic purpose + const std::vector interpolated_points = + interpolation_utils::getInterpolatedPoints(full_points, eb_param_.delta_arc_length_for_eb); if (interpolated_points.empty()) { return boost::none; } - debug_data->interpolated_points = interpolated_points; + debug_data_ptr->interpolated_points = interpolated_points; + // number of optimizing points const int farthest_idx = std::min( - (traj_param_.num_sampling_points - 1), static_cast(interpolated_points.size() - 1)); + (eb_param_.num_sampling_points_for_eb - 1), static_cast(interpolated_points.size() - 1)); + // number of fixed points in interpolated_points const int num_fixed_points = getNumFixedPoints(candidate_points.fixed_points, interpolated_points, farthest_idx); - const int straight_line_idx = getStraightLineIdx( - interpolated_points, farthest_idx, only_objects_clearance_map, path.drivable_area.info, - debug_data->straight_points); + // TODO(murooka) try this printing. something may be wrong + // std::cerr << num_fixed_points << std::endl; + + // consider straight after `straight_line_idx` and then tighten space for optimization after + // `straight_line_idx` + const int straight_line_idx = + getStraightLineIdx(interpolated_points, farthest_idx, debug_data_ptr->straight_points); + + // if `farthest_idx` is lower than `number_of_sampling_points`, duplicate the point at the end of + // `interpolated_points` + // This aims for using hotstart by not changing the size of matrix std::vector padded_interpolated_points = getPaddedInterpolatedPoints(interpolated_points, farthest_idx); - auto t_start1 = std::chrono::high_resolution_clock::now(); + const auto rectangles = getConstrainRectangleVec( - path, padded_interpolated_points, num_fixed_points, farthest_idx, straight_line_idx, - clearance_map, only_objects_clearance_map); - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Make constrain rectangle time: = %f [ms]", elapsed_ms1); + path, padded_interpolated_points, num_fixed_points, farthest_idx, straight_line_idx); if (!rectangles) { return boost::none; } - const auto traj_points = calculateTrajectory( - padded_interpolated_points, rectangles.get(), farthest_idx, OptMode::Normal); - debug_data->smoothed_points = traj_points; - return traj_points; -} + const auto traj_points = + calculateTrajectory(padded_interpolated_points, rectangles.get(), farthest_idx, debug_data_ptr); -std::vector -EBPathOptimizer::getExtendedOptimizedTrajectory( - const std::vector & path_points, - const std::vector & optimized_points, - DebugData * debug_data) -{ - if (static_cast(optimized_points.size()) <= traj_param_.num_fix_points_for_extending) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(10000).count(), "[Avoidance] Not extend trajectory"); - return std::vector{}; - } - const double accum_arc_length = util::getArcLength(optimized_points); - if ( - accum_arc_length > traj_param_.trajectory_length || - util::getLastExtendedPoint( - path_points.back(), optimized_points.back().pose, - traj_param_.delta_yaw_threshold_for_closest_point, - traj_param_.max_dist_for_extending_end_point)) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(10000).count(), "[Avoidance] Not extend trajectory"); - return std::vector{}; - } - const int default_idx = -1; - int begin_idx = util::getNearestIdx( - path_points, optimized_points.back().pose, default_idx, - traj_param_.delta_yaw_threshold_for_closest_point); - if (begin_idx == default_idx) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(10000).count(), - "[Avoidance] Not extend traj since could not find nearest idx from last opt point"); - return std::vector{}; - } - begin_idx = std::min( - begin_idx + traj_param_.num_offset_for_begin_idx, static_cast(path_points.size()) - 1); - const double extending_trajectory_length = traj_param_.trajectory_length - accum_arc_length; - const int end_extended_path_idx = - getEndPathIdx(path_points, begin_idx, extending_trajectory_length); - std::vector non_fixed_points; - for (int i = begin_idx; i <= end_extended_path_idx; i++) { - non_fixed_points.push_back(path_points[i].pose); - } - - std::vector fixed_points; - for (int i = traj_param_.num_fix_points_for_extending; i > 0; i--) { - fixed_points.push_back(optimized_points[optimized_points.size() - i].pose); - } - - const double delta_arc_length_for_extending_trajectory = std::fmax( - traj_param_.delta_arc_length_for_optimization, - static_cast(extending_trajectory_length / traj_param_.num_sampling_points)); - std::vector interpolated_points = util::getInterpolatedPoints( - fixed_points, non_fixed_points, delta_arc_length_for_extending_trajectory); - if (interpolated_points.empty()) { - RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("EBPathOptimizer"), logger_ros_clock_, - std::chrono::milliseconds(1000).count(), - "[Avoidance] Not extend traj since empty interpolated points"); - return std::vector{}; - } - const int farthest_idx = std::min( - (traj_param_.num_sampling_points - 1), static_cast(interpolated_points.size() - 1)); - std::vector padded_interpolated_points = - getPaddedInterpolatedPoints(interpolated_points, farthest_idx); - const double arc_length = util::getArcLength(fixed_points); - const int num_fix_points = - static_cast(arc_length) / delta_arc_length_for_extending_trajectory; - std::vector constrain_rectangles = - getConstrainRectangleVec(path_points, padded_interpolated_points, num_fix_points, farthest_idx); - - const auto extended_traj_points = calculateTrajectory( - padded_interpolated_points, constrain_rectangles, farthest_idx, OptMode::Extending); - - const int default_extend_begin_idx = 0; - const int extend_begin_idx = util::getNearestIdxOverPoint( - extended_traj_points, optimized_points.back().pose, default_extend_begin_idx, - traj_param_.delta_yaw_threshold_for_closest_point); - - std::vector extended_traj; - for (std::size_t i = extend_begin_idx; i < extended_traj_points.size(); i++) { - extended_traj.push_back(extended_traj_points[i]); - } - - debug_data->fixed_points_for_extending = fixed_points; - debug_data->non_fixed_points_for_extending = non_fixed_points; - debug_data->constrain_rectangles_for_extending = constrain_rectangles; - debug_data->interpolated_points_for_extending = interpolated_points; - return extended_traj; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return traj_points; } std::vector EBPathOptimizer::calculateTrajectory( const std::vector & padded_interpolated_points, const std::vector & constrain_rectangles, const int farthest_idx, - const OptMode & opt_mode) + std::shared_ptr debug_data_ptr) { + stop_watch_.tic(__func__); + // update constrain for QP based on constrain rectangles - updateConstrain(padded_interpolated_points, constrain_rectangles, opt_mode); + updateConstrain(padded_interpolated_points, constrain_rectangles); // solve QP and get optimized trajectory - auto t_start2 = std::chrono::high_resolution_clock::now(); - std::vector optimized_points = solveQP(opt_mode); - auto t_end2 = std::chrono::high_resolution_clock::now(); - float elapsed_ms2 = std::chrono::duration(t_end2 - t_start2).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, "Optimization time: = %f [ms]", - elapsed_ms2); - auto traj_points = + std::vector optimized_points = solveQP(); + + const auto traj_points = convertOptimizedPointsToTrajectory(optimized_points, constrain_rectangles, farthest_idx); + + if (debug_data_ptr) { + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + } return traj_points; } -std::vector EBPathOptimizer::solveQP(const OptMode & opt_mode) +std::vector EBPathOptimizer::solveQP() { - std::vector optimized_points; - if (opt_mode == OptMode::Normal) { - osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); - osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs); - auto result = osqp_solver_ptr_->optimize(); - optimized_points = std::get<0>(result); - util::logOSQPSolutionStatus(std::get<3>(result)); - } else if (opt_mode == OptMode::Extending) { - ex_osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs_for_extending); - ex_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel_for_extending); - auto result = ex_osqp_solver_ptr_->optimize(); - optimized_points = std::get<0>(result); - util::logOSQPSolutionStatus(std::get<3>(result)); - } else if (opt_mode == OptMode::Visualizing) { - vis_osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs_for_visualizing); - vis_osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel_for_visualizing); - auto result = vis_osqp_solver_ptr_->optimize(); - optimized_points = std::get<0>(result); - util::logOSQPSolutionStatus(std::get<3>(result)); - } + osqp_solver_ptr_->updateEpsRel(qp_param_.eps_rel); + osqp_solver_ptr_->updateEpsAbs(qp_param_.eps_abs); + + const auto result = osqp_solver_ptr_->optimize(); + const auto optimized_points = std::get<0>(result); + + utils::logOSQPSolutionStatus(std::get<3>(result)); + return optimized_points; } std::vector EBPathOptimizer::getFixedPoints( const geometry_msgs::msg::Pose & ego_pose, [[maybe_unused]] const std::vector & path_points, - const std::unique_ptr & prev_trajs, [[maybe_unused]] const cv::Mat & drivable_area, - [[maybe_unused]] const nav_msgs::msg::MapMetaData & map_info) + const std::unique_ptr & prev_trajs) { /* use of prev_traj_points(fine resolution) instead of prev_opt_traj(coarse resolution) stabilize trajectory's yaw*/ @@ -381,19 +225,25 @@ std::vector EBPathOptimizer::getFixedPoints( std::vector empty_points; return empty_points; } - const int default_idx = 0; - int begin_idx = util::getNearestIdx( - prev_trajs->smoothed_trajectory, ego_pose, default_idx, + const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + prev_trajs->smoothed_trajectory, ego_pose, std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); + const int begin_idx = opt_begin_idx ? *opt_begin_idx : 0; const int backward_fixing_idx = std::max( static_cast( begin_idx - traj_param_.backward_fixing_distance / traj_param_.delta_arc_length_for_trajectory), 0); + + // NOTE: Fixed length of EB has to be longer than that of MPT. + constexpr double forward_fixed_length_margin = 5.0; + const double forward_fixed_length = std::max( + current_ego_vel_ * traj_param_.forward_fixing_min_time + forward_fixed_length_margin, + traj_param_.forward_fixing_min_distance); + const int forward_fixing_idx = std::min( static_cast( - begin_idx + - traj_param_.forward_fixing_distance / traj_param_.delta_arc_length_for_trajectory), + begin_idx + forward_fixed_length / traj_param_.delta_arc_length_for_trajectory), static_cast(prev_trajs->smoothed_trajectory.size() - 1)); std::vector fixed_points; for (int i = backward_fixing_idx; i <= forward_fixing_idx; i++) { @@ -406,31 +256,33 @@ std::vector EBPathOptimizer::getFixedPoints( } } -CandidatePoints EBPathOptimizer::getCandidatePoints( +EBPathOptimizer::CandidatePoints EBPathOptimizer::getCandidatePoints( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const std::unique_ptr & prev_trajs, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info, DebugData * debug_data) + const std::unique_ptr & prev_trajs, std::shared_ptr debug_data_ptr) { const std::vector fixed_points = - getFixedPoints(ego_pose, path_points, prev_trajs, drivable_area, map_info); + getFixedPoints(ego_pose, path_points, prev_trajs); if (fixed_points.empty()) { CandidatePoints candidate_points = getDefaultCandidatePoints(path_points); return candidate_points; } - const int default_idx = -1; - int begin_idx = util::getNearestIdx( - path_points, fixed_points.back(), default_idx, + + // try to find non-fix points + const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + path_points, fixed_points.back(), std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); - if (begin_idx == default_idx) { + if (!opt_begin_idx) { CandidatePoints candidate_points; candidate_points.fixed_points = fixed_points; candidate_points.begin_path_idx = path_points.size(); candidate_points.end_path_idx = path_points.size(); return candidate_points; } - begin_idx = std::min( - begin_idx + traj_param_.num_offset_for_begin_idx, static_cast(path_points.size()) - 1); + + const int begin_idx = std::min( + static_cast(opt_begin_idx.get()) + eb_param_.num_offset_for_begin_idx, + static_cast(path_points.size()) - 1); std::vector non_fixed_points; for (size_t i = begin_idx; i < path_points.size(); i++) { @@ -442,8 +294,8 @@ CandidatePoints EBPathOptimizer::getCandidatePoints( candidate_points.begin_path_idx = begin_idx; candidate_points.end_path_idx = path_points.size() - 1; - debug_data->fixed_points = candidate_points.fixed_points; - debug_data->non_fixed_points = candidate_points.non_fixed_points; + debug_data_ptr->fixed_points = candidate_points.fixed_points; + debug_data_ptr->non_fixed_points = candidate_points.non_fixed_points; return candidate_points; } @@ -451,7 +303,7 @@ std::vector EBPathOptimizer::getPaddedInterpolatedPoi const std::vector & interpolated_points, const int farthest_point_idx) { std::vector padded_interpolated_points; - for (int i = 0; i < traj_param_.num_sampling_points; i++) { + for (int i = 0; i < eb_param_.num_sampling_points_for_eb; i++) { if (i > farthest_point_idx) { padded_interpolated_points.push_back(interpolated_points[farthest_point_idx]); } else { @@ -461,7 +313,7 @@ std::vector EBPathOptimizer::getPaddedInterpolatedPoi return padded_interpolated_points; } -CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( +EBPathOptimizer::CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( const std::vector & path_points) { double accum_arc_length = 0; @@ -469,12 +321,11 @@ CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( std::vector fixed_points; for (size_t i = 0; i < path_points.size(); i++) { if (i > 0) { - accum_arc_length += - util::calculate2DDistance(path_points[i].pose.position, path_points[i - 1].pose.position); + accum_arc_length += tier4_autoware_utils::calcDistance2d( + path_points[i].pose.position, path_points[i - 1].pose.position); } if ( - accum_arc_length > - traj_param_.num_sampling_points * traj_param_.delta_arc_length_for_optimization) { + accum_arc_length > eb_param_.num_sampling_points_for_eb * eb_param_.delta_arc_length_for_eb) { break; } end_path_idx = i; @@ -487,99 +338,6 @@ CandidatePoints EBPathOptimizer::getDefaultCandidatePoints( return candidate_points; } -bool EBPathOptimizer::isPointInsideDrivableArea( - const geometry_msgs::msg::Point & point, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info) -{ - bool is_inside = true; - unsigned char occupancy_value = std::numeric_limits::max(); - const auto image_point = util::transformMapToOptionalImage(point, map_info); - if (image_point) { - occupancy_value = drivable_area.ptr( - static_cast(image_point.get().y))[static_cast(image_point.get().x)]; - } - if (!image_point || occupancy_value < epsilon_) { - is_inside = false; - } - return is_inside; -} - -int EBPathOptimizer::getEndPathIdx( - const std::vector & path_points, - const int begin_path_idx, const double required_trajectory_length) -{ - double accum_dist = 0; - int end_path_idx = begin_path_idx; - for (std::size_t i = begin_path_idx; i < path_points.size(); i++) { - if (static_cast(i) > begin_path_idx) { - const double dist = - util::calculate2DDistance(path_points[i].pose.position, path_points[i - 1].pose.position); - accum_dist += dist; - } - end_path_idx = i; - if (accum_dist > required_trajectory_length) { - break; - } - } - return end_path_idx; -} - -int EBPathOptimizer::getEndPathIdxInsideArea( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const int begin_path_idx, const cv::Mat & drivable_area, - const nav_msgs::msg::MapMetaData & map_info) -{ - int end_path_idx = path_points.size() - 1; - const int default_idx = 0; - const int initial_idx = util::getNearestIdx( - path_points, ego_pose, default_idx, traj_param_.delta_yaw_threshold_for_closest_point); - for (std::size_t i = initial_idx; i < path_points.size(); i++) { - geometry_msgs::msg::Point p = path_points[i].pose.position; - geometry_msgs::msg::Point top_image_point; - end_path_idx = i; - if (util::transformMapToImage(p, map_info, top_image_point)) { - const unsigned char value = drivable_area.ptr( - static_cast(top_image_point.y))[static_cast(top_image_point.x)]; - if (value < epsilon_) { - break; - } - } else { - break; - } - } - - int end_path_idx_inside_drivable_area = begin_path_idx; - for (int i = end_path_idx; i >= begin_path_idx; i--) { - geometry_msgs::msg::Point rel_top_left_point; - rel_top_left_point.x = vehicle_param_.length; - rel_top_left_point.y = vehicle_param_.width * 0.5; - geometry_msgs::msg::Point abs_top_left_point = - util::transformToAbsoluteCoordinate2D(rel_top_left_point, path_points[i].pose); - geometry_msgs::msg::Point top_left_image_point; - - geometry_msgs::msg::Point rel_top_right_point; - rel_top_right_point.x = vehicle_param_.length; - rel_top_right_point.y = -vehicle_param_.width * 0.5; - geometry_msgs::msg::Point abs_top_right_point = - util::transformToAbsoluteCoordinate2D(rel_top_right_point, path_points[i].pose); - geometry_msgs::msg::Point top_right_image_point; - if ( - util::transformMapToImage(abs_top_left_point, map_info, top_left_image_point) && - util::transformMapToImage(abs_top_right_point, map_info, top_right_image_point)) { - const unsigned char top_left_occupancy_value = drivable_area.ptr( - static_cast(top_left_image_point.y))[static_cast(top_left_image_point.x)]; - const unsigned char top_right_occupancy_value = drivable_area.ptr( - static_cast(top_right_image_point.y))[static_cast(top_right_image_point.x)]; - if (top_left_occupancy_value > epsilon_ && top_right_occupancy_value > epsilon_) { - end_path_idx_inside_drivable_area = i; - break; - } - } - } - return end_path_idx_inside_drivable_area; -} - int EBPathOptimizer::getNumFixedPoints( const std::vector & fixed_points, const std::vector & interpolated_points, const int farthest_idx) @@ -587,7 +345,7 @@ int EBPathOptimizer::getNumFixedPoints( int num_fixed_points = 0; if (!fixed_points.empty() && !interpolated_points.empty()) { std::vector interpolated_points = - util::getInterpolatedPoints(fixed_points, traj_param_.delta_arc_length_for_optimization); + interpolation_utils::getInterpolatedPoints(fixed_points, eb_param_.delta_arc_length_for_eb); num_fixed_points = interpolated_points.size(); } num_fixed_points = std::min(num_fixed_points, farthest_idx); @@ -603,16 +361,16 @@ EBPathOptimizer::convertOptimizedPointsToTrajectory( for (int i = 0; i <= farthest_idx; i++) { autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; tmp_point.pose.position.x = optimized_points[i]; - tmp_point.pose.position.y = optimized_points[i + traj_param_.num_sampling_points]; + tmp_point.pose.position.y = optimized_points[i + eb_param_.num_sampling_points_for_eb]; tmp_point.longitudinal_velocity_mps = constraints[i].velocity; traj_points.push_back(tmp_point); } for (size_t i = 0; i < traj_points.size(); i++) { if (i > 0) { - traj_points[i].pose.orientation = util::getQuaternionFromPoints( + traj_points[i].pose.orientation = geometry_utils::getQuaternionFromPoints( traj_points[i].pose.position, traj_points[i - 1].pose.position); } else if (i == 0 && traj_points.size() > 1) { - traj_points[i].pose.orientation = util::getQuaternionFromPoints( + traj_points[i].pose.orientation = geometry_utils::getQuaternionFromPoints( traj_points[i + 1].pose.position, traj_points[i].pose.position); } } @@ -622,402 +380,84 @@ EBPathOptimizer::convertOptimizedPointsToTrajectory( boost::optional> EBPathOptimizer::getConstrainRectangleVec( const autoware_auto_planning_msgs::msg::Path & path, const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx, - [[maybe_unused]] const cv::Mat & clearance_map, const cv::Mat & only_objects_clearance_map) + const int farthest_point_idx, const int straight_idx) { - const nav_msgs::msg::MapMetaData map_info = path.drivable_area.info; - std::vector smooth_constrain_rects(traj_param_.num_sampling_points); - for (int i = 0; i < traj_param_.num_sampling_points; i++) { + auto curvatures = points_utils::calcCurvature(interpolated_points, 10); + + std::vector smooth_constrain_rects(eb_param_.num_sampling_points_for_eb); + for (int i = 0; i < eb_param_.num_sampling_points_for_eb; i++) { + // `Anchor` has pose + velocity const Anchor anchor = getAnchor(interpolated_points, i, path.points); + + // four types of rectangle for various types if (i == 0 || i == 1 || i >= farthest_point_idx - 1 || i < num_fixed_points - 1) { - const auto rect = getConstrainRectangle(anchor, constrain_param_.clearance_for_fixing); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + // rect has four points for a rectangle in map coordinate + const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_fixing); + smooth_constrain_rects[i] = rect; } else if ( // NOLINT - i >= num_fixed_points - traj_param_.num_joint_buffer_points && - i <= num_fixed_points + traj_param_.num_joint_buffer_points) { - const auto rect = getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + i >= num_fixed_points - eb_param_.num_joint_buffer_points && + i <= num_fixed_points + eb_param_.num_joint_buffer_points) { + const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_joint); + smooth_constrain_rects[i] = rect; } else if (i >= straight_idx) { - const auto rect = getConstrainRectangle(anchor, constrain_param_.clearance_for_straight_line); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + const auto rect = getConstrainRectangle(anchor, eb_param_.clearance_for_straight_line); + smooth_constrain_rects[i] = rect; } else { - const auto rect = - getConstrainRectangle(anchor, constrain_param_.clearance_for_only_smoothing); - const auto updated_rect = getUpdatedConstrainRectangle( - rect, anchor.pose.position, map_info, only_objects_clearance_map); - smooth_constrain_rects[i] = updated_rect; + const double min_x = -eb_param_.clearance_for_only_smoothing; + const double max_x = eb_param_.clearance_for_only_smoothing; + const double min_y = curvatures[i] > 0 ? 0 : -eb_param_.clearance_for_only_smoothing; + const double max_y = curvatures[i] <= 0 ? 0 : eb_param_.clearance_for_only_smoothing; + const auto rect = getConstrainRectangle(anchor, min_x, max_x, min_y, max_y); + smooth_constrain_rects[i] = rect; } } return smooth_constrain_rects; } -boost::optional> EBPathOptimizer::getConstrainRectangleVec( - const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx, const int straight_idx, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, DebugData * debug_data) -{ - const nav_msgs::msg::MapMetaData map_info = path.drivable_area.info; - std::vector object_road_constrain_ranges(traj_param_.num_sampling_points); - std::vector road_constrain_ranges(traj_param_.num_sampling_points); - std::vector only_smooth_constrain_ranges(traj_param_.num_sampling_points); - for (int i = 0; i < traj_param_.num_sampling_points; i++) { - const Anchor anchor = getAnchor(interpolated_points, i, path.points); - if (i == 0 || i == 1 || i >= farthest_point_idx - 1 || i < num_fixed_points - 1) { - const ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_fixing); - object_road_constrain_ranges[i] = getUpdatedConstrainRectangle( - rectangle, anchor.pose.position, map_info, only_objects_clearance_map); - road_constrain_ranges[i] = rectangle; - only_smooth_constrain_ranges[i] = rectangle; - } else { - if ( - i >= num_fixed_points - traj_param_.num_joint_buffer_points && - i <= num_fixed_points + traj_param_.num_joint_buffer_points) { - const ConstrainRectangle rectangle = - getConstrainRectangle(path.points, anchor, clearance_map, map_info); - object_road_constrain_ranges[i] = getUpdatedConstrainRectangle( - rectangle, anchor.pose.position, map_info, only_objects_clearance_map); - road_constrain_ranges[i] = rectangle; - only_smooth_constrain_ranges[i] = rectangle; - } else { - if (i >= straight_idx) { - const ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_straight_line); - object_road_constrain_ranges[i] = getUpdatedConstrainRectangle( - rectangle, anchor.pose.position, map_info, only_objects_clearance_map); - road_constrain_ranges[i] = rectangle; - only_smooth_constrain_ranges[i] = rectangle; - } else { - const ConstrainRectangles constrain_rectangles = - getConstrainRectangles(anchor, clearance_map, only_objects_clearance_map, map_info); - object_road_constrain_ranges[i] = constrain_rectangles.object_constrain_rectangle; - road_constrain_ranges[i] = constrain_rectangles.road_constrain_rectangle; - only_smooth_constrain_ranges[i] = - getConstrainRectangle(anchor, constrain_param_.clearance_for_only_smoothing); - } - } - } - } - debug_data->foa_data = - getFOAData(object_road_constrain_ranges, interpolated_points, farthest_point_idx); - boost::optional> constrain_ranges = - getPostProcessedConstrainRectangles( - enable_avoidance, object_road_constrain_ranges, road_constrain_ranges, - only_smooth_constrain_ranges, interpolated_points, path.points, farthest_point_idx, - num_fixed_points, straight_idx, debug_data); - return constrain_ranges; -} - -std::vector EBPathOptimizer::getConstrainRectangleVec( - const std::vector & path_points, - const std::vector & interpolated_points, const int num_fixed_points, - const int farthest_point_idx) -{ - std::vector only_smooth_constrain_ranges(traj_param_.num_sampling_points); - for (int i = 0; i < traj_param_.num_sampling_points; i++) { - const Anchor anchor = getAnchor(interpolated_points, i, path_points); - if (i < num_fixed_points || i >= farthest_point_idx - 1) { - ConstrainRectangle rectangle = getConstrainRectangle(anchor, 0); - only_smooth_constrain_ranges[i] = rectangle; - } else { - if ( - i >= num_fixed_points && - i <= num_fixed_points + traj_param_.num_joint_buffer_points_for_extending) { - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.range_for_extend_joint); - only_smooth_constrain_ranges[i] = rectangle; - } else { - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_only_smoothing); - only_smooth_constrain_ranges[i] = rectangle; - } - } - } - return only_smooth_constrain_ranges; -} - -boost::optional> -EBPathOptimizer::getPostProcessedConstrainRectangles( - const bool enable_avoidance, const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - const int farthest_point_idx, const int num_fixed_points, const int straight_idx, - DebugData * debug_data) const -{ - bool is_using_road_constrain = false; - if (!enable_avoidance) { - is_using_road_constrain = true; - } - bool is_using_only_smooth_constrain = false; - if (isFixingPathPoint(path_points)) { - is_using_only_smooth_constrain = true; - } - if (constrain_param_.is_getting_constraints_close2path_points) { - return getConstrainRectanglesClose2PathPoints( - is_using_only_smooth_constrain, is_using_road_constrain, object_constrains, road_constrains, - only_smooth_constrains, debug_data); - - } else { - return getConstrainRectanglesWithinArea( - is_using_only_smooth_constrain, is_using_road_constrain, farthest_point_idx, num_fixed_points, - straight_idx, object_constrains, road_constrains, only_smooth_constrains, interpolated_points, - path_points, debug_data); - } -} - -boost::optional> -EBPathOptimizer::getConstrainRectanglesClose2PathPoints( - const bool is_using_only_smooth_constrain, const bool is_using_road_constrain, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const -{ - if (is_using_only_smooth_constrain) { - return only_smooth_constrains; - } - if (is_using_road_constrain) { - return getValidConstrainRectangles(road_constrains, only_smooth_constrains, debug_data); - } else { - return getValidConstrainRectangles(object_constrains, only_smooth_constrains, debug_data); - } -} - -boost::optional> EBPathOptimizer::getValidConstrainRectangles( - const std::vector & constrains, - const std::vector & only_smooth_constrains, DebugData * debug_data) const -{ - bool only_smooth = true; - int debug_cnt = 0; - for (const auto & rect : constrains) { - if (rect.is_empty_driveable_area) { - debug_data->constrain_rectangles = constrains; - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, "Constraint failed at %d", - debug_cnt); - return boost::none; - } - if (!rect.is_including_only_smooth_range) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Constraint does not include only smooth range at %d", debug_cnt); - only_smooth = false; - } - debug_cnt++; - } - if (only_smooth) { - debug_data->constrain_rectangles = only_smooth_constrains; - return only_smooth_constrains; - } else { - debug_data->constrain_rectangles = constrains; - return constrains; - } -} - -boost::optional> EBPathOptimizer::getConstrainRectanglesWithinArea( - bool is_using_only_smooth_constrain, bool is_using_road_constrain, const int farthest_point_idx, - const int num_fixed_points, const int straight_idx, - const std::vector & object_constrains, - const std::vector & road_constrains, - const std::vector & only_smooth_constrains, - const std::vector & interpolated_points, - const std::vector & path_points, - DebugData * debug_data) const -{ - if (is_using_road_constrain) { - debug_data->constrain_rectangles = road_constrains; - } else if (is_using_only_smooth_constrain) { - debug_data->constrain_rectangles = only_smooth_constrains; - } else { - debug_data->constrain_rectangles = object_constrains; - } - - std::vector constrain_ranges(traj_param_.num_sampling_points); - int origin_dynamic_joint_idx = traj_param_.num_sampling_points; - for (int i = 0; i < traj_param_.num_sampling_points; i++) { - if (isPreFixIdx(i, farthest_point_idx, num_fixed_points, straight_idx)) { - constrain_ranges[i] = only_smooth_constrains[i]; - if (object_constrains[i].is_empty_driveable_area) { - is_using_road_constrain = true; - return boost::none; - } - } else { - if ( - i > origin_dynamic_joint_idx && - i <= origin_dynamic_joint_idx + traj_param_.num_joint_buffer_points) { - const Anchor anchor = getAnchor(interpolated_points, i, path_points); - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - constrain_ranges[i] = rectangle; - } else if (is_using_only_smooth_constrain) { - constrain_ranges[i] = only_smooth_constrains[i]; - } else if (is_using_road_constrain) { - constrain_ranges[i] = road_constrains[i]; - if (constrain_ranges[i].is_empty_driveable_area) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Only road clearance optimization failed at %d", i); - is_using_only_smooth_constrain = true; - origin_dynamic_joint_idx = i; - return boost::none; - } - } else { - constrain_ranges[i] = object_constrains[i]; - if (constrain_ranges[i].is_empty_driveable_area) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("EBPathOptimizer"), is_showing_debug_info_, - "Object clearance optimization failed at %d", i); - return boost::none; - } - } - } - } - return constrain_ranges; -} - -bool EBPathOptimizer::isPreFixIdx( - const int target_idx, const int farthest_point_idx, const int num_fixed_points, - const int straight_idx) const -{ - if ( - target_idx == 0 || target_idx == 1 || target_idx >= farthest_point_idx - 1 || - target_idx < num_fixed_points - 1 || - (target_idx >= num_fixed_points - traj_param_.num_joint_buffer_points && - target_idx <= num_fixed_points + traj_param_.num_joint_buffer_points) || - target_idx >= straight_idx) { - return true; - } else { - return false; - } -} - -bool EBPathOptimizer::isClose2Object( - const geometry_msgs::msg::Point & point, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map, const double distance_threshold) const -{ - const auto image_point = util::transformMapToOptionalImage(point, map_info); - if (!image_point) { - return false; - } - const float object_clearance = only_objects_clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - if (object_clearance < distance_threshold) { - return true; - } - return false; -} - void EBPathOptimizer::updateConstrain( const std::vector & interpolated_points, - const std::vector & rectangle_points, const OptMode & opt_mode) + const std::vector & rectangle_points) { + const int num_points = eb_param_.num_sampling_points_for_eb; + Eigen::MatrixXd A = default_a_matrix_; - std::vector lower_bound(traj_param_.num_sampling_points * 2, 0.0); - std::vector upper_bound(traj_param_.num_sampling_points * 2, 0.0); - for (int i = 0; i < traj_param_.num_sampling_points; ++i) { + std::vector lower_bound(num_points * 2, 0.0); + std::vector upper_bound(num_points * 2, 0.0); + for (int i = 0; i < num_points; ++i) { Constrain constrain = getConstrainFromConstrainRectangle(interpolated_points[i], rectangle_points[i]); A(i, i) = constrain.top_and_bottom.x_coef; - A(i, i + traj_param_.num_sampling_points) = constrain.top_and_bottom.y_coef; - A(i + traj_param_.num_sampling_points, i) = constrain.left_and_right.x_coef; - A(i + traj_param_.num_sampling_points, i + traj_param_.num_sampling_points) = - constrain.left_and_right.y_coef; + A(i, i + num_points) = constrain.top_and_bottom.y_coef; + A(i + num_points, i) = constrain.left_and_right.x_coef; + A(i + num_points, i + num_points) = constrain.left_and_right.y_coef; lower_bound[i] = constrain.top_and_bottom.lower_bound; upper_bound[i] = constrain.top_and_bottom.upper_bound; - lower_bound[i + traj_param_.num_sampling_points] = constrain.left_and_right.lower_bound; - upper_bound[i + traj_param_.num_sampling_points] = constrain.left_and_right.upper_bound; - } - if (opt_mode == OptMode::Normal) { - osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - osqp_solver_ptr_->updateA(A); - } else if (opt_mode == OptMode::Extending) { - ex_osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - ex_osqp_solver_ptr_->updateA(A); - } else if (opt_mode == OptMode::Visualizing) { - vis_osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); - vis_osqp_solver_ptr_->updateA(A); + lower_bound[i + num_points] = constrain.left_and_right.lower_bound; + upper_bound[i + num_points] = constrain.left_and_right.upper_bound; } -} - -Rectangle EBPathOptimizer::getAbsShapeRectangle( - const Rectangle & rel_shape_rectangle_points, const geometry_msgs::msg::Point & offset_point, - const geometry_msgs::msg::Pose & origin) const -{ - geometry_msgs::msg::Point abs_target_point = - util::transformToAbsoluteCoordinate2D(offset_point, origin); - - geometry_msgs::msg::Point abs_top_left; - abs_top_left.x = (rel_shape_rectangle_points.top_left.x + abs_target_point.x); - abs_top_left.y = (rel_shape_rectangle_points.top_left.y + abs_target_point.y); - - geometry_msgs::msg::Point abs_top_right; - abs_top_right.x = (rel_shape_rectangle_points.top_right.x + abs_target_point.x); - abs_top_right.y = (rel_shape_rectangle_points.top_right.y + abs_target_point.y); - - geometry_msgs::msg::Point abs_bottom_left; - abs_bottom_left.x = (rel_shape_rectangle_points.bottom_left.x + abs_target_point.x); - abs_bottom_left.y = (rel_shape_rectangle_points.bottom_left.y + abs_target_point.y); - - geometry_msgs::msg::Point abs_bottom_right; - abs_bottom_right.x = (rel_shape_rectangle_points.bottom_right.x + abs_target_point.x); - abs_bottom_right.y = (rel_shape_rectangle_points.bottom_right.y + abs_target_point.y); - Rectangle abs_shape_rectangle_points; - abs_shape_rectangle_points.top_left = abs_top_left; - abs_shape_rectangle_points.top_right = abs_top_right; - abs_shape_rectangle_points.bottom_left = abs_bottom_left; - abs_shape_rectangle_points.bottom_right = abs_bottom_right; - return abs_shape_rectangle_points; + osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); + osqp_solver_ptr_->updateA(A); } -geometry_msgs::msg::Pose EBPathOptimizer::getOriginPose( - const std::vector & interpolated_points, const int interpolated_idx, - const std::vector & path_points) -{ - geometry_msgs::msg::Pose pose; - pose.position = interpolated_points[interpolated_idx]; - if (interpolated_idx > 0) { - pose.orientation = util::getQuaternionFromPoints( - interpolated_points[interpolated_idx], interpolated_points[interpolated_idx - 1]); - } else if (interpolated_idx == 0 && interpolated_points.size() > 1) { - pose.orientation = util::getQuaternionFromPoints( - interpolated_points[interpolated_idx + 1], interpolated_points[interpolated_idx]); - } - const int default_idx = 0; - const int nearest_id = util::getNearestIdx( - path_points, pose, default_idx, traj_param_.delta_yaw_threshold_for_closest_point); - const geometry_msgs::msg::Quaternion nearest_q = path_points[nearest_id].pose.orientation; - geometry_msgs::msg::Pose origin; - origin.position = interpolated_points[interpolated_idx]; - origin.orientation = nearest_q; - return origin; -} - -Anchor EBPathOptimizer::getAnchor( +EBPathOptimizer::Anchor EBPathOptimizer::getAnchor( const std::vector & interpolated_points, const int interpolated_idx, const std::vector & path_points) const { geometry_msgs::msg::Pose pose; pose.position = interpolated_points[interpolated_idx]; if (interpolated_idx > 0) { - pose.orientation = util::getQuaternionFromPoints( + pose.orientation = geometry_utils::getQuaternionFromPoints( interpolated_points[interpolated_idx], interpolated_points[interpolated_idx - 1]); } else if (interpolated_idx == 0 && interpolated_points.size() > 1) { - pose.orientation = util::getQuaternionFromPoints( + pose.orientation = geometry_utils::getQuaternionFromPoints( interpolated_points[interpolated_idx + 1], interpolated_points[interpolated_idx]); } - const int default_idx = 0; - const int nearest_idx = util::getNearestIdx( - path_points, pose, default_idx, traj_param_.delta_yaw_threshold_for_closest_point); + const auto opt_nearest_idx = tier4_autoware_utils::findNearestIndex( + path_points, pose, std::numeric_limits::max(), + traj_param_.delta_yaw_threshold_for_closest_point); + const int nearest_idx = opt_nearest_idx ? *opt_nearest_idx : 0; + const geometry_msgs::msg::Quaternion nearest_q = path_points[nearest_idx].pose.orientation; Anchor anchor; anchor.pose.position = interpolated_points[interpolated_idx]; @@ -1026,268 +466,26 @@ Anchor EBPathOptimizer::getAnchor( return anchor; } -boost::optional>> -EBPathOptimizer::getOccupancyPoints( - const geometry_msgs::msg::Pose & origin, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - geometry_msgs::msg::Point image_point; - if (!util::transformMapToImage(origin.position, map_info, image_point)) { - return boost::none; - } - const float clearance = std::max( - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)] * - map_info.resolution, - static_cast(keep_space_shape_ptr_->y)); - const float y_constrain_search_range = clearance - 0.5 * keep_space_shape_ptr_->y; - int y_side_length = 0; - for (float y = -y_constrain_search_range; y <= y_constrain_search_range + epsilon_; - y += map_info.resolution * constrain_param_.coef_y_constrain_search_resolution) { - y_side_length++; - } - const float x_constrain_search_range = - std::fmin(constrain_param_.max_x_constrain_search_range, y_constrain_search_range); - int x_side_length = 0; - for (float x = -1 * x_constrain_search_range; x <= x_constrain_search_range + epsilon_; - x += map_info.resolution * constrain_param_.coef_x_constrain_search_resolution) { - x_side_length++; - } - if (x_side_length == 0 || y_side_length == 0) { - return boost::none; - } - std::vector> occupancy_points( - x_side_length, std::vector(y_side_length)); - int x_idx_in_occupancy_map = 0; - int y_idx_in_occupancy_map = 0; - for (float x = -1 * x_constrain_search_range; x <= x_constrain_search_range + epsilon_; - x += map_info.resolution * constrain_param_.coef_x_constrain_search_resolution) { - for (float y = -1 * y_constrain_search_range; y <= y_constrain_search_range + epsilon_; - y += map_info.resolution * constrain_param_.coef_y_constrain_search_resolution) { - geometry_msgs::msg::Point relative_point; - relative_point.x = x; - relative_point.y = y; - geometry_msgs::msg::Point abs_point = - util::transformToAbsoluteCoordinate2D(relative_point, origin); - const int x_idx = x_side_length - x_idx_in_occupancy_map - 1; - const int y_idx = y_side_length - y_idx_in_occupancy_map - 1; - if (x_idx < 0 || x_idx >= x_side_length || y_idx < 0 || y_idx >= y_side_length) { - continue; - } - occupancy_points[x_idx][y_idx] = abs_point; - y_idx_in_occupancy_map++; - } - x_idx_in_occupancy_map++; - y_idx_in_occupancy_map = 0; - } - return occupancy_points; -} - -Rectangle EBPathOptimizer::getRelShapeRectangle( - const geometry_msgs::msg::Vector3 & vehicle_shape, const geometry_msgs::msg::Pose & origin) const -{ - geometry_msgs::msg::Point top_left; - top_left.x = vehicle_shape.x; - top_left.y = 0.5 * vehicle_shape.y; - geometry_msgs::msg::Point top_right; - top_right.x = vehicle_shape.x; - top_right.y = -0.5 * vehicle_shape.y; - geometry_msgs::msg::Point bottom_left; - bottom_left.x = 0.0; - bottom_left.y = 0.5 * vehicle_shape.y; - geometry_msgs::msg::Point bottom_right; - bottom_right.x = 0.0; - bottom_right.y = -0.5 * vehicle_shape.y; - - geometry_msgs::msg::Pose tmp_origin; - tmp_origin.orientation = origin.orientation; - top_left = util::transformToAbsoluteCoordinate2D(top_left, tmp_origin); - top_right = util::transformToAbsoluteCoordinate2D(top_right, tmp_origin); - bottom_left = util::transformToAbsoluteCoordinate2D(bottom_left, tmp_origin); - bottom_right = util::transformToAbsoluteCoordinate2D(bottom_right, tmp_origin); - Rectangle rectangle; - rectangle.top_left = top_left; - rectangle.top_right = top_right; - rectangle.bottom_left = bottom_left; - rectangle.bottom_right = bottom_right; - return rectangle; -} - -ConstrainRectangles EBPathOptimizer::getConstrainRectangles( - const Anchor & anchor, const cv::Mat & clearance_map, const cv::Mat & only_objects_clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const auto occupancy_points_opt = getOccupancyPoints(anchor.pose, clearance_map, map_info); - const auto image_point = util::transformMapToOptionalImage(anchor.pose.position, map_info); - if (!image_point || !occupancy_points_opt) { - ConstrainRectangle rectangle = - getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - rectangle.is_empty_driveable_area = true; - ConstrainRectangles constrain_rectangles; - constrain_rectangles.object_constrain_rectangle = rectangle; - constrain_rectangles.road_constrain_rectangle = rectangle; - return constrain_rectangles; - } - OccupancyMaps occupancy_maps = getOccupancyMaps( - occupancy_points_opt.get(), anchor.pose, image_point.get(), clearance_map, - only_objects_clearance_map, map_info); - - ConstrainRectangle object_constrain = getConstrainRectangle( - occupancy_maps.object_occupancy_map, occupancy_points_opt.get(), anchor, map_info, - only_objects_clearance_map); - - ConstrainRectangles constrain_rectangles; - constrain_rectangles.object_constrain_rectangle = getUpdatedConstrainRectangle( - object_constrain, anchor.pose.position, map_info, only_objects_clearance_map); - constrain_rectangles.road_constrain_rectangle = getConstrainRectangle( - occupancy_maps.road_occupancy_map, occupancy_points_opt.get(), anchor, map_info, - only_objects_clearance_map); - return constrain_rectangles; -} - -OccupancyMaps EBPathOptimizer::getOccupancyMaps( - const std::vector> & occupancy_points, - const geometry_msgs::msg::Pose & origin_pose, - const geometry_msgs::msg::Point & origin_point_in_image, const cv::Mat & clearance_map, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info) const -{ - Rectangle rel_shape_rectangles = getRelShapeRectangle(*keep_space_shape_ptr_, origin_pose); - const float clearance = std::max( - clearance_map.ptr( - static_cast(origin_point_in_image.y))[static_cast(origin_point_in_image.x)] * - map_info.resolution, - static_cast(keep_space_shape_ptr_->y)); - const float y_constrain_search_range = clearance - 0.5 * keep_space_shape_ptr_->y; - const float x_constrain_search_range = - std::fmin(constrain_param_.max_x_constrain_search_range, y_constrain_search_range); - std::vector> object_occupancy_map( - occupancy_points.size(), std::vector(occupancy_points.front().size(), 0)); - std::vector> road_occupancy_map( - occupancy_points.size(), std::vector(occupancy_points.front().size(), 0)); - int x_idx_in_occupancy_map = 0; - int y_idx_in_occupancy_map = 0; - for (float x = -1 * x_constrain_search_range; x <= x_constrain_search_range + epsilon_; - x += map_info.resolution * constrain_param_.coef_x_constrain_search_resolution) { - for (float y = -1 * y_constrain_search_range; y <= y_constrain_search_range + epsilon_; - y += map_info.resolution * constrain_param_.coef_y_constrain_search_resolution) { - geometry_msgs::msg::Point rel_target_point; - rel_target_point.x = x; - rel_target_point.y = y; - Rectangle abs_shape_rectangles = - getAbsShapeRectangle(rel_shape_rectangles, rel_target_point, origin_pose); - float top_left_clearance = std::numeric_limits::lowest(); - float top_left_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point top_left_image; - if (util::transformMapToImage(abs_shape_rectangles.top_left, map_info, top_left_image)) { - top_left_clearance = clearance_map.ptr(static_cast( - top_left_image.y))[static_cast(top_left_image.x)] * - map_info.resolution; - top_left_objects_clearance = only_objects_clearance_map.ptr(static_cast( - top_left_image.y))[static_cast(top_left_image.x)] * - map_info.resolution; - } - - float top_right_clearance = std::numeric_limits::lowest(); - float top_right_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point top_right_image; - if (util::transformMapToImage(abs_shape_rectangles.top_right, map_info, top_right_image)) { - top_right_clearance = clearance_map.ptr(static_cast( - top_right_image.y))[static_cast(top_right_image.x)] * - map_info.resolution; - top_right_objects_clearance = only_objects_clearance_map.ptr(static_cast( - top_right_image.y))[static_cast(top_right_image.x)] * - map_info.resolution; - } - float bottom_left_clearance = std::numeric_limits::lowest(); - float bottom_left_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point bottom_left_image; - if (util::transformMapToImage( - abs_shape_rectangles.bottom_left, map_info, bottom_left_image)) { - bottom_left_clearance = clearance_map.ptr(static_cast( - bottom_left_image.y))[static_cast(bottom_left_image.x)] * - map_info.resolution; - bottom_left_objects_clearance = - only_objects_clearance_map.ptr( - static_cast(bottom_left_image.y))[static_cast(bottom_left_image.x)] * - map_info.resolution; - } - float bottom_right_clearance = std::numeric_limits::lowest(); - float bottom_right_objects_clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point bottom_right_image; - if (util::transformMapToImage( - abs_shape_rectangles.bottom_right, map_info, bottom_right_image)) { - bottom_right_clearance = clearance_map.ptr(static_cast( - bottom_right_image.y))[static_cast(bottom_right_image.x)] * - map_info.resolution; - bottom_right_objects_clearance = - only_objects_clearance_map.ptr( - static_cast(bottom_right_image.y))[static_cast(bottom_right_image.x)] * - map_info.resolution; - } - - const int x_idx = occupancy_points.size() - x_idx_in_occupancy_map - 1; - const int y_idx = occupancy_points.front().size() - y_idx_in_occupancy_map - 1; - if ( - x_idx < 0 || x_idx >= static_cast(occupancy_points.size()) || y_idx < 0 || - y_idx >= static_cast(occupancy_points.front().size())) { - continue; - } - if ( - top_left_clearance < constrain_param_.clearance_from_road || - top_right_clearance < constrain_param_.clearance_from_road || - bottom_right_clearance < constrain_param_.clearance_from_road || - bottom_left_clearance < constrain_param_.clearance_from_road || - top_left_objects_clearance < constrain_param_.clearance_from_object || - top_right_objects_clearance < constrain_param_.clearance_from_object || - bottom_right_objects_clearance < constrain_param_.clearance_from_object || - bottom_left_objects_clearance < constrain_param_.clearance_from_object) { - object_occupancy_map[x_idx][y_idx] = 1; - } - if ( - top_left_clearance < constrain_param_.clearance_from_road || - top_right_clearance < constrain_param_.clearance_from_road || - bottom_right_clearance < constrain_param_.clearance_from_road || - bottom_left_clearance < constrain_param_.clearance_from_road) { - road_occupancy_map[x_idx][y_idx] = 1; - } - y_idx_in_occupancy_map++; - } - x_idx_in_occupancy_map++; - y_idx_in_occupancy_map = 0; - } - OccupancyMaps occupancy_maps; - occupancy_maps.object_occupancy_map = object_occupancy_map; - occupancy_maps.road_occupancy_map = road_occupancy_map; - return occupancy_maps; -} - int EBPathOptimizer::getStraightLineIdx( const std::vector & interpolated_points, const int farthest_point_idx, - const cv::Mat & only_objects_clearance_map, const nav_msgs::msg::MapMetaData & map_info, std::vector & debug_detected_straight_points) { double prev_yaw = 0; int straight_line_idx = farthest_point_idx; for (int i = farthest_point_idx; i >= 0; i--) { if (i < farthest_point_idx) { - const double yaw = util::getYawFromPoints(interpolated_points[i + 1], interpolated_points[i]); + const double yaw = + tier4_autoware_utils::calcAzimuthAngle(interpolated_points[i], interpolated_points[i + 1]); const double delta_yaw = yaw - prev_yaw; - const double norm_delta_yaw = util::normalizeRadian(delta_yaw); - float clearance_from_object = std::numeric_limits::max(); - const auto image_point = util::transformMapToOptionalImage(interpolated_points[i], map_info); - if (image_point) { - clearance_from_object = only_objects_clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - } - if ( - std::fabs(norm_delta_yaw) > traj_param_.delta_yaw_threshold_for_straight || - clearance_from_object < constrain_param_.clearance_from_object_for_straight) { + const double norm_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + if (std::fabs(norm_delta_yaw) > traj_param_.delta_yaw_threshold_for_straight) { break; } straight_line_idx = i; prev_yaw = yaw; } else if (i == farthest_point_idx && farthest_point_idx >= 1) { - const double yaw = util::getYawFromPoints(interpolated_points[i], interpolated_points[i - 1]); + const double yaw = + tier4_autoware_utils::calcAzimuthAngle(interpolated_points[i - 1], interpolated_points[i]); prev_yaw = yaw; } } @@ -1297,7 +495,7 @@ int EBPathOptimizer::getStraightLineIdx( return straight_line_idx; } -Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( +EBPathOptimizer::Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( const geometry_msgs::msg::Point & interpolated_point, const ConstrainRectangle & constrain_range) { Constrain constrain; @@ -1353,7 +551,7 @@ Constrain EBPathOptimizer::getConstrainFromConstrainRectangle( return constrain; } -ConstrainLines EBPathOptimizer::getConstrainLines( +EBPathOptimizer::ConstrainLines EBPathOptimizer::getConstrainLines( const double dx, const double dy, const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & opposite_point) { @@ -1381,213 +579,35 @@ ConstrainRectangle EBPathOptimizer::getConstrainRectangle( geometry_msgs::msg::Point top_left; top_left.x = clearance; top_left.y = clearance; - constrain_range.top_left = util::transformToAbsoluteCoordinate2D(top_left, anchor.pose); + constrain_range.top_left = geometry_utils::transformToAbsoluteCoordinate2D(top_left, anchor.pose); geometry_msgs::msg::Point top_right; top_right.x = clearance; top_right.y = -1 * clearance; - constrain_range.top_right = util::transformToAbsoluteCoordinate2D(top_right, anchor.pose); + constrain_range.top_right = + geometry_utils::transformToAbsoluteCoordinate2D(top_right, anchor.pose); geometry_msgs::msg::Point bottom_left; bottom_left.x = -1 * clearance; bottom_left.y = clearance; - constrain_range.bottom_left = util::transformToAbsoluteCoordinate2D(bottom_left, anchor.pose); + constrain_range.bottom_left = + geometry_utils::transformToAbsoluteCoordinate2D(bottom_left, anchor.pose); geometry_msgs::msg::Point bottom_right; bottom_right.x = -1 * clearance; bottom_right.y = -1 * clearance; - constrain_range.bottom_right = util::transformToAbsoluteCoordinate2D(bottom_right, anchor.pose); + constrain_range.bottom_right = + geometry_utils::transformToAbsoluteCoordinate2D(bottom_right, anchor.pose); constrain_range.velocity = anchor.velocity; return constrain_range; } -ConstrainRectangle EBPathOptimizer::getUpdatedConstrainRectangle( - const ConstrainRectangle & rectangle, const geometry_msgs::msg::Point & candidate_point, - const nav_msgs::msg::MapMetaData & map_info, const cv::Mat & only_objects_clearance_map) const -{ - auto rect = rectangle; - if (isClose2Object( - candidate_point, map_info, only_objects_clearance_map, - constrain_param_.min_object_clearance_for_deceleration)) { - rect.velocity = std::fmin(rect.velocity, traj_param_.max_avoiding_ego_velocity_ms); - } - if (isClose2Object( - candidate_point, map_info, only_objects_clearance_map, - constrain_param_.min_object_clearance_for_joint)) { - rect.is_including_only_smooth_range = false; - } - return rect; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const std::vector & path_points, - const Anchor & anchor, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const int default_idx = -1; - const auto interpolated_path_points = - util::getInterpolatedPoints(path_points, traj_param_.delta_arc_length_for_trajectory); - const int nearest_idx = util::getNearestIdx( - interpolated_path_points, anchor.pose, default_idx, - traj_param_.delta_yaw_threshold_for_closest_point); - - float clearance = std::numeric_limits::lowest(); - geometry_msgs::msg::Point image_point; - if (util::transformMapToImage(interpolated_path_points[nearest_idx], map_info, image_point)) { - clearance = - clearance_map.ptr(static_cast(image_point.y))[static_cast(image_point.x)] * - map_info.resolution; - } - const double dist = - util::calculate2DDistance(anchor.pose.position, interpolated_path_points[nearest_idx]); - ConstrainRectangle constrain_rectangle; - // idx is valid && anchor is close to nearest path point - if (nearest_idx > default_idx && dist < clearance) { - constrain_rectangle = - getConstrainRectangle(anchor, nearest_idx, interpolated_path_points, clearance_map, map_info); - } else { - constrain_rectangle = getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - } - return constrain_rectangle; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const Anchor & anchor, const int & nearest_idx, - const std::vector & interpolated_points, const cv::Mat & clearance_map, - const nav_msgs::msg::MapMetaData & map_info) const -{ - Anchor replaced_anchor = anchor; - replaced_anchor.pose.position = interpolated_points[nearest_idx]; - if (nearest_idx > 0) { - replaced_anchor.pose.orientation = util::getQuaternionFromPoints( - interpolated_points[nearest_idx], interpolated_points[nearest_idx - 1]); - } else if (nearest_idx == 0 && interpolated_points.size() > 1) { - replaced_anchor.pose.orientation = util::getQuaternionFromPoints( - interpolated_points[nearest_idx + 1], interpolated_points[nearest_idx]); - } - ConstrainRectangles rectangles = - getConstrainRectangles(replaced_anchor, clearance_map, clearance_map, map_info); - const double rel_plus_y = util::calculate2DDistance( - rectangles.road_constrain_rectangle.top_left, replaced_anchor.pose.position); - const double rel_minus_y = util::calculate2DDistance( - rectangles.road_constrain_rectangle.top_right, replaced_anchor.pose.position); - - ConstrainRectangle constrain_rectangle; - geometry_msgs::msg::Point top_left; - top_left.x = constrain_param_.clearance_for_joint; - top_left.y = rel_plus_y; - constrain_rectangle.top_left = - util::transformToAbsoluteCoordinate2D(top_left, replaced_anchor.pose); - geometry_msgs::msg::Point top_right; - top_right.x = constrain_param_.clearance_for_joint; - top_right.y = -1 * rel_minus_y; - constrain_rectangle.top_right = - util::transformToAbsoluteCoordinate2D(top_right, replaced_anchor.pose); - geometry_msgs::msg::Point bottom_left; - bottom_left.x = -1 * constrain_param_.clearance_for_joint; - bottom_left.y = rel_plus_y; - constrain_rectangle.bottom_left = - util::transformToAbsoluteCoordinate2D(bottom_left, replaced_anchor.pose); - geometry_msgs::msg::Point bottom_right; - bottom_right.x = -1 * constrain_param_.clearance_for_joint; - bottom_right.y = -1 * rel_minus_y; - constrain_rectangle.bottom_right = - util::transformToAbsoluteCoordinate2D(bottom_right, replaced_anchor.pose); - constrain_rectangle.velocity = anchor.velocity; - return constrain_rectangle; -} - -ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const std::vector> & occupancy_points, - const util::Rectangle & util_rect, const Anchor & anchor) const -{ - ConstrainRectangle constrain_rectangle; - constrain_rectangle.bottom_left = occupancy_points[util_rect.min_x_idx][util_rect.max_y_idx]; - constrain_rectangle.bottom_right = occupancy_points[util_rect.min_x_idx][util_rect.min_y_idx]; - constrain_rectangle.top_left = occupancy_points[util_rect.max_x_idx][util_rect.max_y_idx]; - constrain_rectangle.top_right = occupancy_points[util_rect.max_x_idx][util_rect.min_y_idx]; - - geometry_msgs::msg::Pose left_pose = anchor.pose; - left_pose.position = constrain_rectangle.top_left; - geometry_msgs::msg::Point top_left; - top_left.x = - std::fmin(keep_space_shape_ptr_->x, constrain_param_.max_lon_space_for_driveable_constraint); - top_left.y = 0; - constrain_rectangle.top_left = util::transformToAbsoluteCoordinate2D(top_left, left_pose); - geometry_msgs::msg::Point bottom_left; - bottom_left.x = 0; - bottom_left.y = 0; - constrain_rectangle.bottom_left = util::transformToAbsoluteCoordinate2D(bottom_left, left_pose); - geometry_msgs::msg::Pose right_pose = anchor.pose; - right_pose.position = constrain_rectangle.top_right; - geometry_msgs::msg::Point top_right; - top_right.x = - std::fmin(keep_space_shape_ptr_->x, constrain_param_.max_lon_space_for_driveable_constraint); - top_right.y = 0; - constrain_rectangle.top_right = util::transformToAbsoluteCoordinate2D(top_right, right_pose); - geometry_msgs::msg::Point bottom_right; - bottom_right.x = 0; - bottom_right.y = 0; - constrain_rectangle.bottom_right = - util::transformToAbsoluteCoordinate2D(bottom_right, right_pose); - constrain_rectangle.velocity = anchor.velocity; - return constrain_rectangle; -} - ConstrainRectangle EBPathOptimizer::getConstrainRectangle( - const std::vector> & occupancy_map, - const std::vector> & occupancy_points, - const Anchor & anchor, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & only_objects_clearance_map) const -{ - util::Rectangle util_rect = util::getLargestRectangle(occupancy_map); - - ConstrainRectangle constrain_rectangle; - if (util_rect.area < epsilon_) { - constrain_rectangle = getConstrainRectangle(anchor, constrain_param_.clearance_for_joint); - constrain_rectangle.is_empty_driveable_area = true; - } else { - constrain_rectangle = getConstrainRectangle(occupancy_points, util_rect, anchor); - } - geometry_msgs::msg::Point max_abs_y = occupancy_points[util_rect.max_x_idx][util_rect.max_y_idx]; - geometry_msgs::msg::Point min_abs_y = occupancy_points[util_rect.max_x_idx][util_rect.min_y_idx]; - geometry_msgs::msg::Point max_rel_y = - util::transformToRelativeCoordinate2D(max_abs_y, anchor.pose); - geometry_msgs::msg::Point min_rel_y = - util::transformToRelativeCoordinate2D(min_abs_y, anchor.pose); - if ( - (max_rel_y.y < -1 * constrain_param_.clearance_for_only_smoothing || - min_rel_y.y > constrain_param_.clearance_for_only_smoothing) && - isClose2Object( - anchor.pose.position, map_info, only_objects_clearance_map, - constrain_param_.clearance_from_object)) { - constrain_rectangle.is_including_only_smooth_range = false; - } - return constrain_rectangle; -} - -bool EBPathOptimizer::isFixingPathPoint( - [[maybe_unused]] const std::vector & path_points) - const -{ - return false; -} - -FOAData EBPathOptimizer::getFOAData( - const std::vector & rectangles, - const std::vector & interpolated_points, const int farthest_idx) -{ - FOAData foa_data; - for (const auto & rect : rectangles) { - if (rect.is_empty_driveable_area) { - foa_data.is_avoidance_possible = false; - } - } - if (!foa_data.is_avoidance_possible) { - RCLCPP_WARN( - rclcpp::get_logger("EBPathOptimizer"), - "[ObstacleAvoidancePlanner] Fail to make new trajectory from empty drivable area"); - } - - foa_data.constrain_rectangles = rectangles; - foa_data.avoiding_traj_points = calculateTrajectory( - interpolated_points, foa_data.constrain_rectangles, farthest_idx, OptMode::Visualizing); - return foa_data; + const Anchor & anchor, const double min_x, const double max_x, const double min_y, + const double max_y) const +{ + ConstrainRectangle rect; + rect.top_left = tier4_autoware_utils::calcOffsetPose(anchor.pose, max_x, max_y, 0.0).position; + rect.top_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, max_x, min_y, 0.0).position; + rect.bottom_left = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, max_y, 0.0).position; + rect.bottom_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, min_y, 0.0).position; + rect.velocity = anchor.velocity; + return rect; } diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 28620c3ce5f51..4427cbaf2e437 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -14,879 +14,875 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "obstacle_avoidance_planner/utils.hpp" #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "tf2/utils.h" -#include -#include +#include "nav_msgs/msg/map_meta_data.hpp" -#include - -#include - -#include +#include "boost/optional.hpp" #include #include #include #include +#include #include +namespace +{ +geometry_msgs::msg::Pose convertRefPointsToPose(const ReferencePoint & ref_point) +{ + geometry_msgs::msg::Pose pose; + pose.position = ref_point.p; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); + return pose; +} + +std::tuple extractBounds( + const std::vector & ref_points, const size_t l_idx) +{ + Eigen::VectorXd ub_vec(ref_points.size()); + Eigen::VectorXd lb_vec(ref_points.size()); + for (size_t i = 0; i < ref_points.size(); ++i) { + ub_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).upper_bound; + lb_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).lower_bound; + } + return {ub_vec, lb_vec}; +} + +Bounds findWidestBounds(const BoundsCandidates & front_bounds_candidates) +{ + double max_width = std::numeric_limits::min(); + size_t max_width_index = 0; + if (front_bounds_candidates.size() != 1) { + for (size_t candidate_idx = 0; candidate_idx < front_bounds_candidates.size(); + ++candidate_idx) { + const auto & front_bounds_candidate = front_bounds_candidates.at(candidate_idx); + const double bound_width = + front_bounds_candidate.upper_bound - front_bounds_candidate.lower_bound; + if (max_width < bound_width) { + max_width_index = candidate_idx; + max_width = bound_width; + } + } + } + return front_bounds_candidates.at(max_width_index); +} + +double calcOverlappedBounds( + const geometry_msgs::msg::Pose & front_point, const Bounds & front_bounds_candidate, + const geometry_msgs::msg::Pose & prev_front_point, const Bounds & prev_front_continuous_bounds) +{ + const double avoiding_yaw = + tier4_autoware_utils::normalizeRadian(tf2::getYaw(front_point.orientation) + M_PI_2); + + geometry_msgs::msg::Point ub_pos; + ub_pos.x = front_point.position.x + front_bounds_candidate.upper_bound * std::cos(avoiding_yaw); + ub_pos.y = front_point.position.y + front_bounds_candidate.upper_bound * std::sin(avoiding_yaw); + + geometry_msgs::msg::Point lb_pos; + lb_pos.x = front_point.position.x + front_bounds_candidate.lower_bound * std::cos(avoiding_yaw); + lb_pos.y = front_point.position.y + front_bounds_candidate.lower_bound * std::sin(avoiding_yaw); + + const double projected_ub_y = + geometry_utils::transformToRelativeCoordinate2D(ub_pos, prev_front_point).y; + const double projected_lb_y = + geometry_utils::transformToRelativeCoordinate2D(lb_pos, prev_front_point).y; + + const double min_ub = std::min(projected_ub_y, prev_front_continuous_bounds.upper_bound); + const double max_lb = std::max(projected_lb_y, prev_front_continuous_bounds.lower_bound); + + const double overlapped_length = min_ub - max_lb; + return overlapped_length; +} + +geometry_msgs::msg::Pose calcVehiclePose( + const ReferencePoint & ref_point, const double lat_error, const double yaw_error, + const double offset) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = ref_point.p.x - std::sin(ref_point.yaw) * lat_error - + std::cos(ref_point.yaw + yaw_error) * offset; + pose.position.y = ref_point.p.y + std::cos(ref_point.yaw) * lat_error - + std::sin(ref_point.yaw + yaw_error) * offset; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + yaw_error); + + return pose; +} + +template +void trimPoints(std::vector & points) +{ + std::vector trimmed_points; + constexpr double epsilon = 1e-6; + + auto itr = points.begin(); + while (itr != points.end() - 1) { + bool is_overlapping = false; + if (itr != points.begin()) { + const auto & p_front = tier4_autoware_utils::getPoint(*itr); + const auto & p_back = tier4_autoware_utils::getPoint(*(itr + 1)); + + const double dx = p_front.x - p_back.x; + const double dy = p_front.y - p_back.y; + if (dx * dx + dy * dy < epsilon) { + is_overlapping = true; + } + } + if (is_overlapping) { + itr = points.erase(itr); + } else { + ++itr; + } + } +} + +std::vector eigenVectorToStdVector(const Eigen::VectorXd & eigen_vec) +{ + return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()}; +} + +double calcLateralError( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose ref_pose) +{ + const double err_x = target_point.x - ref_pose.position.x; + const double err_y = target_point.y - ref_pose.position.y; + const double ref_yaw = tf2::getYaw(ref_pose.orientation); + const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; + return lat_err; +} + +Eigen::Vector2d getState( + const geometry_msgs::msg::Pose & target_pose, const geometry_msgs::msg::Pose & ref_pose) +{ + const double lat_error = calcLateralError(target_pose.position, ref_pose); + const double yaw_error = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(target_pose.orientation) - tf2::getYaw(ref_pose.orientation)); + Eigen::VectorXd kinematics = Eigen::VectorXd::Zero(2); + kinematics << lat_error, yaw_error; + return kinematics; +} + +std::vector slerpYawFromReferencePoints(const std::vector & ref_points) +{ + if (ref_points.size() == 1) { + return {ref_points.front().yaw}; + } + + std::vector points; + for (const auto & ref_point : ref_points) { + points.push_back(ref_point.p); + } + return interpolation::slerpYawFromPoints(points); +} +} // namespace + MPTOptimizer::MPTOptimizer( - const bool is_showing_debug_info, const QPParam & qp_param, const TrajectoryParam & traj_param, - const ConstrainParam & constraint_param, const VehicleParam & vehicle_param, - const MPTParam & mpt_param) -: is_showing_debug_info_(is_showing_debug_info) + const bool is_showing_debug_info, const TrajectoryParam & traj_param, + const VehicleParam & vehicle_param, const MPTParam & mpt_param) +: is_showing_debug_info_(is_showing_debug_info), + traj_param_(traj_param), + vehicle_param_(vehicle_param), + mpt_param_(mpt_param), + vehicle_model_ptr_( + std::make_unique(vehicle_param_.wheelbase, mpt_param_.max_steer_rad)), + osqp_solver_ptr_(std::make_unique(osqp_epsilon_)) { - qp_param_ptr_ = std::make_unique(qp_param); - traj_param_ptr_ = std::make_unique(traj_param); - constraint_param_ptr_ = std::make_unique(constraint_param); - vehicle_param_ptr_ = std::make_unique(vehicle_param); - mpt_param_ptr_ = std::make_unique(mpt_param); - - vehicle_model_ptr_ = std::make_unique( - vehicle_param_ptr_->wheelbase, vehicle_param_ptr_->max_steer_rad, - vehicle_param_ptr_->steer_tau); } -boost::optional MPTOptimizer::getModelPredictiveTrajectory( +boost::optional MPTOptimizer::getModelPredictiveTrajectory( const bool enable_avoidance, const std::vector & smoothed_points, const std::vector & path_points, const std::unique_ptr & prev_trajs, const CVMaps & maps, - const geometry_msgs::msg::Pose & ego_pose, DebugData * debug_data) + const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel, + std::shared_ptr debug_data_ptr) { - auto t_start1 = std::chrono::high_resolution_clock::now(); + stop_watch_.tic(__func__); + + current_ego_pose_ = current_ego_pose; + current_ego_vel_ = current_ego_vel; + if (smoothed_points.empty()) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "return boost::none since smoothed_points is empty"); return boost::none; } - geometry_msgs::msg::Pose origin_pose = smoothed_points.front().pose; - if (prev_trajs) { - const int prev_nearest_idx = util::getNearestIdx( - prev_trajs->model_predictive_trajectory, smoothed_points.front().pose.position); - origin_pose = prev_trajs->model_predictive_trajectory.at(prev_nearest_idx).pose; - } - std::vector ref_points = getReferencePoints( - origin_pose, ego_pose, smoothed_points, path_points, prev_trajs, maps, debug_data); - if (ref_points.empty()) { + std::vector full_ref_points = + getReferencePoints(smoothed_points, prev_trajs, enable_avoidance, maps, debug_data_ptr); + if (full_ref_points.empty()) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "return boost::none since ref_points is empty"); return boost::none; - } - - const auto mpt_matrix = generateMPTMatrix(ref_points, path_points, prev_trajs); - if (!mpt_matrix) { + } else if (full_ref_points.size() == 1) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, - "return boost::none since matrix has nan"); + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, + "return boost::none since ref_points.size() == 1"); return boost::none; } + debug_data_ptr->mpt_fixed_traj = getMPTFixedPoints(full_ref_points); + + std::vector fixed_ref_points; + std::vector non_fixed_ref_points; + bool is_fixing_ref_points = true; + for (size_t i = 0; i < full_ref_points.size(); ++i) { + if (i == full_ref_points.size() - 1) { + if (!full_ref_points.at(i).fix_kinematic_state) { + is_fixing_ref_points = false; + } + } else if ( + // fix first three points + full_ref_points.at(i).fix_kinematic_state && full_ref_points.at(i + 1).fix_kinematic_state && + (i + 2 < full_ref_points.size() && full_ref_points.at(i + 2).fix_kinematic_state) && + (i + 3 < full_ref_points.size() && full_ref_points.at(i + 3).fix_kinematic_state)) { + } else { + is_fixing_ref_points = false; + } + + if (is_fixing_ref_points) { + fixed_ref_points.push_back(full_ref_points.at(i)); + } else { + non_fixed_ref_points.push_back(full_ref_points.at(i)); + } + } + + // calculate B and W matrices + const auto mpt_matrix = generateMPTMatrix(non_fixed_ref_points, debug_data_ptr); + + // calculate Q and R matrices + const auto val_matrix = generateValueMatrix(non_fixed_ref_points, path_points, debug_data_ptr); + const auto optimized_control_variables = executeOptimization( - enable_avoidance, mpt_matrix.get(), ref_points, path_points, maps, debug_data); + prev_trajs, enable_avoidance, mpt_matrix, val_matrix, non_fixed_ref_points, debug_data_ptr); if (!optimized_control_variables) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "return boost::none since could not solve qp"); return boost::none; } - const auto mpt_points = - getMPTPoints(ref_points, optimized_control_variables.get(), mpt_matrix.get(), smoothed_points); + const auto mpt_points = getMPTPoints( + fixed_ref_points, non_fixed_ref_points, optimized_control_variables.get(), mpt_matrix, + debug_data_ptr); + + auto full_optimized_ref_points = fixed_ref_points; + full_optimized_ref_points.insert( + full_optimized_ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, "MPT time: = %f [ms]", elapsed_ms1); MPTTrajs mpt_trajs; mpt_trajs.mpt = mpt_points; - mpt_trajs.ref_points = ref_points; + mpt_trajs.ref_points = full_optimized_ref_points; return mpt_trajs; } std::vector MPTOptimizer::getReferencePoints( - const geometry_msgs::msg::Pose & origin_pose, const geometry_msgs::msg::Pose & ego_pose, - const std::vector & points, - const std::vector & path_points, - const std::unique_ptr & prev_trajs, const CVMaps & maps, - DebugData * debug_data) const + const std::vector & smoothed_points, + const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const CVMaps & maps, std::shared_ptr debug_data_ptr) const { - const auto ref_points = - convertToReferencePoints(points, path_points, prev_trajs, ego_pose, maps, debug_data); - - const int begin_idx = util::getNearestPointIdx(ref_points, origin_pose.position); - const auto first_it = ref_points.begin() + begin_idx; - int num_points = std::min( - static_cast(ref_points.size()) - 1 - begin_idx, traj_param_ptr_->num_sampling_points); - num_points = std::max(num_points, 0); - std::vector truncated_points(first_it, first_it + num_points); - calcInitialState(&truncated_points, origin_pose); - return truncated_points; -} + stop_watch_.tic(__func__); + + const auto ref_points = [&]() { + auto ref_points = [&]() { + // TODO(murooka) consider better algorithm to calculate fixed/non-fixed reference points + const auto fixed_ref_points = getFixedReferencePoints(smoothed_points, prev_trajs); + + // if no fixed_ref_points + if (fixed_ref_points.empty()) { + // interpolate and crop backward + const auto interpolated_points = interpolation_utils::getInterpolatedPoints( + smoothed_points, mpt_param_.delta_arc_length_for_mpt_points); + const auto cropped_interpolated_points = points_utils::clipBackwardPoints( + interpolated_points, current_ego_pose_.position, traj_param_.backward_fixing_distance, + mpt_param_.delta_arc_length_for_mpt_points); + + return points_utils::convertToReferencePoints(cropped_interpolated_points); + } -std::vector MPTOptimizer::convertToReferencePoints( - const std::vector & points, - [[maybe_unused]] const std::vector & path_points, - const std::unique_ptr & prev_trajs, - [[maybe_unused]] const geometry_msgs::msg::Pose & ego_pose, [[maybe_unused]] const CVMaps & maps, - DebugData * debug_data) const -{ - const auto interpolated_points = - util::getInterpolatedPoints(points, traj_param_ptr_->delta_arc_length_for_mpt_points); - if (interpolated_points.empty()) { + // calc non fixed traj points + const size_t seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(smoothed_points, fixed_ref_points.back().p); + const auto non_fixed_traj_points = + std::vector{ + smoothed_points.begin() + seg_idx, smoothed_points.end()}; + + const double offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + non_fixed_traj_points, 0, fixed_ref_points.back().p) + + mpt_param_.delta_arc_length_for_mpt_points; + const auto non_fixed_interpolated_traj_points = interpolation_utils::getInterpolatedPoints( + non_fixed_traj_points, mpt_param_.delta_arc_length_for_mpt_points, offset); + const auto non_fixed_ref_points = + points_utils::convertToReferencePoints(non_fixed_interpolated_traj_points); + + // make ref points + auto ref_points = fixed_ref_points; + ref_points.insert(ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); + + return ref_points; + }(); + + if (ref_points.empty()) { + return std::vector{}; + } + + // set some information to reference points considering fix kinematics + trimPoints(ref_points); + calcOrientation(ref_points); + calcVelocity(ref_points, smoothed_points); + calcCurvature(ref_points); + calcArcLength(ref_points); + calcPlanningFromEgo( + ref_points); // NOTE: fix_kinematic_state will be updated when planning from ego + + // crop trajectory with margin to calculate vehicle bounds at the end point + constexpr double tmp_ref_points_margin = 20.0; + const double ref_length_with_margin = + traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points + + tmp_ref_points_margin; + ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length_with_margin); + + // set bounds information + calcBounds(ref_points, enable_avoidance, maps, debug_data_ptr); + calcVehicleBounds(ref_points, maps, debug_data_ptr, enable_avoidance); + + // set extra information (alpha and has_object_collision) + // NOTE: This must be after bounds calculation. + calcExtraPoints(ref_points, prev_trajs); + + const double ref_length = + traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; + ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length); + + // bounds information is assigned to debug data after truncating reference points + debug_data_ptr->ref_points = ref_points; + + return ref_points; + }(); + if (ref_points.empty()) { return std::vector{}; } - auto reference_points = getBaseReferencePoints(interpolated_points, prev_trajs, debug_data); - - calcOrientation(&reference_points); - calcVelocity(&reference_points, points); - calcCurvature(&reference_points); - calcArcLength(&reference_points); - calcExtraPoints(&reference_points); - return reference_points; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return ref_points; } -void MPTOptimizer::calcOrientation(std::vector * ref_points) const +void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) const { - if (!ref_points) { - return; - } - for (std::size_t i = 0; i < ref_points->size(); i++) { - if (ref_points->at(i).fix_state) { - continue; + // if plan from ego + constexpr double epsilon = 1e-04; + const bool plan_from_ego = + mpt_param_.plan_from_ego && std::abs(current_ego_vel_) < epsilon && ref_points.size() > 1; + if (plan_from_ego) { + for (auto & ref_point : ref_points) { + ref_point.fix_kinematic_state = boost::none; } - if (i > 0) { - ref_points->at(i).q = - util::getQuaternionFromPoints(ref_points->at(i).p, ref_points->at(i - 1).p); - } else if (i == 0 && ref_points->size() > 1) { - ref_points->at(i).q = - util::getQuaternionFromPoints(ref_points->at(i + 1).p, ref_points->at(i).p); - } - ref_points->at(i).yaw = tf2::getYaw(ref_points->at(i).q); + /* + // interpolate and crop backward + const auto interpolated_points = interpolation_utils::getInterpolatedPoints( + points, + mpt_param_.delta_arc_length_for_mpt_points); const auto cropped_interpolated_points = + points_utils::clipBackwardPoints( interpolated_points, current_pose_.position, + traj_param_.backward_fixing_distance, mpt_param_.delta_arc_length_for_mpt_points); + + auto cropped_ref_points = + points_utils::convertToReferencePoints(cropped_interpolated_points); + */ + + // assign fix kinematics + const size_t nearest_ref_idx = + tier4_autoware_utils::findNearestIndex(ref_points, current_ego_pose_.position); + + // calculate cropped_ref_points.at(nearest_ref_idx) with yaw + const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose { + geometry_msgs::msg::Pose pose; + pose.position = ref_points.at(nearest_ref_idx).p; + + const size_t prev_nearest_ref_idx = + (nearest_ref_idx == ref_points.size() - 1) ? nearest_ref_idx - 1 : nearest_ref_idx; + pose.orientation = geometry_utils::getQuaternionFromPoints( + ref_points.at(prev_nearest_ref_idx + 1).p, ref_points.at(prev_nearest_ref_idx).p); + return pose; + }(); + + ref_points.at(nearest_ref_idx).fix_kinematic_state = + getState(current_ego_pose_, nearest_ref_pose); + ref_points.at(nearest_ref_idx).plan_from_ego = true; } } -void MPTOptimizer::calcVelocity( - std::vector * ref_points, - const std::vector & points) const +std::vector MPTOptimizer::getFixedReferencePoints( + const std::vector & points, + const std::unique_ptr & prev_trajs) const { - if (!ref_points) { - return; - } - for (std::size_t i = 0; i < ref_points->size(); i++) { - ref_points->at(i).v = - points[util::getNearestIdx(points, ref_points->at(i).p)].longitudinal_velocity_mps; + if ( + !prev_trajs || prev_trajs->model_predictive_trajectory.empty() || + prev_trajs->mpt_ref_points.empty() || + prev_trajs->model_predictive_trajectory.size() != prev_trajs->mpt_ref_points.size()) { + return std::vector(); } -} -void MPTOptimizer::calcCurvature(std::vector * ref_points) const -{ - if (!ref_points) { - return; + if (!mpt_param_.fix_points_around_ego) { + return std::vector(); } - int num_points = static_cast(ref_points->size()); - - /* calculate curvature by circle fitting from three points */ - geometry_msgs::msg::Point p1, p2, p3; - int max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); - int L = std::min(mpt_param_ptr_->num_curvature_sampling_points, max_smoothing_num); - for (int i = L; i < num_points - L; ++i) { - p1 = ref_points->at(i - L).p; - p2 = ref_points->at(i).p; - p3 = ref_points->at(i + L).p; - double den = std::max( - util::calculate2DDistance(p1, p2) * util::calculate2DDistance(p2, p3) * - util::calculate2DDistance(p3, p1), - 0.0001); - const double curvature = - 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; - if (!ref_points->at(i).fix_state) { - ref_points->at(i).k = curvature; + const auto & prev_ref_points = prev_trajs->mpt_ref_points; + const int nearest_prev_ref_idx = + tier4_autoware_utils::findNearestIndex(prev_ref_points, current_ego_pose_.position); + + // calculate begin_prev_ref_idx + const int begin_prev_ref_idx = [&]() { + const int backward_fixing_num = + traj_param_.backward_fixing_distance / mpt_param_.delta_arc_length_for_mpt_points; + + return std::max(0, nearest_prev_ref_idx - backward_fixing_num); + }(); + + // calculate end_prev_ref_idx + const int end_prev_ref_idx = [&]() { + const double forward_fixed_length = std::max( + current_ego_vel_ * traj_param_.forward_fixing_min_time, + traj_param_.forward_fixing_min_distance); + + const int forward_fixing_num = + forward_fixed_length / mpt_param_.delta_arc_length_for_mpt_points; + return std::min( + static_cast(prev_ref_points.size()) - 1, nearest_prev_ref_idx + forward_fixing_num); + }(); + + bool points_reaching_prev_points_flag = false; + std::vector fixed_ref_points; + for (size_t i = begin_prev_ref_idx; i <= static_cast(end_prev_ref_idx); ++i) { + const auto & prev_ref_point = prev_ref_points.at(i); + + if (!points_reaching_prev_points_flag) { + if (tier4_autoware_utils::calcSignedArcLength(points, 0, prev_ref_point.p) < 0) { + continue; + } + points_reaching_prev_points_flag = true; } - } - /* first and last curvature is copied from next value */ - for (int i = 0; i < std::min(L, num_points); ++i) { - if (!ref_points->at(i).fix_state) { - ref_points->at(i).k = ref_points->at(std::min(L, num_points - 1)).k; - } - if (!ref_points->at(num_points - i - 1).fix_state) { - ref_points->at(num_points - i - 1).k = ref_points->at(std::max(num_points - L - 1, 0)).k; - } + ReferencePoint fixed_ref_point; + fixed_ref_point = prev_ref_point; + fixed_ref_point.fix_kinematic_state = prev_ref_point.optimized_kinematic_state; + + fixed_ref_points.push_back(fixed_ref_point); } + + return fixed_ref_points; } -void MPTOptimizer::calcArcLength(std::vector * ref_points) const +std::vector MPTOptimizer::getMPTFixedPoints( + const std::vector & ref_points) const { - if (!ref_points) { - return; - } - for (std::size_t i = 0; i < ref_points->size(); i++) { - if (i > 0) { - geometry_msgs::msg::Point a, b; - a = ref_points->at(i).p; - b = ref_points->at(i - 1).p; - ref_points->at(i).s = ref_points->at(i - 1).s + util::calculate2DDistance(a, b); + std::vector mpt_fixed_traj; + for (size_t i = 0; i < ref_points.size(); ++i) { + const auto & ref_point = ref_points.at(i); + + if (ref_point.fix_kinematic_state) { + const double lat_error = ref_point.fix_kinematic_state.get()(0); + const double yaw_error = ref_point.fix_kinematic_state.get()(1); + + autoware_auto_planning_msgs::msg::TrajectoryPoint fixed_traj_point; + fixed_traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0); + mpt_fixed_traj.push_back(fixed_traj_point); } } + + return mpt_fixed_traj; } -void MPTOptimizer::calcExtraPoints(std::vector * ref_points) const +// predict equation: x = Bex u + Wex (u includes x_0) +// cost function: J = x' Qex x + u' Rex u +MPTOptimizer::MPTMatrix MPTOptimizer::generateMPTMatrix( + const std::vector & ref_points, std::shared_ptr debug_data_ptr) const { - if (!ref_points) { - return; + if (ref_points.empty()) { + return MPTMatrix{}; } - for (std::size_t i = 0; i < ref_points->size(); i++) { - const double p1_target_s = ref_points->at(i).s + mpt_param_ptr_->top_point_dist_from_base_link; - const int nearest_p1_idx = util::getNearestIdx(*ref_points, p1_target_s, i); - ref_points->at(i).top_pose.position = ref_points->at(nearest_p1_idx).p; - - const double p2_target_s = ref_points->at(i).s + mpt_param_ptr_->mid_point_dist_from_base_link; - const int nearest_p2_idx = util::getNearestIdx(*ref_points, p2_target_s, i); - ref_points->at(i).mid_pose.position = ref_points->at(nearest_p2_idx).p; - - ref_points->at(i).top_pose.orientation = - util::getQuaternionFromPoints(ref_points->at(i).top_pose.position, ref_points->at(i).p); - if (static_cast(i) == nearest_p1_idx && i > 0) { - ref_points->at(i).top_pose.orientation = - util::getQuaternionFromPoints(ref_points->at(i).p, ref_points->at(i - 1).p); - } else if (static_cast(i) == nearest_p1_idx) { - ref_points->at(i).top_pose.orientation = ref_points->at(i).q; - } - ref_points->at(i).mid_pose.orientation = ref_points->at(i).top_pose.orientation; - const double delta_yaw = - tf2::getYaw(ref_points->at(i).top_pose.orientation) - ref_points->at(i).yaw; - const double norm_delta_yaw = util::normalizeRadian(delta_yaw); - ref_points->at(i).delta_yaw_from_p1 = norm_delta_yaw; - ref_points->at(i).delta_yaw_from_p2 = ref_points->at(i).delta_yaw_from_p1; - } -} + stop_watch_.tic(__func__); -void MPTOptimizer::calcInitialState( - std::vector * ref_points, const geometry_msgs::msg::Pose & origin_pose) const -{ - if (!ref_points) { - return; - } + // NOTE: center offset of vehicle is always 0 in this algorithm. + // vehicle_model_ptr_->updateCenterOffset(0.0); - if (ref_points->empty()) { - return; - } + const size_t N_ref = ref_points.size(); + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t D_v = D_x + D_u * (N_ref - 1); - boost::optional begin_idx = boost::none; - double accum_s = 0; + Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); + Eigen::VectorXd Wex = Eigen::VectorXd::Zero(D_x * N_ref); - for (std::size_t i = 0; i < ref_points->size(); i++) { - double ds = 0.0; - if (i < ref_points->size() - 1) { - ds = ref_points->at(i + 1).s - ref_points->at(i).s; - } else if (i == ref_points->size() - 1 && ref_points->size() > 1) { - ds = ref_points->at(i).s - ref_points->at(i - 1).s; - } - accum_s += ds; - constexpr double max_s_for_prev_point = 3; - if (accum_s < max_s_for_prev_point && ref_points->at(i).fix_state) { - begin_idx = i; - break; + Eigen::MatrixXd Ad(D_x, D_x); + Eigen::MatrixXd Bd(D_x, D_u); + Eigen::MatrixXd Wd(D_x, 1); + + // predict kinematics for N_ref times + for (size_t i = 0; i < N_ref; ++i) { + if (i == 0) { + Bex.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); + continue; } - } - Eigen::VectorXd initial_state; - if (begin_idx) { - *ref_points = - std::vector{ref_points->begin() + begin_idx.get(), ref_points->end()}; - ref_points->front().optimized_state = ref_points->front().fix_state.get(); - } else { - ref_points->front().optimized_state = getState(origin_pose, ref_points->front()); - } -} + const int idx_x_i = i * D_x; + const int idx_x_i_prev = (i - 1) * D_x; + const int idx_u_i_prev = (i - 1) * D_u; -/* - * predict equation: Xec = Aex * x0 + Bex * Uex + Wex - * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex - * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) - */ -boost::optional MPTOptimizer::generateMPTMatrix( - const std::vector & reference_points, - const std::vector & path_points, - const std::unique_ptr & prev_trajs) const -{ - const int N = reference_points.size(); - const int DIM_X = vehicle_model_ptr_->getDimX(); - const int DIM_U = vehicle_model_ptr_->getDimU(); - const int DIM_Y = vehicle_model_ptr_->getDimY(); - - Eigen::MatrixXd Aex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_X); // state transition - Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_U * N); // control input - Eigen::MatrixXd Wex = Eigen::MatrixXd::Zero(DIM_X * N, 1); - Eigen::MatrixXd Cex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_X * N); - Eigen::MatrixXd Qex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_Y * N); - Eigen::MatrixXd R1ex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N); - Eigen::MatrixXd R2ex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N); - Eigen::MatrixXd Uref_ex = Eigen::MatrixXd::Zero(DIM_U * N, 1); - - /* weight matrix depends on the vehicle model */ - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_U, DIM_U); - Eigen::MatrixXd Q_adaptive = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - Eigen::MatrixXd R_adaptive = Eigen::MatrixXd::Zero(DIM_U, DIM_U); - Q(0, 0) = mpt_param_ptr_->lat_error_weight; - if (!prev_trajs) { - const double initial_lat_error_weight = mpt_param_ptr_->lat_error_weight * 1000; - Q(0, 0) = initial_lat_error_weight; - } - Q(1, 1) = mpt_param_ptr_->yaw_error_weight; - R(0, 0) = mpt_param_ptr_->steer_input_weight; - - Eigen::MatrixXd Ad(DIM_X, DIM_X); - Eigen::MatrixXd Bd(DIM_X, DIM_U); - Eigen::MatrixXd Wd(DIM_X, 1); - Eigen::MatrixXd Cd(DIM_Y, DIM_X); - Eigen::MatrixXd Uref(DIM_U, 1); - - geometry_msgs::msg::Pose last_ref_pose; - last_ref_pose.position = reference_points.back().p; - last_ref_pose.orientation = reference_points.back().q; - const auto last_extended_point = util::getLastExtendedPoint( - path_points.back(), last_ref_pose, traj_param_ptr_->delta_yaw_threshold_for_closest_point, - traj_param_ptr_->max_dist_for_extending_end_point); - - /* predict dynamics for N times */ - for (int i = 0; i < N; ++i) { - const double ref_k = reference_points[i].k; - double ds = 0.0; - if (i < N - 1) { - ds = reference_points[i + 1].s - reference_points[i].s; - } else if (i == N - 1 && N > 1) { - ds = reference_points[i].s - reference_points[i - 1].s; - } + const double ds = [&]() { + if (N_ref == 1) { + return 0.0; + } + const size_t prev_idx = (i < N_ref - 1) ? i + 1 : i; + return ref_points.at(prev_idx).s - ref_points.at(prev_idx - 1).s; + }(); - /* get discrete state matrix A, B, C, W */ + // get discrete kinematics matrix A, B, W + const double ref_k = ref_points.at(std::max(0, static_cast(i) - 1)).k; vehicle_model_ptr_->setCurvature(ref_k); - vehicle_model_ptr_->calculateDiscreteMatrix(&Ad, &Bd, &Cd, &Wd, ds); - - Q_adaptive = Q; - R_adaptive = R; - if (i == N - 1 && last_extended_point) { - Q_adaptive(0, 0) = mpt_param_ptr_->terminal_path_lat_error_weight; - Q_adaptive(1, 1) = mpt_param_ptr_->terminal_path_yaw_error_weight; - } else if (i == N - 1) { - Q_adaptive(0, 0) = mpt_param_ptr_->terminal_lat_error_weight; - Q_adaptive(1, 1) = mpt_param_ptr_->terminal_yaw_error_weight; - } + vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, ds); - /* update mpt matrix */ - int idx_x_i = i * DIM_X; - int idx_x_i_prev = (i - 1) * DIM_X; - int idx_u_i = i * DIM_U; - int idx_y_i = i * DIM_Y; - if (i == 0) { - Aex.block(0, 0, DIM_X, DIM_X) = Ad; - Bex.block(0, 0, DIM_X, DIM_U) = Bd; - Wex.block(0, 0, DIM_X, 1) = Wd; - } else { - Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X); - for (int j = 0; j < i; ++j) { - int idx_u_j = j * DIM_U; - Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) = - Ad * Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U); - } - Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd; - } - Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd; - Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd; - Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive; - R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive; - - /* get reference input (feed-forward) */ - vehicle_model_ptr_->calculateReferenceInput(&Uref); - if (std::fabs(Uref(0, 0)) < mpt_param_ptr_->zero_ff_steer_angle * M_PI / 180.0) { - Uref(0, 0) = 0.0; // ignore curvature noise + Bex.block(idx_x_i, 0, D_x, D_x) = Ad * Bex.block(idx_x_i_prev, 0, D_x, D_x); + Bex.block(idx_x_i, D_x + idx_u_i_prev, D_x, D_u) = Bd; + + for (size_t j = 0; j < i - 1; ++j) { + size_t idx_u_j = j * D_u; + Bex.block(idx_x_i, D_x + idx_u_j, D_x, D_u) = + Ad * Bex.block(idx_x_i_prev, D_x + idx_u_j, D_x, D_u); } - Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; - } - addSteerWeightR(&R1ex, reference_points); + Wex.segment(idx_x_i, D_x) = Ad * Wex.block(idx_x_i_prev, 0, D_x, 1) + Wd; + } MPTMatrix m; - m.Aex = Aex; m.Bex = Bex; m.Wex = Wex; - m.Cex = Cex; - m.Qex = Qex; - m.R1ex = R1ex; - m.R2ex = R2ex; - m.Uref_ex = Uref_ex; - if ( - m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() || - m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() || - m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any()) { - RCLCPP_WARN(rclcpp::get_logger("MPTOptimizer"), "[Avoidance] MPT matrix includes NaN."); - return boost::none; - } + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return m; } -void MPTOptimizer::addSteerWeightR( - Eigen::MatrixXd * R, const std::vector & ref_points) const +MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( + const std::vector & ref_points, + const std::vector & path_points, + std::shared_ptr debug_data_ptr) const { - const int N = ref_points.size(); - constexpr double DT = 0.1; - constexpr double ctrl_period = 0.03; - - /* add steering rate : weight for (u(i) - u(i-1) / dt )^2 */ - for (int i = 0; i < N - 1; ++i) { - const double steer_rate_r = mpt_param_ptr_->steer_rate_weight / (DT * DT); - (*R)(i + 0, i + 0) += steer_rate_r; - (*R)(i + 1, i + 0) -= steer_rate_r; - (*R)(i + 0, i + 1) -= steer_rate_r; - (*R)(i + 1, i + 1) += steer_rate_r; - } - if (N > 1) { - // steer rate i = 0 - (*R)(0, 0) += mpt_param_ptr_->steer_rate_weight / (ctrl_period * ctrl_period); + if (ref_points.empty()) { + return ValueMatrix{}; } - /* add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2 */ - const double steer_acc_r = mpt_param_ptr_->steer_acc_weight / std::pow(DT, 4); - const double steer_acc_r_cp1 = mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 3) * ctrl_period); - const double steer_acc_r_cp2 = - mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 2) * std::pow(ctrl_period, 2)); - const double steer_acc_r_cp4 = mpt_param_ptr_->steer_acc_weight / std::pow(ctrl_period, 4); - for (int i = 1; i < N - 1; ++i) { - (*R)(i - 1, i - 1) += (steer_acc_r); - (*R)(i - 1, i + 0) += (steer_acc_r * -2.0); - (*R)(i - 1, i + 1) += (steer_acc_r); - (*R)(i + 0, i - 1) += (steer_acc_r * -2.0); - (*R)(i + 0, i + 0) += (steer_acc_r * 4.0); - (*R)(i + 0, i + 1) += (steer_acc_r * -2.0); - (*R)(i + 1, i - 1) += (steer_acc_r); - (*R)(i + 1, i + 0) += (steer_acc_r * -2.0); - (*R)(i + 1, i + 1) += (steer_acc_r); - } - if (N > 1) { - // steer acc i = 1 - (*R)(0, 0) += steer_acc_r * 1.0 + steer_acc_r_cp2 * 1.0 + steer_acc_r_cp1 * 2.0; - (*R)(1, 0) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0; - (*R)(0, 1) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0; - (*R)(1, 1) += steer_acc_r * 1.0; - // steer acc i = 0 - (*R)(0, 0) += steer_acc_r_cp4 * 1.0; - } -} + stop_watch_.tic(__func__); + + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t N_ref = ref_points.size(); + + const size_t D_v = D_x + (N_ref - 1) * D_u; + + const bool is_containing_path_terminal_point = points_utils::isNearLastPathPoint( + ref_points.back(), path_points, traj_param_.max_dist_for_extending_end_point, + traj_param_.delta_yaw_threshold_for_closest_point); + + // update Q + Eigen::SparseMatrix Qex_sparse_mat(D_x * N_ref, D_x * N_ref); + std::vector> Qex_triplet_vec; + for (size_t i = 0; i < N_ref; ++i) { + // this is for plan_from_ego + const bool near_kinematic_state_while_planning_from_ego = [&]() { + const size_t min_idx = static_cast(std::max(0, static_cast(i) - 20)); + const size_t max_idx = std::min(ref_points.size() - 1, i + 20); + for (size_t j = min_idx; j <= max_idx; ++j) { + if (ref_points.at(j).plan_from_ego && ref_points.at(j).fix_kinematic_state) { + return true; + } + } + return false; + }(); + + const auto adaptive_error_weight = [&]() -> std::array { + if (near_kinematic_state_while_planning_from_ego) { + constexpr double obstacle_avoid_error_weight_ratio = 1 / 100.0; + return { + mpt_param_.obstacle_avoid_lat_error_weight * obstacle_avoid_error_weight_ratio, + mpt_param_.obstacle_avoid_yaw_error_weight * obstacle_avoid_error_weight_ratio}; + } else if (ref_points.at(i).near_objects) { + return { + mpt_param_.obstacle_avoid_lat_error_weight, mpt_param_.obstacle_avoid_yaw_error_weight}; + } else if (i == N_ref - 1 && is_containing_path_terminal_point) { + return { + mpt_param_.terminal_path_lat_error_weight, mpt_param_.terminal_path_yaw_error_weight}; + } else if (i == N_ref - 1) { + return {mpt_param_.terminal_lat_error_weight, mpt_param_.terminal_yaw_error_weight}; + } + // NOTE: may be better to add decreasing weights in a narrow and sharp curve + // else if (std::abs(ref_points[i].k) > 0.3) { + // return {0.0, 0.0}; + // } -void MPTOptimizer::addSteerWeightF(Eigen::VectorXd * f) const -{ - constexpr double DT = 0.1; - constexpr double ctrl_period = 0.03; - constexpr double raw_steer_cmd_prev = 0; - constexpr double raw_steer_cmd_pprev = 0; + return {mpt_param_.lat_error_weight, mpt_param_.yaw_error_weight}; + }(); - if (f->rows() < 2) { - return; - } + const double adaptive_lat_error_weight = adaptive_error_weight.at(0); + const double adaptive_yaw_error_weight = adaptive_error_weight.at(1); - // steer rate for i = 0 - (*f)(0) += -2.0 * mpt_param_ptr_->steer_rate_weight / (std::pow(DT, 2)) * 0.5; + Qex_triplet_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, adaptive_lat_error_weight)); + Qex_triplet_vec.push_back( + Eigen::Triplet(i * D_x + 1, i * D_x + 1, adaptive_yaw_error_weight)); + } + Qex_sparse_mat.setFromTriplets(Qex_triplet_vec.begin(), Qex_triplet_vec.end()); + + // update R + Eigen::SparseMatrix Rex_sparse_mat(D_v, D_v); + std::vector> Rex_triplet_vec; + for (size_t i = 0; i < N_ref - 1; ++i) { + const double adaptive_steer_weight = ref_points.at(i).near_objects + ? mpt_param_.obstacle_avoid_steer_input_weight + : mpt_param_.steer_input_weight; + Rex_triplet_vec.push_back( + Eigen::Triplet(D_x + D_u * i, D_x + D_u * i, adaptive_steer_weight)); + } + addSteerWeightR(Rex_triplet_vec, ref_points); - // const double steer_acc_r = mpt_param_.weight_steer_acc / std::pow(DT, 4); - const double steer_acc_r_cp1 = mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 3) * ctrl_period); - const double steer_acc_r_cp2 = - mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 2) * std::pow(ctrl_period, 2)); - const double steer_acc_r_cp4 = mpt_param_ptr_->steer_acc_weight / std::pow(ctrl_period, 4); + Rex_sparse_mat.setFromTriplets(Rex_triplet_vec.begin(), Rex_triplet_vec.end()); - // steer acc i = 0 - (*f)(0) += ((-2.0 * raw_steer_cmd_prev + raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5; + ValueMatrix m; + m.Qex = Qex_sparse_mat; + m.Rex = Rex_sparse_mat; - // steer acc for i = 1 - (*f)(0) += (-2.0 * raw_steer_cmd_prev * (steer_acc_r_cp1 + steer_acc_r_cp2)) * 0.5; - (*f)(1) += (2.0 * raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5; + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return m; } boost::optional MPTOptimizer::executeOptimization( - const bool enable_avoidance, const MPTMatrix & m, const std::vector & ref_points, - const std::vector & path_points, const CVMaps & maps, - DebugData * debug_data) + const std::unique_ptr & prev_trajs, const bool enable_avoidance, + const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, + const std::vector & ref_points, std::shared_ptr debug_data_ptr) { - auto t_start1 = std::chrono::high_resolution_clock::now(); + if (ref_points.empty()) { + return Eigen::VectorXd{}; + } + + stop_watch_.tic(__func__); + + const size_t N_ref = ref_points.size(); - const auto x0 = ref_points.front().optimized_state; - ObjectiveMatrix obj_m = getObjectiveMatrix(x0, m); + // get matrix + ObjectiveMatrix obj_m = getObjectiveMatrix(mpt_mat, val_mat, ref_points, debug_data_ptr); ConstraintMatrix const_m = - getConstraintMatrix(enable_avoidance, x0, m, maps, ref_points, path_points, debug_data); + getConstraintMatrix(enable_avoidance, mpt_mat, ref_points, debug_data_ptr); + + // manual warm start + Eigen::VectorXd u0 = Eigen::VectorXd::Zero(obj_m.gradient.size()); + + if (mpt_param_.enable_manual_warm_start) { + const size_t D_x = vehicle_model_ptr_->getDimX(); + + if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) { + const size_t seg_idx = tier4_autoware_utils::findNearestSegmentIndex( + prev_trajs->mpt_ref_points, ref_points.front().p); + double offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + prev_trajs->mpt_ref_points, seg_idx, ref_points.front().p); + + u0(0) = prev_trajs->mpt_ref_points.at(seg_idx).optimized_kinematic_state(0); + u0(1) = prev_trajs->mpt_ref_points.at(seg_idx).optimized_kinematic_state(1); + + for (size_t i = 0; i + 1 < N_ref; ++i) { + size_t prev_idx = seg_idx + i; + const size_t prev_N_ref = prev_trajs->mpt_ref_points.size(); + if (prev_idx + 2 > prev_N_ref) { + prev_idx = static_cast(prev_N_ref) - 2; + offset = 0.5; + } + + const double prev_val = prev_trajs->mpt_ref_points.at(prev_idx).optimized_input; + const double next_val = prev_trajs->mpt_ref_points.at(prev_idx + 1).optimized_input; + u0(D_x + i) = interpolation::lerp(prev_val, next_val, offset); + } + } + } + + const Eigen::MatrixXd & H = obj_m.hessian; + const Eigen::MatrixXd & A = const_m.linear; + std::vector f; + std::vector upper_bound; + std::vector lower_bound; + + if (mpt_param_.enable_manual_warm_start) { + f = eigenVectorToStdVector(obj_m.gradient + H * u0); + Eigen::VectorXd A_times_u0 = A * u0; + upper_bound = eigenVectorToStdVector(const_m.upper_bound - A_times_u0); + lower_bound = eigenVectorToStdVector(const_m.lower_bound - A_times_u0); + } else { + f = eigenVectorToStdVector(obj_m.gradient); + upper_bound = eigenVectorToStdVector(const_m.upper_bound); + lower_bound = eigenVectorToStdVector(const_m.lower_bound); + } + + // initialize or update solver with warm start + stop_watch_.tic("initOsqp"); + autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); + autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); + if (mpt_param_.enable_warm_start && prev_mat_n == H.rows() && prev_mat_m == A.rows()) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "warm start"); + + osqp_solver_ptr_->updateCscP(P_csc); + osqp_solver_ptr_->updateQ(f); + osqp_solver_ptr_->updateCscA(A_csc); + osqp_solver_ptr_->updateL(lower_bound); + osqp_solver_ptr_->updateU(upper_bound); + } else { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "no warm start"); + + osqp_solver_ptr_ = std::make_unique( + // obj_m.hessian, const_m.linear, obj_m.gradient, const_m.lower_bound, const_m.upper_bound, + P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); + } + prev_mat_n = H.rows(); + prev_mat_m = A.rows(); - osqp_solver_ptr_ = std::make_unique( - obj_m.hessian, const_m.linear, obj_m.gradient, const_m.lower_bound, const_m.upper_bound, - 1.0e-3); - osqp_solver_ptr_->updateEpsRel(1.0e-3); + debug_data_ptr->msg_stream << " " + << "initOsqp" + << ":= " << stop_watch_.toc("initOsqp") << " [ms]\n"; + + // solve + stop_watch_.tic("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); + debug_data_ptr->msg_stream << " " + << "solveOsqp" + << ":= " << stop_watch_.toc("solveOsqp") << " [ms]\n"; - int solution_status = std::get<3>(result); + // check solution status + const int solution_status = std::get<3>(result); if (solution_status != 1) { - util::logOSQPSolutionStatus(solution_status); + utils::logOSQPSolutionStatus(solution_status); return boost::none; } + // print iteration + const int iteration_status = std::get<4>(result); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_, "iteration: %d", iteration_status); + + // get result std::vector result_vec = std::get<0>(result); + + const size_t DIM_U = vehicle_model_ptr_->getDimU(); + const size_t DIM_X = vehicle_model_ptr_->getDimX(); const Eigen::VectorXd optimized_control_variables = - Eigen::Map(&result_vec[0], ref_points.size()); + Eigen::Map(&result_vec[0], DIM_X + (N_ref - 1) * DIM_U); - auto t_end1 = std::chrono::high_resolution_clock::now(); - float elapsed_ms1 = std::chrono::duration(t_end1 - t_start1).count(); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, "mpt opt time: = %f [ms]", - elapsed_ms1); - return optimized_control_variables; -} + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; -double MPTOptimizer::calcLateralError( - const geometry_msgs::msg::Point & target_point, const ReferencePoint & ref_point) const -{ - const double err_x = target_point.x - ref_point.p.x; - const double err_y = target_point.y - ref_point.p.y; - const double ref_yaw = tf2::getYaw(ref_point.q); - const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y; - return lat_err; -} - -Eigen::VectorXd MPTOptimizer::getState( - const geometry_msgs::msg::Pose & target_pose, const ReferencePoint & nearest_ref_point) const -{ - const double lat_error = calcLateralError(target_pose.position, nearest_ref_point); - const double yaw_error = - util::normalizeRadian(tf2::getYaw(target_pose.orientation) - nearest_ref_point.yaw); - Eigen::VectorXd x0 = Eigen::VectorXd::Zero(3); - x0 << lat_error, yaw_error, std::atan(vehicle_param_ptr_->wheelbase * nearest_ref_point.k); - return x0; + const Eigen::VectorXd optimized_control_variables_with_offset = + mpt_param_.enable_manual_warm_start + ? optimized_control_variables + u0.segment(0, DIM_X + (N_ref - 1) * DIM_U) + : optimized_control_variables; + return optimized_control_variables_with_offset; } -std::vector MPTOptimizer::getMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & Uex, - const MPTMatrix & mpt_matrix, - [[maybe_unused]] const std::vector & - optimized_points) const +MPTOptimizer::ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( + const MPTMatrix & mpt_mat, const ValueMatrix & val_mat, + [[maybe_unused]] const std::vector & ref_points, + std::shared_ptr debug_data_ptr) const { - const int DIM_X = vehicle_model_ptr_->getDimX(); - const auto x0 = ref_points.front().optimized_state; - Eigen::VectorXd Xex = mpt_matrix.Aex * x0 + mpt_matrix.Bex * Uex + mpt_matrix.Wex; + stop_watch_.tic(__func__); - std::vector traj_points; - { - const double lat_error = x0(0); - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose.position.x = ref_points[0].p.x - std::sin(ref_points[0].yaw) * lat_error; - traj_point.pose.position.y = ref_points[0].p.y + std::cos(ref_points[0].yaw) * lat_error; - traj_point.longitudinal_velocity_mps = ref_points.front().v; - traj_points.push_back(traj_point); - } + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t N_ref = ref_points.size(); - for (std::size_t i = 1; i < ref_points.size(); ++i) { - const double lat_error = Xex((i - 1) * DIM_X); - Eigen::Vector3d state = Xex.segment((i - 1) * DIM_X, DIM_X); - setOptimizedState(&ref_points[i], state); - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose.position.x = ref_points[i].p.x - std::sin(ref_points[i].yaw) * lat_error; - traj_point.pose.position.y = ref_points[i].p.y + std::cos(ref_points[i].yaw) * lat_error; - traj_point.longitudinal_velocity_mps = ref_points[i].v; + const size_t D_xn = D_x * N_ref; + const size_t D_v = D_x + (N_ref - 1) * D_u; - traj_points.push_back(traj_point); - } - for (std::size_t i = 0; i < traj_points.size(); ++i) { - if (i > 0 && traj_points.size() > 1) { - traj_points[i].pose.orientation = util::getQuaternionFromPoints( - traj_points[i].pose.position, traj_points[i - 1].pose.position); - } else if (traj_points.size() > 1) { - traj_points[i].pose.orientation = util::getQuaternionFromPoints( - traj_points[i + 1].pose.position, traj_points[i].pose.position); - } else { - traj_points[i].pose.orientation = ref_points[i].q; - } - } - return traj_points; -} + // generate T matrix and vector to shift optimization center + // define Z as time-series vector of shifted deviation error + // Z = sparse_T_mat * (Bex * U + Wex) + T_vec + Eigen::SparseMatrix sparse_T_mat(D_xn, D_xn); + Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(D_xn); + std::vector> triplet_T_vec; + const double offset = mpt_param_.optimization_center_offset; -void MPTOptimizer::setOptimizedState( - ReferencePoint * ref_point, const Eigen::Vector3d & optimized_state) const -{ - ref_point->optimized_state = optimized_state; -} + for (size_t i = 0; i < N_ref; ++i) { + const double alpha = ref_points.at(i).alpha; -std::vector MPTOptimizer::getReferenceBounds( - const bool enable_avoidance, const std::vector & ref_points, const CVMaps & maps, - DebugData * debug_data) const -{ - std::vector ref_bounds; - std::vector debug_bounds_candidate_for_base_points; - std::vector debug_bounds_candidate_for_top_points; - std::vector debug_bounds_candidate_for_mid_points; - int cnt = 0; - for (const auto & point : ref_points) { - ReferencePoint ref_base_point; - ref_base_point.p = point.p; - ref_base_point.yaw = point.yaw; - - ReferencePoint ref_top_point; - ref_top_point.p = point.top_pose.position; - ref_top_point.yaw = tf2::getYaw(point.top_pose.orientation); - - ReferencePoint ref_mid_point; - ref_mid_point.p = point.mid_pose.position; - ref_mid_point.yaw = tf2::getYaw(point.mid_pose.orientation); - - geometry_msgs::msg::Pose debug_for_base_point; - debug_for_base_point.position = ref_base_point.p; - debug_for_base_point.orientation = point.q; - debug_bounds_candidate_for_base_points.push_back(debug_for_base_point); - - geometry_msgs::msg::Pose debug_for_top_point; - debug_for_top_point.position = ref_top_point.p; - debug_for_top_point.orientation = point.top_pose.orientation; - debug_bounds_candidate_for_top_points.push_back(debug_for_top_point); - - geometry_msgs::msg::Pose debug_for_mid_point; - debug_for_mid_point.position = ref_mid_point.p; - debug_for_mid_point.orientation = point.top_pose.orientation; - debug_bounds_candidate_for_mid_points.push_back(debug_for_mid_point); - - if ( - !util::transformMapToOptionalImage(ref_base_point.p, maps.map_info) || - !util::transformMapToOptionalImage(ref_mid_point.p, maps.map_info) || - !util::transformMapToOptionalImage(ref_top_point.p, maps.map_info)) { - Bounds bounds; - bounds.c0 = {1, -1}; - bounds.c1 = {1, -1}; - bounds.c2 = {1, -1}; - ref_bounds.emplace_back(bounds); - continue; - } + triplet_T_vec.push_back(Eigen::Triplet(i * D_x, i * D_x, std::cos(alpha))); + triplet_T_vec.push_back(Eigen::Triplet(i * D_x, i * D_x + 1, offset * std::cos(alpha))); + triplet_T_vec.push_back(Eigen::Triplet(i * D_x + 1, i * D_x + 1, 1.0)); - // Calculate boundaries. - auto lat_bounds_0 = getBound(enable_avoidance, ref_base_point, maps); - auto lat_bounds_1 = getBound(enable_avoidance, ref_top_point, maps); - auto lat_bounds_2 = getBound(enable_avoidance, ref_mid_point, maps); - if (!lat_bounds_0 || !lat_bounds_1 || !lat_bounds_2) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("MPTOptimizer"), is_showing_debug_info_, "Path is blocked at %i ", cnt); - Bounds bounds; - bounds.c0 = {1, -1}; - bounds.c1 = {1, -1}; - bounds.c2 = {1, -1}; - ref_bounds.emplace_back(bounds); - cnt++; - continue; - } - Bounds bounds; - bounds.c0 = lat_bounds_0.get(); - bounds.c1 = lat_bounds_1.get(); - bounds.c2 = lat_bounds_2.get(); - ref_bounds.emplace_back(bounds); - cnt++; + T_vec(i * D_x) = -offset * std::sin(alpha); } - debug_data->bounds = ref_bounds; - debug_data->bounds_candidate_for_base_points = debug_bounds_candidate_for_base_points; - debug_data->bounds_candidate_for_top_points = debug_bounds_candidate_for_top_points; - debug_data->bounds_candidate_for_mid_points = debug_bounds_candidate_for_mid_points; - return ref_bounds; -} + sparse_T_mat.setFromTriplets(triplet_T_vec.begin(), triplet_T_vec.end()); -boost::optional> MPTOptimizer::getBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const -{ - const auto rough_road_bound = getRoughBound(enable_avoidance, ref_point, maps); + const Eigen::MatrixXd B = sparse_T_mat * mpt_mat.Bex; + const Eigen::MatrixXd QB = val_mat.Qex * B; + const Eigen::MatrixXd R = val_mat.Rex; - if (!rough_road_bound) { - return boost::none; - } + // min J(v) = min (v'Hv + v'f) + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(D_v, D_v); + H.triangularView() = B.transpose() * QB + R; + H.triangularView() = H.transpose(); - std::vector broad_bound; - // initialize right bound with rough left bound - double rel_right_bound = rough_road_bound.get()[0]; - auto t_start = std::chrono::high_resolution_clock::now(); - float elapsed_ms = 0.0; - while (true) { - const auto bound_candidate = getBoundCandidate( - enable_avoidance, ref_point, maps, mpt_param_ptr_->clearance_from_road, - mpt_param_ptr_->clearance_from_object, rel_right_bound, rough_road_bound.get()); - - if (!bound_candidate) { - break; - } + // Eigen::VectorXd f = ((sparse_T_mat * mpt_mat.Wex + T_vec).transpose() * QB).transpose(); + Eigen::VectorXd f = (sparse_T_mat * mpt_mat.Wex + T_vec).transpose() * QB; - if (broad_bound.empty()) { - broad_bound = bound_candidate.get(); - } else { - const double bound_candidate_diff = - std::fabs(bound_candidate.get()[0] - bound_candidate.get()[1]); - const double broad_bound_diff = std::fabs(broad_bound[0] - broad_bound[1]); - if (bound_candidate_diff > broad_bound_diff) { - broad_bound = bound_candidate.get(); + const size_t N_avoid = mpt_param_.vehicle_circle_longitudinal_offsets.size(); + const size_t N_first_slack = [&]() -> size_t { + if (mpt_param_.soft_constraint) { + if (mpt_param_.l_inf_norm) { + return 1; } + return N_avoid; } - rel_right_bound = bound_candidate.get()[1]; - auto t_end = std::chrono::high_resolution_clock::now(); - elapsed_ms = std::chrono::duration(t_end - t_start).count(); - constexpr float max_ms = 10; - if (elapsed_ms > max_ms) { - // ROS_WARN_THROTTLE(1.0, "take too much time for calculating bound"); - return boost::none; + return 0; + }(); + const size_t N_second_slack = [&]() -> size_t { + if (mpt_param_.two_step_soft_constraint) { + return N_first_slack; } - } + return 0; + }(); - if (broad_bound.empty()) { - return boost::none; - } else { - return broad_bound; - } -} + // number of slack variables for one step + const size_t N_slack = N_first_slack + N_second_slack; -boost::optional> MPTOptimizer::getBoundCandidate( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps, - const double min_road_clearance, const double min_obj_clearance, const double rel_initial_lat, - const std::vector & rough_road_bound) const -{ - const bool search_expanding_side = true; - const auto left_bound = getValidLatError( - enable_avoidance, ref_point, rel_initial_lat, maps, min_road_clearance, min_obj_clearance, - rough_road_bound, search_expanding_side); - if (!left_bound) { - return boost::none; - } - constexpr double min_valid_lat_error = 0.1; - const double initial_right_bound = left_bound.get() - min_valid_lat_error; - const auto right_bound = getValidLatError( - enable_avoidance, ref_point, initial_right_bound, maps, min_road_clearance, min_obj_clearance, - rough_road_bound); - if (!right_bound) { - return boost::none; - } - if (std::fabs(left_bound.get() - right_bound.get()) < 1e-5) { - return boost::none; - } - std::vector bound{left_bound.get(), right_bound.get()}; - return bound; -} + // extend H for slack variables + Eigen::MatrixXd full_H = Eigen::MatrixXd::Zero(D_v + N_ref * N_slack, D_v + N_ref * N_slack); + full_H.block(0, 0, D_v, D_v) = H; -boost::optional> MPTOptimizer::getRoughBound( - const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const -{ - double left_bound = 0; - double right_bound = 0; - const double left_angle = util::normalizeRadian(ref_point.yaw + M_PI_2); - const double right_angle = util::normalizeRadian(ref_point.yaw - M_PI_2); - - geometry_msgs::msg::Point new_position; - new_position.x = ref_point.p.x; - new_position.y = ref_point.p.y; - auto original_clearance = getClearance(maps.clearance_map, new_position, maps.map_info); - auto original_object_clearance = - getClearance(maps.only_objects_clearance_map, new_position, maps.map_info); - if (!original_clearance || !original_object_clearance) { - return boost::none; - } - if (!enable_avoidance) { - original_object_clearance.emplace(std::numeric_limits::max()); - } - constexpr double min_road_clearance = 0.1; - constexpr double min_obj_clearance = 0.1; - if ( - original_clearance.get() > min_road_clearance && - original_object_clearance.get() > min_obj_clearance) { - const double initial_dist = 0; - right_bound = -1 * getTraversedDistance( - enable_avoidance, ref_point, right_angle, initial_dist, maps, - min_road_clearance, min_obj_clearance); - left_bound = getTraversedDistance( - enable_avoidance, ref_point, left_angle, initial_dist, maps, min_road_clearance, - min_obj_clearance); - } else { - const double initial_dist = 0; - const bool search_expanding_side = true; - const double right_s = getTraversedDistance( - enable_avoidance, ref_point, right_angle, initial_dist, maps, min_road_clearance, - min_obj_clearance, search_expanding_side); - const double left_s = getTraversedDistance( - enable_avoidance, ref_point, left_angle, initial_dist, maps, min_road_clearance, - min_obj_clearance, search_expanding_side); - if (left_s < right_s) { - // Pick left side: - right_bound = left_s; - left_bound = getTraversedDistance( - enable_avoidance, ref_point, left_angle, left_s, maps, min_road_clearance, - min_obj_clearance); - } else { - // Pick right side: - left_bound = -right_s; - right_bound = -getTraversedDistance( - enable_avoidance, ref_point, right_angle, right_s, maps, min_road_clearance, - min_obj_clearance); - } - } - if (std::fabs(left_bound - right_bound) < 1e-6) { - return boost::none; - } - std::vector bound{left_bound, right_bound}; - return bound; -} + // extend f for slack variables + Eigen::VectorXd full_f(D_v + N_ref * N_slack); -boost::optional MPTOptimizer::getClearance( - const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & map_info) const -{ - const auto image_point = util::transformMapToOptionalImage(map_point, map_info); - if (!image_point) { - return boost::none; + full_f.segment(0, D_v) = f; + if (N_first_slack > 0) { + full_f.segment(D_v, N_ref * N_first_slack) = + mpt_param_.soft_avoidance_weight * Eigen::VectorXd::Ones(N_ref * N_first_slack); + } + if (N_second_slack > 0) { + full_f.segment(D_v + N_ref * N_first_slack, N_ref * N_second_slack) = + mpt_param_.soft_second_avoidance_weight * Eigen::VectorXd::Ones(N_ref * N_second_slack); } - const float clearance = clearance_map.ptr(static_cast( - image_point.get().y))[static_cast(image_point.get().x)] * - map_info.resolution; - return clearance; -} -ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( - const Eigen::VectorXd & x0, const MPTMatrix & m) const -{ - const int DIM_U_N = m.Uref_ex.rows(); - const Eigen::MatrixXd CB = m.Cex * m.Bex; - const Eigen::MatrixXd QCB = m.Qex * CB; - // Eigen::MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; - // This calculation is heavy. looking for a good way. - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(DIM_U_N, DIM_U_N); - H.triangularView() = CB.transpose() * QCB; - H.triangularView() += m.R1ex + m.R2ex; - H.triangularView() = H.transpose(); - Eigen::VectorXd f = - (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex; - addSteerWeightF(&f); - - constexpr int num_lat_constraint = 3; - const int num_objective_variables = DIM_U_N * (1 + num_lat_constraint); - Eigen::MatrixXd extend_h = Eigen::MatrixXd::Zero(DIM_U_N, DIM_U_N); - Eigen::VectorXd extend_f = Eigen::VectorXd::Ones(DIM_U_N); - Eigen::MatrixXd concat_h = - Eigen::MatrixXd::Zero(num_objective_variables, num_objective_variables); - Eigen::VectorXd concat_f = Eigen::VectorXd::Zero(num_objective_variables); - concat_h.block(0, 0, DIM_U_N, DIM_U_N) = H; - concat_h.block(DIM_U_N, DIM_U_N, DIM_U_N, DIM_U_N) = extend_h; - concat_h.block(DIM_U_N * 2, DIM_U_N * 2, DIM_U_N, DIM_U_N) = extend_h; - concat_h.block(DIM_U_N * 3, DIM_U_N * 3, DIM_U_N, DIM_U_N) = extend_h; - concat_f << f, mpt_param_ptr_->base_point_weight * extend_f, - mpt_param_ptr_->top_point_weight * extend_f, mpt_param_ptr_->mid_point_weight * extend_f; ObjectiveMatrix obj_matrix; - obj_matrix.hessian = concat_h; - obj_matrix.gradient = {concat_f.data(), concat_f.data() + concat_f.rows()}; + obj_matrix.hessian = full_H; + obj_matrix.gradient = full_f; + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return obj_matrix; } @@ -894,368 +890,737 @@ ObjectiveMatrix MPTOptimizer::getObjectiveMatrix( // Set constraint: lb <= Ax <= ub // decision variable // x := [u0, ..., uN-1 | z00, ..., z0N-1 | z10, ..., z1N-1 | z20, ..., z2N-1] -// \in \mathbb{R}^{N * (N_point + 1)} -ConstraintMatrix MPTOptimizer::getConstraintMatrix( - const bool enable_avoidance, const Eigen::VectorXd & x0, const MPTMatrix & m, const CVMaps & maps, +// \in \mathbb{R}^{N * (N_vehicle_circle + 1)} +MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix( + [[maybe_unused]] const bool enable_avoidance, const MPTMatrix & mpt_mat, const std::vector & ref_points, - [[maybe_unused]] const std::vector & path_points, - DebugData * debug_data) const + [[maybe_unused]] std::shared_ptr debug_data_ptr) const { - std::vector dist_vec{ - mpt_param_ptr_->base_point_dist_from_base_link, mpt_param_ptr_->top_point_dist_from_base_link, - mpt_param_ptr_->mid_point_dist_from_base_link}; - - const size_t N_ref = m.Uref_ex.rows(); - const size_t N_state = vehicle_model_ptr_->getDimX(); - const size_t N_point = dist_vec.size(); - const size_t N_dec = N_ref * (N_point + 1); - - const auto bounds = getReferenceBounds(enable_avoidance, ref_points, maps, debug_data); - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * N_ref * N_point + N_ref, N_dec); - Eigen::VectorXd lb = - Eigen::VectorXd::Constant(3 * N_ref * N_point + N_ref, -autoware::common::osqp::INF); - Eigen::VectorXd ub = - Eigen::VectorXd::Constant(3 * N_ref * N_point + N_ref, autoware::common::osqp::INF); - - // Define constraint matrices and vectors - // Gap from reference point around vehicle base_link - { - // C := [I | O | O] - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); - for (size_t i = 0; i < N_ref; ++i) { - C(i, N_state * i) = 1; - } - // bias := Cast * (Aex * x0 + Wex) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); - // A_blk := [C * Bex | I | O | O - // -C * Bex | I | O | O - // O | I | O | O] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec); - A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex; - A_blk.block(0, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex; - A_blk.block(N_ref, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb_blk := [-bias + bounds.lb - // bias - bounds.ub - // 0] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - lb_blk(i) = -bias(i) + bounds[i].c0.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i].c0.ub; - } else { - lb_blk(i) = -bias(i) + bounds[i + 1].c0.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c0.ub; - } - } - // Assign - A.block(0, 0, 3 * N_ref, N_dec) = A_blk; - lb.segment(0, 3 * N_ref) = lb_blk; - } + stop_watch_.tic(__func__); - // Gap from reference point around vehicle top - { - // C := diag([cos(alpha1) | l1*cos(alpha1) | 0]) - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); - for (size_t i = 0; i < N_ref; ++i) { - Eigen::MatrixXd Cast = Eigen::MatrixXd::Zero(1, N_state); - if (i == N_ref - 1) { - Cast(0, 0) = std::cos(ref_points[i].delta_yaw_from_p1); - Cast(0, 1) = dist_vec[1] * std::cos(ref_points[i].delta_yaw_from_p1); - } else { - Cast(0, 0) = std::cos(ref_points[i + 1].delta_yaw_from_p1); - Cast(0, 1) = dist_vec[1] * std::cos(ref_points[i + 1].delta_yaw_from_p1); + // NOTE: currently, add additional length to soft bounds approximately + // for soft second and hard bounds + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t N_ref = ref_points.size(); + + const size_t N_u = (N_ref - 1) * D_u; + const size_t D_v = D_x + N_u; + + const size_t N_avoid = mpt_param_.vehicle_circle_longitudinal_offsets.size(); + + // number of slack variables for one step + const size_t N_first_slack = [&]() -> size_t { + if (mpt_param_.soft_constraint) { + if (mpt_param_.l_inf_norm) { + return 1; } - C.block(i, N_state * i, 1, N_state) = Cast; + return N_avoid; } - // bias := Cast * (Aex * x0 + Wex) - l1 * sin(alpha1) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - bias(i) -= dist_vec[1] * std::sin(ref_points[i].delta_yaw_from_p1); - } else { - bias(i) -= dist_vec[1] * std::sin(ref_points[i + 1].delta_yaw_from_p1); - } + return 0; + }(); + const size_t N_second_slack = [&]() -> size_t { + if (mpt_param_.soft_constraint && mpt_param_.two_step_soft_constraint) { + return N_first_slack; } - // A_blk := [C * Bex | O | I | O - // -C * Bex | O | I | O - // O | O | I | O] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec); - A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex; - A_blk.block(0, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex; - A_blk.block(N_ref, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb_blk := [-bias + bounds.lb - // bias - bounds.ub - // 0] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - lb_blk(i) = -bias(i) + bounds[i].c1.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i].c1.ub; - } else { - lb_blk(i) = -bias(i) + bounds[i + 1].c1.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c1.ub; - } + return 0; + }(); + + // number of all slack variables is N_ref * N_slack + const size_t N_slack = N_first_slack + N_second_slack; + const size_t N_soft = mpt_param_.two_step_soft_constraint ? 2 : 1; + + const size_t A_cols = [&] { + if (mpt_param_.soft_constraint) { + return D_v + N_ref * N_slack; // initial_state + steer + soft + } + return D_v; // initial state + steer + }(); + + // calculate indices of fixed points + std::vector fixed_points_indices; + for (size_t i = 0; i < N_ref; ++i) { + if (ref_points.at(i).fix_kinematic_state) { + fixed_points_indices.push_back(i); } - // Assign - A.block(3 * N_ref, 0, 3 * N_ref, N_dec) = A_blk; - lb.segment(3 * N_ref, 3 * N_ref) = lb_blk; } - // Gap from reference point around vehicle middle - { - // C := [diag(cos(alpha2)) | diag(l2*cos(alpha2)) | O] - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); + + // calculate rows of A + size_t A_rows = 0; + if (mpt_param_.soft_constraint) { + // 3 means slack variable constraints to be between lower and upper bounds, and positive. + A_rows += 3 * N_ref * N_avoid * N_soft; + } + if (mpt_param_.hard_constraint) { + A_rows += N_ref * N_avoid; + } + A_rows += fixed_points_indices.size() * D_x; + if (mpt_param_.steer_limit_constraint) { + A_rows += N_u; + } + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, A_cols); + Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); + Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); + size_t A_rows_end = 0; + + // CX = C(Bv + w) + C \in R^{N_ref, N_ref * D_x} + for (size_t l_idx = 0; l_idx < N_avoid; ++l_idx) { + // create C := [1 | l | O] + Eigen::SparseMatrix C_sparse_mat(N_ref, N_ref * D_x); + std::vector> C_triplet_vec; + Eigen::VectorXd C_vec = Eigen::VectorXd::Zero(N_ref); + + // calculate C mat and vec for (size_t i = 0; i < N_ref; ++i) { - Eigen::MatrixXd Cast = Eigen::MatrixXd::Zero(1, N_state); - if (i == N_ref - 1) { - Cast(0, 0) = std::cos(ref_points[i].delta_yaw_from_p2); - Cast(0, 1) = dist_vec[2] * std::cos(ref_points[i].delta_yaw_from_p2); - } else { - Cast(0, 0) = std::cos(ref_points[i + 1].delta_yaw_from_p2); - Cast(0, 1) = dist_vec[2] * std::cos(ref_points[i + 1].delta_yaw_from_p2); - } - C.block(i, N_state * i, 1, N_state) = Cast; + const double beta = ref_points.at(i).beta.at(l_idx).get(); + const double avoid_offset = mpt_param_.vehicle_circle_longitudinal_offsets.at(l_idx); + + C_triplet_vec.push_back(Eigen::Triplet(i, i * D_x, 1.0 * std::cos(beta))); + C_triplet_vec.push_back( + Eigen::Triplet(i, i * D_x + 1, avoid_offset * std::cos(beta))); + C_vec(i) = avoid_offset * std::sin(beta); } - // bias := Cast * (Aex * x0 + Wex) - l2 * sin(alpha2) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - bias(i) -= dist_vec[2] * std::sin(ref_points[i].delta_yaw_from_p2); - } else { - bias(i) -= dist_vec[2] * std::sin(ref_points[i + 1].delta_yaw_from_p2); + C_sparse_mat.setFromTriplets(C_triplet_vec.begin(), C_triplet_vec.end()); + + // calculate CB, and CW + const Eigen::MatrixXd CB = C_sparse_mat * mpt_mat.Bex; + const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.Wex + C_vec; + + // calculate bounds + const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx); + + // soft constraints + if (mpt_param_.soft_constraint) { + size_t A_offset_cols = D_v; + for (size_t s_idx = 0; s_idx < N_soft; ++s_idx) { + const size_t A_blk_rows = 3 * N_ref; + + // A := [C * Bex | O | ... | O | I | O | ... + // -C * Bex | O | ... | O | I | O | ... + // O | O | ... | O | I | O | ... ] + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); + A_blk.block(0, 0, N_ref, D_v) = CB; + A_blk.block(N_ref, 0, N_ref, D_v) = -CB; + + size_t local_A_offset_cols = A_offset_cols; + if (!mpt_param_.l_inf_norm) { + local_A_offset_cols += N_ref * l_idx; + } + A_blk.block(0, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); + A_blk.block(N_ref, local_A_offset_cols, N_ref, N_ref) = + Eigen::MatrixXd::Identity(N_ref, N_ref); + A_blk.block(2 * N_ref, local_A_offset_cols, N_ref, N_ref) = + Eigen::MatrixXd::Identity(N_ref, N_ref); + + // lb := [lower_bound - CW + // CW - upper_bound + // O ] + Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(A_blk_rows); + lb_blk.segment(0, N_ref) = -CW + part_lb; + lb_blk.segment(N_ref, N_ref) = CW - part_ub; + + if (s_idx == 1) { + // add additional clearance + const double diff_clearance = + mpt_param_.soft_second_clearance_from_road - mpt_param_.soft_clearance_from_road; + lb_blk.segment(0, N_ref) -= Eigen::MatrixXd::Constant(N_ref, 1, diff_clearance); + lb_blk.segment(N_ref, N_ref) -= Eigen::MatrixXd::Constant(N_ref, 1, diff_clearance); + } + + A_offset_cols += N_ref * N_first_slack; + + A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; + lb.segment(A_rows_end, A_blk_rows) = lb_blk; + + A_rows_end += A_blk_rows; } } - // A_blk := [C * Bex | O | O | I - // -C * Bex | O | O | I - // O | O | O | I] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec); - A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex; - A_blk.block(0, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex; - A_blk.block(N_ref, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - A_blk.block(2 * N_ref, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb_blk := [-bias + bounds.lb - // bias - bounds.ub - // 0] - Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref); - for (size_t i = 0; i < N_ref; ++i) { - if (i == N_ref - 1) { - lb_blk(i) = -bias(i) + bounds[i].c2.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i].c2.ub; - } else { - lb_blk(i) = -bias(i) + bounds[i + 1].c2.lb; - lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c2.ub; - } + + // hard constraints + if (mpt_param_.hard_constraint) { + const size_t A_blk_rows = N_ref; + + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); + A_blk.block(0, 0, N_ref, N_ref) = CB; + + A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; + lb.segment(A_rows_end, A_blk_rows) = part_lb - CW; + ub.segment(A_rows_end, A_blk_rows) = part_ub - CW; + + A_rows_end += A_blk_rows; } - // Assign - A.block(2 * 3 * N_ref, 0, 3 * N_ref, N_dec) = A_blk; - lb.segment(2 * 3 * N_ref, 3 * N_ref) = lb_blk; } - // Fixed points constraint - { - // C := [I | O | O] - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state); - for (size_t i = 0; i < N_ref; ++i) { - C(i, N_state * i) = 1; - } - // bias := Cast * (Aex * x0 + Wex) - Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex); + // fixed points constraint + // CX = C(B v + w) where C extracts fixed points + if (fixed_points_indices.size() > 0) { + for (const size_t i : fixed_points_indices) { + A.block(A_rows_end, 0, D_x, D_v) = mpt_mat.Bex.block(i * D_x, 0, D_x, D_v); - // Assign - A.block(3 * N_point * N_ref, 0, N_ref, N_ref) = C * m.Bex; - for (size_t i = 0; i < N_ref; ++i) { - if (ref_points[i + 1].fix_state && i + 1 < N_ref) { - lb(3 * N_point * N_ref + i) = ref_points[i + 1].fix_state.get()(0) - bias(i); - ub(3 * N_point * N_ref + i) = ref_points[i + 1].fix_state.get()(0) - bias(i); - } else if (i == ref_points.size() - 1 && mpt_param_ptr_->is_hard_fixing_terminal_point) { - lb(3 * N_point * N_ref + i) = -bias(i); - ub(3 * N_point * N_ref + i) = -bias(i); - } + lb.segment(A_rows_end, D_x) = + ref_points[i].fix_kinematic_state.get() - mpt_mat.Wex.segment(i * D_x, D_x); + ub.segment(A_rows_end, D_x) = + ref_points[i].fix_kinematic_state.get() - mpt_mat.Wex.segment(i * D_x, D_x); + + A_rows_end += D_x; } } - ConstraintMatrix constraint_matrix; - - constraint_matrix.linear = A; + // steer max limit + if (mpt_param_.steer_limit_constraint) { + A.block(A_rows_end, D_x, N_u, N_u) = Eigen::MatrixXd::Identity(N_u, N_u); + lb.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, -mpt_param_.max_steer_rad); + ub.segment(A_rows_end, N_u) = Eigen::MatrixXd::Constant(N_u, 1, mpt_param_.max_steer_rad); - for (int i = 0; i < lb.size(); ++i) { - constraint_matrix.lower_bound.push_back(lb(i)); - constraint_matrix.upper_bound.push_back(ub(i)); + A_rows_end += N_u; } + ConstraintMatrix constraint_matrix; + constraint_matrix.linear = A; + constraint_matrix.lower_bound = lb; + constraint_matrix.upper_bound = ub; + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; return constraint_matrix; } -std::vector MPTOptimizer::getBaseReferencePoints( - const std::vector & interpolated_points, - const std::unique_ptr & prev_trajs, DebugData * debug_data) const +std::vector MPTOptimizer::getMPTPoints( + std::vector & fixed_ref_points, + std::vector & non_fixed_ref_points, const Eigen::VectorXd & Uex, + const MPTMatrix & mpt_mat, std::shared_ptr debug_data_ptr) { - std::vector reference_points; - for (const auto & e : interpolated_points) { - ReferencePoint ref_point; - ref_point.p = e; - reference_points.push_back(ref_point); - } - if (!prev_trajs) { - return reference_points; + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t N_ref = static_cast(Uex.rows() - D_x) + 1; + + stop_watch_.tic(__func__); + + std::vector lat_error_vec; + std::vector yaw_error_vec; + for (size_t i = 0; i < fixed_ref_points.size(); ++i) { + const auto & ref_point = fixed_ref_points.at(i); + + lat_error_vec.push_back(ref_point.fix_kinematic_state.get()(0)); + yaw_error_vec.push_back(ref_point.fix_kinematic_state.get()(1)); } - if (prev_trajs->model_predictive_trajectory.size() != prev_trajs->mpt_ref_points.size()) { - return reference_points; + + const size_t N_kinematic_state = vehicle_model_ptr_->getDimX(); + const Eigen::VectorXd Xex = mpt_mat.Bex * Uex + mpt_mat.Wex; + + for (size_t i = 0; i < non_fixed_ref_points.size(); ++i) { + lat_error_vec.push_back(Xex(i * N_kinematic_state)); + yaw_error_vec.push_back(Xex(i * N_kinematic_state + 1)); } - // re-calculating points' position for fixing - std::vector cropped_interpolated_points; - double accum_s_for_interpolated = 0; - for (std::size_t i = 0; i < interpolated_points.size(); i++) { - if (i > 0) { - accum_s_for_interpolated += - util::calculate2DDistance(interpolated_points[i], interpolated_points[i - 1]); + // calculate trajectory from optimization result + std::vector traj_points; + debug_data_ptr->vehicle_circles_pose.resize(lat_error_vec.size()); + for (size_t i = 0; i < lat_error_vec.size(); ++i) { + auto & ref_point = (i < fixed_ref_points.size()) + ? fixed_ref_points.at(i) + : non_fixed_ref_points.at(i - fixed_ref_points.size()); + const double lat_error = lat_error_vec.at(i); + const double yaw_error = yaw_error_vec.at(i); + + geometry_msgs::msg::Pose ref_pose; + ref_pose.position = ref_point.p; + ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); + debug_data_ptr->mpt_ref_poses.push_back(ref_pose); + debug_data_ptr->lateral_errors.push_back(lat_error); + + ref_point.optimized_kinematic_state << lat_error_vec.at(i), yaw_error_vec.at(i); + if (i >= fixed_ref_points.size()) { + const size_t j = i - fixed_ref_points.size(); + if (j == N_ref - 1) { + ref_point.optimized_input = 0.0; + } else { + ref_point.optimized_input = Uex(D_x + j * D_u); + } } - if ( - accum_s_for_interpolated > - traj_param_ptr_->num_sampling_points * traj_param_ptr_->delta_arc_length_for_mpt_points) { - break; + + autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0); + + traj_point.longitudinal_velocity_mps = ref_point.v; + traj_points.push_back(traj_point); + + { // for debug visualization + const double base_x = ref_point.p.x - std::sin(ref_point.yaw) * lat_error; + const double base_y = ref_point.p.y + std::cos(ref_point.yaw) * lat_error; + + // NOTE: coordinate of vehicle_circle_longitudinal_offsets is back wheel center + for (const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { + geometry_msgs::msg::Pose vehicle_circle_pose; + + vehicle_circle_pose.position.x = base_x + d * std::cos(ref_point.yaw + yaw_error); + vehicle_circle_pose.position.y = base_y + d * std::sin(ref_point.yaw + yaw_error); + + vehicle_circle_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw + ref_point.alpha); + + debug_data_ptr->vehicle_circles_pose.at(i).push_back(vehicle_circle_pose); + } } - cropped_interpolated_points.push_back(interpolated_points[i]); } - constexpr double fine_resolution = 0.005; - std::vector fine_interpolated_points; - fine_interpolated_points = - util::getInterpolatedPoints(cropped_interpolated_points, fine_resolution); - const double max_s = - traj_param_ptr_->backward_fixing_distance + traj_param_ptr_->forward_fixing_mpt_distance; - const int num_points = std::min( - static_cast(reference_points.size()), - static_cast(max_s / traj_param_ptr_->delta_arc_length_for_mpt_points)); - - for (int i = 0; i < num_points; i++) { - const int nearest_prev_idx = - util::getNearestPointIdx(prev_trajs->mpt_ref_points, reference_points[i].p); - if ( - util::calculate2DDistance( - prev_trajs->mpt_ref_points[nearest_prev_idx].p, reference_points[i].p) >= - std::fabs(traj_param_ptr_->delta_arc_length_for_mpt_points)) { + + // NOTE: If generated trajectory's orientation is not so smooth or kinematically infeasible, + // recalculate orientation here for (size_t i = 0; i < lat_error_vec.size(); ++i) { + // auto & ref_point = (i < fixed_ref_points.size()) + // ? fixed_ref_points.at(i) + // : non_fixed_ref_points.at(i - fixed_ref_points.size()); + // + // if (i > 0 && traj_points.size() > 1) { + // traj_points.at(i).pose.orientation = geometry_utils::getQuaternionFromPoints( + // traj_points.at(i).pose.position, traj_points.at(i - 1).pose.position); + // } else if (traj_points.size() > 1) { + // traj_points.at(i).pose.orientation = geometry_utils::getQuaternionFromPoints( + // traj_points.at(i + 1).pose.position, traj_points.at(i).pose.position); + // } else { + // traj_points.at(i).pose.orientation = + // tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); + // } + // } + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + + return traj_points; +} + +void MPTOptimizer::calcOrientation(std::vector & ref_points) const +{ + const auto yaw_angles = slerpYawFromReferencePoints(ref_points); + for (size_t i = 0; i < ref_points.size(); ++i) { + if (ref_points.at(i).fix_kinematic_state) { continue; } - const int nearest_idx = - util::getNearestIdx(fine_interpolated_points, prev_trajs->mpt_ref_points[nearest_prev_idx].p); - if ( - util::calculate2DDistance( - fine_interpolated_points[nearest_idx], prev_trajs->mpt_ref_points[nearest_prev_idx].p) < - fine_resolution) { - reference_points[i] = prev_trajs->mpt_ref_points[nearest_prev_idx]; - reference_points[i].fix_state = prev_trajs->mpt_ref_points[nearest_prev_idx].optimized_state; + + ref_points.at(i).yaw = yaw_angles.at(i); + } +} + +void MPTOptimizer::calcVelocity( + std::vector & ref_points, + const std::vector & points) const +{ + for (size_t i = 0; i < ref_points.size(); i++) { + ref_points.at(i).v = points[tier4_autoware_utils::findNearestIndex(points, ref_points.at(i).p)] + .longitudinal_velocity_mps; + } +} + +void MPTOptimizer::calcCurvature(std::vector & ref_points) const +{ + const size_t num_points = static_cast(ref_points.size()); + + // calculate curvature by circle fitting from three points + size_t max_smoothing_num = static_cast(std::floor(0.5 * (num_points - 1))); + size_t L = + std::min(static_cast(mpt_param_.num_curvature_sampling_points), max_smoothing_num); + auto curvatures = points_utils::calcCurvature( + ref_points, static_cast(mpt_param_.num_curvature_sampling_points)); + for (size_t i = L; i < num_points - L; ++i) { + if (!ref_points.at(i).fix_kinematic_state) { + ref_points.at(i).k = curvatures.at(i); + } + } + // first and last curvature is copied from next value + for (size_t i = 0; i < std::min(L, num_points); ++i) { + if (!ref_points.at(i).fix_kinematic_state) { + ref_points.at(i).k = ref_points.at(std::min(L, num_points - 1)).k; + } + if (!ref_points.at(num_points - i - 1).fix_kinematic_state) { + ref_points.at(num_points - i - 1).k = + ref_points.at(std::max(static_cast(num_points) - static_cast(L) - 1, 0)).k; } } +} - std::vector trimmed_reference_points; - for (std::size_t i = 0; i < reference_points.size(); i++) { +void MPTOptimizer::calcArcLength(std::vector & ref_points) const +{ + for (size_t i = 0; i < ref_points.size(); i++) { if (i > 0) { - const double dx = reference_points[i].p.x - reference_points[i - 1].p.x; - const double dy = reference_points[i].p.y - reference_points[i - 1].p.y; - if (std::fabs(dx) < 1e-6 && std::fabs(dy) < 1e-6) { - continue; + geometry_msgs::msg::Point a, b; + a = ref_points.at(i).p; + b = ref_points.at(i - 1).p; + ref_points.at(i).s = ref_points.at(i - 1).s + tier4_autoware_utils::calcDistance2d(a, b); + } + } +} + +void MPTOptimizer::calcExtraPoints( + std::vector & ref_points, const std::unique_ptr & prev_trajs) const +{ + for (size_t i = 0; i < ref_points.size(); ++i) { + // alpha + const double front_wheel_s = + ref_points.at(i).s + + vehicle_param_.wheelbase; // TODO(murooka) use offset instead of wheelbase? + const int front_wheel_nearest_idx = points_utils::getNearestIdx(ref_points, front_wheel_s, i); + const auto front_wheel_pos = ref_points.at(front_wheel_nearest_idx).p; + + const bool are_too_close_points = + tier4_autoware_utils::calcDistance2d(front_wheel_pos, ref_points.at(i).p) < 1e-03; + const auto front_wheel_yaw = are_too_close_points ? ref_points.at(i).yaw + : tier4_autoware_utils::calcAzimuthAngle( + ref_points.at(i).p, front_wheel_pos); + ref_points.at(i).alpha = + tier4_autoware_utils::normalizeRadian(front_wheel_yaw - ref_points.at(i).yaw); + + // near objects + ref_points.at(i).near_objects = [&]() { + const int avoidance_check_steps = + mpt_param_.near_objects_length / mpt_param_.delta_arc_length_for_mpt_points; + + const int avoidance_check_begin_idx = + std::max(0, static_cast(i) - avoidance_check_steps); + const int avoidance_check_end_idx = + std::min(static_cast(ref_points.size()), static_cast(i) + avoidance_check_steps); + + for (int a_idx = avoidance_check_begin_idx; a_idx < avoidance_check_end_idx; ++a_idx) { + if (ref_points.at(a_idx).vehicle_bounds.at(0).hasCollisionWithObject()) { + return true; + } + } + return false; + }(); + + // The point are considered to be near the object if nearest previous ref point is near the + // object. + if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) { + const auto & prev_ref_points = prev_trajs->mpt_ref_points; + + const size_t prev_idx = + tier4_autoware_utils::findNearestIndex(prev_ref_points, ref_points.at(i).p); + const double dist_to_nearest_prev_ref = + tier4_autoware_utils::calcDistance2d(prev_ref_points.at(prev_idx), ref_points.at(i)); + if (dist_to_nearest_prev_ref < 1.0 && prev_ref_points.at(prev_idx).near_objects) { + ref_points.at(i).near_objects = true; } } - trimmed_reference_points.push_back(reference_points[i]); } +} + +void MPTOptimizer::addSteerWeightR( + std::vector> & Rex_triplet_vec, + const std::vector & ref_points) const +{ + const size_t D_x = vehicle_model_ptr_->getDimX(); + const size_t D_u = vehicle_model_ptr_->getDimU(); + const size_t N_ref = ref_points.size(); + const size_t N_u = (N_ref - 1) * D_u; + const size_t D_v = D_x + N_u; + + // add steering rate : weight for (u(i) - u(i-1))^2 + for (size_t i = D_x; i < D_v - 1; ++i) { + Rex_triplet_vec.push_back(Eigen::Triplet(i, i, mpt_param_.steer_rate_weight)); + Rex_triplet_vec.push_back(Eigen::Triplet(i + 1, i, -mpt_param_.steer_rate_weight)); + Rex_triplet_vec.push_back(Eigen::Triplet(i, i + 1, -mpt_param_.steer_rate_weight)); + Rex_triplet_vec.push_back(Eigen::Triplet(i + 1, i + 1, mpt_param_.steer_rate_weight)); + } +} - for (const auto & e : trimmed_reference_points) { - if (e.fix_state) { - geometry_msgs::msg::Point rel_point; - rel_point.y = e.fix_state.get()(0); - geometry_msgs::msg::Pose origin; - origin.position = e.p; - origin.orientation = e.q; - geometry_msgs::msg::Pose debug_pose; - debug_pose.position = util::transformToAbsoluteCoordinate2D(rel_point, origin); - debug_data->fixed_mpt_points.push_back(debug_pose); - continue; +void MPTOptimizer::calcBounds( + std::vector & ref_points, const bool enable_avoidance, const CVMaps & maps, + std::shared_ptr debug_data_ptr) const +{ + stop_watch_.tic(__func__); + + // search bounds candidate for each ref points + SequentialBoundsCandidates sequential_bounds_candidates; + for (const auto & ref_point : ref_points) { + const auto bounds_candidates = getBoundsCandidates( + enable_avoidance, convertRefPointsToPose(ref_point), maps, debug_data_ptr); + sequential_bounds_candidates.push_back(bounds_candidates); + } + + // search continuous and widest bounds only for front point + for (size_t i = 0; i < sequential_bounds_candidates.size(); ++i) { + // NOTE: back() is the front avoiding circle + const auto & bounds_candidates = sequential_bounds_candidates.at(i); + const auto & ref_point = ref_points.at(i); + + // extract only continuous bounds; + if (i == 0) { // TODO(murooka) use previous bounds, not widest bounds + const auto & widest_bounds = findWidestBounds(bounds_candidates); + ref_points.at(i).bounds = widest_bounds; + } else { + const auto & prev_ref_point = ref_points.at(i - 1); + const auto & prev_continuous_bounds = prev_ref_point.bounds; + + // search continuous bounds + double max_length = std::numeric_limits::min(); + int max_idx = -1; + for (size_t c_idx = 0; c_idx < bounds_candidates.size(); ++c_idx) { + const auto & bounds_candidate = bounds_candidates.at(c_idx); + const double overlapped_length = calcOverlappedBounds( + convertRefPointsToPose(ref_point), bounds_candidate, + convertRefPointsToPose(prev_ref_point), prev_continuous_bounds); + if (overlapped_length > 0 && max_length < overlapped_length) { + max_length = overlapped_length; + max_idx = c_idx; + } + } + + // find widest bounds + if (max_idx == -1) { + // NOTE: set invalid bounds so that MPT won't be solved + // TODO(murooka) this invalid bounds even makes optimization solved. consider if this is ok + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger("getBounds: front"), is_showing_debug_info_, "invalid bounds"); + const auto invalid_bounds = + Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; + ref_points.at(i).bounds = invalid_bounds; + } else { + ref_points.at(i).bounds = bounds_candidates.at(max_idx); + } } } - return trimmed_reference_points; + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return; } -double MPTOptimizer::getTraversedDistance( - const bool enable_avoidance, const ReferencePoint & ref_point, const double traverse_angle, - const double initial_value, const CVMaps & maps, const double min_road_clearance, - const double min_obj_clearance, const bool search_expanding_side) const +void MPTOptimizer::calcVehicleBounds( + std::vector & ref_points, [[maybe_unused]] const CVMaps & maps, + std::shared_ptr debug_data_ptr, [[maybe_unused]] const bool enable_avoidance) const { - constexpr double ds = 0.1; - constexpr double lane_width = 7.5; - int n = static_cast(lane_width / ds); - - double traversed_dist = initial_value; - for (int i = 0; i < n; ++i) { - traversed_dist += ds; - geometry_msgs::msg::Point new_position; - new_position.x = ref_point.p.x + traversed_dist * std::cos(traverse_angle); - new_position.y = ref_point.p.y + traversed_dist * std::sin(traverse_angle); - auto clearance = getClearance(maps.clearance_map, new_position, maps.map_info); - auto obj_clearance = getClearance(maps.only_objects_clearance_map, new_position, maps.map_info); - if (!clearance || !obj_clearance) { - clearance.emplace(0); - obj_clearance.emplace(0); + stop_watch_.tic(__func__); + + if (ref_points.size() == 1) { + for ([[maybe_unused]] const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { + ref_points.at(0).vehicle_bounds.push_back(ref_points.at(0).bounds); + ref_points.at(0).beta.push_back(0.0); } - if (!enable_avoidance) { - obj_clearance.emplace(std::numeric_limits::max()); + return; + } + + SplineInterpolationPoints2d ref_points_spline_interpolation; + ref_points_spline_interpolation.calcSplineCoefficients(points_utils::convertToPoints(ref_points)); + + for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { + const auto & ref_point = ref_points.at(p_idx); + ref_points.at(p_idx).vehicle_bounds.clear(); + ref_points.at(p_idx).beta.clear(); + + for (const double d : mpt_param_.vehicle_circle_longitudinal_offsets) { + geometry_msgs::msg::Pose avoid_traj_pose; + avoid_traj_pose.position = + ref_points_spline_interpolation.getSplineInterpolatedPoint(p_idx, d); + const double vehicle_bounds_pose_yaw = + ref_points_spline_interpolation.getSplineInterpolatedYaw(p_idx, d); + avoid_traj_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(vehicle_bounds_pose_yaw); + + const double avoid_yaw = std::atan2( + avoid_traj_pose.position.y - ref_point.p.y, avoid_traj_pose.position.x - ref_point.p.x); + const double beta = ref_point.yaw - vehicle_bounds_pose_yaw; + ref_points.at(p_idx).beta.push_back(beta); + + const double offset_y = -tier4_autoware_utils::calcDistance2d(ref_point, avoid_traj_pose) * + std::sin(avoid_yaw - vehicle_bounds_pose_yaw); + + const auto vehicle_bounds_pose = + tier4_autoware_utils::calcOffsetPose(avoid_traj_pose, 0.0, offset_y, 0.0); + + // interpolate bounds + const double avoid_s = ref_points_spline_interpolation.getAccumulatedLength(p_idx) + d; + for (size_t cp_idx = p_idx; cp_idx < ref_points.size(); ++cp_idx) { + const double current_s = ref_points_spline_interpolation.getAccumulatedLength(cp_idx); + if (avoid_s <= current_s) { + double prev_avoid_idx; + if (cp_idx == 0) { + prev_avoid_idx = cp_idx; + } else { + prev_avoid_idx = cp_idx - 1; + } + + const double prev_s = + ref_points_spline_interpolation.getAccumulatedLength(prev_avoid_idx); + const double next_s = + ref_points_spline_interpolation.getAccumulatedLength(prev_avoid_idx + 1); + const double ratio = (avoid_s - prev_s) / (next_s - prev_s); + + const auto prev_bounds = ref_points.at(prev_avoid_idx).bounds; + const auto next_bounds = ref_points.at(prev_avoid_idx + 1).bounds; + + auto bounds = Bounds::lerp(prev_bounds, next_bounds, ratio); + bounds.translate(offset_y); + + ref_points.at(p_idx).vehicle_bounds.push_back(bounds); + break; + } + + if (cp_idx == ref_points.size() - 1) { + ref_points.at(p_idx).vehicle_bounds.push_back(ref_points.back().bounds); + } + } + + ref_points.at(p_idx).vehicle_bounds_poses.push_back(vehicle_bounds_pose); } - if (search_expanding_side) { - if (clearance.get() > min_road_clearance && obj_clearance.get() > min_obj_clearance) { - break; + } + + debug_data_ptr->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; +} + +BoundsCandidates MPTOptimizer::getBoundsCandidates( + const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, const CVMaps & maps, + [[maybe_unused]] std::shared_ptr debug_data_ptr) const +{ + BoundsCandidates bounds_candidate; + + constexpr double max_search_lane_width = 5.0; + const std::vector ds_vec{0.45, 0.15, 0.05, 0.01}; + + // search right to left + const double bound_angle = + tier4_autoware_utils::normalizeRadian(tf2::getYaw(avoiding_point.orientation) + M_PI_2); + + double traversed_dist = -max_search_lane_width; + double current_right_bound = -max_search_lane_width; + + // calculate the initial position is empty or not + // 0.drivable, 1.out of map 2.out of road or object + CollisionType previous_collision_type = + getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); + + const auto has_collision = [&](const CollisionType & collision_type) -> bool { + return collision_type == CollisionType::OUT_OF_ROAD || collision_type == CollisionType::OBJECT; + }; + CollisionType latest_right_bound_collision_type = CollisionType::OUT_OF_ROAD; + + while (traversed_dist < max_search_lane_width) { + for (size_t ds_idx = 0; ds_idx < ds_vec.size(); ++ds_idx) { + const double ds = ds_vec.at(ds_idx); + while (true) { + const CollisionType current_collision_type = + getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle); + + // return only full bounds whenever finding out of map + if (current_collision_type == CollisionType::OUT_OF_SIGHT) { + const auto full_bounds = Bounds{ + -max_search_lane_width, max_search_lane_width, CollisionType::OUT_OF_SIGHT, + CollisionType::OUT_OF_SIGHT}; + return BoundsCandidates({full_bounds}); + } + + if (has_collision(previous_collision_type)) { + if (!has_collision(current_collision_type)) { // if target_position becomes no collision + if (ds_idx == ds_vec.size() - 1) { + current_right_bound = traversed_dist - ds / 2.0; + latest_right_bound_collision_type = previous_collision_type; + previous_collision_type = current_collision_type; + } + break; + } + } else { + if (has_collision(current_collision_type)) { + if (ds_idx == ds_vec.size() - 1) { + const double left_bound = traversed_dist - ds / 2.0; + bounds_candidate.push_back(Bounds{ + current_right_bound, left_bound, latest_right_bound_collision_type, + current_collision_type}); + previous_collision_type = current_collision_type; + } + break; + } + } + + // if target_position is out of lane + if (traversed_dist >= max_search_lane_width) { + if (!has_collision(previous_collision_type) && ds_idx == ds_vec.size() - 1) { + const double left_bound = traversed_dist - ds / 2.0; + bounds_candidate.push_back(Bounds{ + current_right_bound, left_bound, latest_right_bound_collision_type, + CollisionType::OUT_OF_ROAD}); + } + break; + } + + // go forward with ds + traversed_dist += ds; + previous_collision_type = current_collision_type; } - } else { - if (clearance.get() < min_road_clearance || obj_clearance.get() < min_obj_clearance) { - break; + + if (ds_idx != ds_vec.size() - 1) { + // go back with ds since target_position became empty or road/object + // NOTE: if ds is the last of ds_vec, don't have to go back + traversed_dist -= ds; } } } - return traversed_dist; + + // if empty + // TODO(murooka) sometimes this condition realizes + if (bounds_candidate.empty()) { + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger("getBoundsCandidate"), is_showing_debug_info_, "empty bounds candidate"); + // NOTE: set invalid bounds so that MPT won't be solved + const auto invalid_bounds = + Bounds{-5.0, 5.0, CollisionType::OUT_OF_ROAD, CollisionType::OUT_OF_ROAD}; + bounds_candidate.push_back(invalid_bounds); + } + + return bounds_candidate; } -boost::optional MPTOptimizer::getValidLatError( - const bool enable_avoidance, const ReferencePoint & ref_point, const double initial_value, - const CVMaps & maps, const double min_road_clearance, const double min_obj_clearance, - const std::vector & rough_road_bound, const bool search_expanding_side) const +// 0.drivable, 1.out of drivable area 2.out of road or object +// 0.NO_COLLISION, 1.OUT_OF_SIGHT, 2.OUT_OF_ROAD, 3.OBJECT +CollisionType MPTOptimizer::getCollisionType( + const CVMaps & maps, const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point, + const double traversed_dist, const double bound_angle) const { - constexpr double ds = 0.01; - constexpr double lane_width = 7.5; - int n = static_cast(lane_width / ds); - - double rel_value = initial_value; - for (int i = 0; i < n; ++i) { - rel_value -= ds; - if (rel_value > rough_road_bound[0] || rel_value < rough_road_bound[1]) { - return boost::none; - } - geometry_msgs::msg::Point rel_point; - rel_point.y = rel_value; - geometry_msgs::msg::Pose origin; - origin.position = ref_point.p; - origin.orientation = util::getQuaternionFromYaw(ref_point.yaw); - const auto new_position = util::transformToAbsoluteCoordinate2D(rel_point, origin); - auto clearance = getClearance(maps.clearance_map, new_position, maps.map_info); - auto obj_clearance = getClearance(maps.only_objects_clearance_map, new_position, maps.map_info); - if (!clearance || !obj_clearance) { - return boost::none; + // calculate clearance + const double min_soft_road_clearance = mpt_param_.vehicle_circle_radius + + mpt_param_.soft_clearance_from_road + + mpt_param_.extra_desired_clearance_from_road; + const double min_obj_clearance = mpt_param_.vehicle_circle_radius + + mpt_param_.clearance_from_object + + mpt_param_.soft_clearance_from_road; + + // calculate target position + const geometry_msgs::msg::Point target_pos = tier4_autoware_utils::createPoint( + avoiding_point.position.x + traversed_dist * std::cos(bound_angle), + avoiding_point.position.y + traversed_dist * std::sin(bound_angle), 0.0); + + const auto opt_road_clearance = getClearance(maps.clearance_map, target_pos, maps.map_info); + const auto opt_obj_clearance = + getClearance(maps.only_objects_clearance_map, target_pos, maps.map_info); + + // object has more priority than road, so its condition exists first + if (enable_avoidance && opt_obj_clearance) { + const bool is_obj = opt_obj_clearance.get() < min_obj_clearance; + if (is_obj) { + return CollisionType::OBJECT; } - if (!enable_avoidance) { - obj_clearance.emplace(std::numeric_limits::max()); - } - if (search_expanding_side) { - if (clearance.get() > min_road_clearance && obj_clearance.get() > min_obj_clearance) { - break; - } + } + + if (opt_road_clearance) { + const bool out_of_road = opt_road_clearance.get() < min_soft_road_clearance; + if (out_of_road) { + return CollisionType::OUT_OF_ROAD; } else { - if (clearance.get() < min_road_clearance || obj_clearance.get() < min_obj_clearance) { - break; - } + return CollisionType::NO_COLLISION; } } - return rel_value; + + return CollisionType::OUT_OF_SIGHT; +} + +boost::optional MPTOptimizer::getClearance( + const cv::Mat & clearance_map, const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & map_info) const +{ + const auto image_point = geometry_utils::transformMapToOptionalImage(map_point, map_info); + if (!image_point) { + return boost::none; + } + const float clearance = clearance_map.ptr(static_cast( + image_point.get().y))[static_cast(image_point.get().x)] * + map_info.resolution; + return clearance; } diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 68064d2be7896..a5caef472e665 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -14,32 +14,20 @@ #include "obstacle_avoidance_planner/node.hpp" -#include "obstacle_avoidance_planner/debug.hpp" -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/process_cv.hpp" -#include "obstacle_avoidance_planner/util.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "obstacle_avoidance_planner/cv_utils.hpp" +#include "obstacle_avoidance_planner/debug_visualization.hpp" +#include "obstacle_avoidance_planner/utils.hpp" +#include "rclcpp/time.hpp" +#include "tf2/utils.h" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "std_msgs/msg/bool.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include #include @@ -48,34 +36,189 @@ #include #include +namespace +{ +template +size_t searchExtendedZeroVelocityIndex( + const std::vector & fine_points, const std::vector & vel_points) +{ + const auto opt_zero_vel_idx = tier4_autoware_utils::searchZeroVelocityIndex(vel_points); + const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; + return tier4_autoware_utils::findNearestIndex( + fine_points, vel_points.at(zero_vel_idx).pose.position); +} + +bool isPathShapeChanged( + const geometry_msgs::msg::Pose & ego_pose, + const std::vector & path_points, + const std::unique_ptr> & + prev_path_points, + const double max_mpt_length, const double max_path_shape_change_dist, + const double delta_yaw_threshold) +{ + if (!prev_path_points) { + return false; + } + + // truncate prev points from ego pose to fixed end points + const auto opt_prev_begin_idx = tier4_autoware_utils::findNearestIndex( + *prev_path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); + const size_t prev_begin_idx = opt_prev_begin_idx ? *opt_prev_begin_idx : 0; + const auto truncated_prev_points = + points_utils::clipForwardPoints(*prev_path_points, prev_begin_idx, max_mpt_length); + + // truncate points from ego pose to fixed end points + const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); + const size_t begin_idx = opt_begin_idx ? *opt_begin_idx : 0; + const auto truncated_points = + points_utils::clipForwardPoints(path_points, begin_idx, max_mpt_length); + + // guard for lateral offset + if (truncated_prev_points.size() < 2 || truncated_points.size() < 2) { + return false; + } + + // calculate lateral deviations between truncated path_points and prev_path_points + for (const auto & prev_point : truncated_prev_points) { + const double dist = + tier4_autoware_utils::calcLateralOffset(truncated_points, prev_point.pose.position); + if (dist > max_path_shape_change_dist) { + return true; + } + } + + return false; +} + +bool isPathGoalChanged( + const double current_vel, + const std::vector & path_points, + const std::unique_ptr> & + prev_path_points) +{ + if (!prev_path_points) { + return false; + } + + constexpr double min_vel = 1e-3; + if (std::abs(current_vel) > min_vel) { + return false; + } + + // NOTE: Path may be cropped and does not contain the goal. + // Therefore we set a large value to distance threshold. + constexpr double max_goal_moving_dist = 1.0; + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(path_points.back(), prev_path_points->back()); + if (goal_moving_dist < max_goal_moving_dist) { + return false; + } + + return true; +} + +bool hasValidNearestPointFromEgo( + const geometry_msgs::msg::Pose & ego_pose, const Trajectories & trajs, + const TrajectoryParam & traj_param) +{ + const auto traj = trajs.model_predictive_trajectory; + const auto interpolated_points = + interpolation_utils::getInterpolatedPoints(traj, traj_param.delta_arc_length_for_trajectory); + + const auto interpolated_poses_with_yaw = + points_utils::convertToPosesWithYawEstimation(interpolated_points); + const auto opt_nearest_idx = tier4_autoware_utils::findNearestIndex( + interpolated_poses_with_yaw, ego_pose, traj_param.delta_dist_threshold_for_closest_point, + traj_param.delta_yaw_threshold_for_closest_point); + + if (!opt_nearest_idx) { + return false; + } + return true; +} + +std::tuple> calcVehicleCirclesInfo( + const VehicleParam & vehicle_param, const size_t circle_num_for_constraints, + const size_t circle_num_for_radius, const double radius_ratio) +{ + const double radius = std::hypot( + vehicle_param.length / static_cast(circle_num_for_radius) / 2.0, + vehicle_param.width / 2.0) * + radius_ratio; + + std::vector longitudinal_offsets; + const double unit_lon_length = vehicle_param.length / static_cast(circle_num_for_radius); + for (size_t i = 0; i < circle_num_for_constraints; ++i) { + longitudinal_offsets.push_back( + unit_lon_length / 2.0 + + (unit_lon_length * (circle_num_for_radius - 1)) / + static_cast(circle_num_for_constraints - 1) * i - + vehicle_param.rear_overhang); + } + + return {radius, longitudinal_offsets}; +} + +[[maybe_unused]] void fillYawInTrajectoryPoint( + std::vector & traj_points) +{ + std::vector points; + for (const auto & traj_point : traj_points) { + points.push_back(traj_point.pose.position); + } + const auto yaw_vec = interpolation::slerpYawFromPoints(points); + + for (size_t i = 0; i < traj_points.size(); ++i) { + traj_points.at(i).pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i)); + } +} + +} // namespace + ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) -: Node("obstacle_avoidance_planner", node_options), min_num_points_for_getting_yaw_(2) +: Node("obstacle_avoidance_planner", node_options), + logger_ros_clock_(RCL_ROS_TIME), + eb_solved_count_(0) { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); - tf_buffer_ptr_ = std::make_unique(clock); - tf_listener_ptr_ = std::make_unique(*tf_buffer_ptr_); + // qos rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); - trajectory_pub_ = - create_publisher("~/output/path", 1); - avoiding_traj_pub_ = create_publisher( - "/planning/scenario_planning/lane_driving/obstacle_avoidance_candidate_trajectory", - durable_qos); - debug_smoothed_points_pub_ = create_publisher( - "~/debug/smoothed_points", durable_qos); - is_avoidance_possible_pub_ = create_publisher( - "/planning/scenario_planning/lane_driving/obstacle_avoidance_ready", durable_qos); + // publisher to other nodes + traj_pub_ = create_publisher("~/output/path", 1); + + // debug publisher + debug_eb_traj_pub_ = create_publisher( + "~/debug/eb_trajectory", durable_qos); + debug_extended_fixed_traj_pub_ = create_publisher( + "~/debug/extended_fixed_traj", 1); + debug_extended_non_fixed_traj_pub_ = + create_publisher( + "~/debug/extended_non_fixed_traj", 1); + debug_mpt_fixed_traj_pub_ = + create_publisher("~/debug/mpt_fixed_traj", 1); + debug_mpt_ref_traj_pub_ = + create_publisher("~/debug/mpt_ref_traj", 1); + debug_mpt_traj_pub_ = + create_publisher("~/debug/mpt_traj", 1); debug_markers_pub_ = create_publisher("~/debug/marker", durable_qos); + debug_wall_markers_pub_ = + create_publisher("~/debug/wall_marker", durable_qos); debug_clearance_map_pub_ = create_publisher("~/debug/clearance_map", durable_qos); debug_object_clearance_map_pub_ = create_publisher("~/debug/object_clearance_map", durable_qos); debug_area_with_objects_pub_ = create_publisher("~/debug/area_with_objects", durable_qos); + debug_msg_pub_ = + create_publisher("~/debug/calculation_time", 1); + // subscriber path_sub_ = create_subscription( "~/input/path", rclcpp::QoS{1}, std::bind(&ObstacleAvoidancePlanner::pathCallback, this, std::placeholders::_1)); @@ -89,213 +232,576 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n "/planning/scenario_planning/lane_driving/obstacle_avoidance_approval", rclcpp::QoS{10}, std::bind(&ObstacleAvoidancePlanner::enableAvoidanceCallback, this, std::placeholders::_1)); - is_publishing_area_with_objects_ = declare_parameter("is_publishing_area_with_objects", false); - is_publishing_clearance_map_ = declare_parameter("is_publishing_clearance_map", false); - is_showing_debug_info_ = declare_parameter("is_showing_debug_info", false); - is_using_vehicle_config_ = declare_parameter("is_using_vehicle_config", false); - is_stopping_if_outside_drivable_area_ = - declare_parameter("is_stopping_if_outside_drivable_area", true); - enable_avoidance_ = declare_parameter("enable_avoidance", true); - - qp_param_ = std::make_unique(); - traj_param_ = std::make_unique(); - constrain_param_ = std::make_unique(); - vehicle_param_ = std::make_unique(); - mpt_param_ = std::make_unique(); - qp_param_->max_iteration = declare_parameter("qp_max_iteration", 10000); - qp_param_->eps_abs = declare_parameter("qp_eps_abs", 1.0e-8); - qp_param_->eps_rel = declare_parameter("qp_eps_rel", 1.0e-11); - qp_param_->eps_abs_for_extending = declare_parameter("qp_eps_abs_for_extending", 1.0e-6); - qp_param_->eps_rel_for_extending = declare_parameter("qp_eps_rel_for_extending", 1.0e-8); - qp_param_->eps_abs_for_visualizing = declare_parameter("qp_eps_abs_for_visualizing", 1.0e-6); - qp_param_->eps_rel_for_visualizing = declare_parameter("qp_eps_rel_for_visualizing", 1.0e-8); - - traj_param_->num_sampling_points = declare_parameter("num_sampling_points", 100); - traj_param_->num_joint_buffer_points = declare_parameter("num_joint_buffer_points", 2); - traj_param_->num_joint_buffer_points_for_extending = - declare_parameter("num_joint_buffer_points_for_extending", 4); - traj_param_->num_offset_for_begin_idx = declare_parameter("num_offset_for_begin_idx", 2); - traj_param_->num_fix_points_for_extending = declare_parameter("num_fix_points_for_extending", 2); - traj_param_->forward_fixing_mpt_distance = declare_parameter("forward_fixing_mpt_distance", 10); - traj_param_->delta_arc_length_for_optimization = - declare_parameter("delta_arc_length_for_optimization", 1.0); - traj_param_->delta_arc_length_for_mpt_points = - declare_parameter("delta_arc_length_for_mpt_points", 1.0); - traj_param_->delta_arc_length_for_trajectory = - declare_parameter("delta_arc_length_for_trajectory", 0.1); - traj_param_->delta_dist_threshold_for_closest_point = - declare_parameter("delta_dist_threshold_for_closest_point", 3.0); - traj_param_->delta_yaw_threshold_for_closest_point = - declare_parameter("delta_yaw_threshold_for_closest_point", 1.0); - traj_param_->delta_yaw_threshold_for_straight = - declare_parameter("delta_yaw_threshold_for_straight", 0.02); - traj_param_->trajectory_length = declare_parameter("trajectory_length", 200); - traj_param_->forward_fixing_distance = declare_parameter("forward_fixing_distance", 10.0); - traj_param_->backward_fixing_distance = declare_parameter("backward_fixing_distance", 5.0); - traj_param_->max_avoiding_ego_velocity_ms = - declare_parameter("max_avoiding_ego_velocity_ms", 6.0); - traj_param_->max_avoiding_objects_velocity_ms = - declare_parameter("max_avoiding_objects_velocity_ms", 0.1); - traj_param_->center_line_width = declare_parameter("center_line_width", 1.7); - traj_param_->acceleration_for_non_deceleration_range = - declare_parameter("acceleration_for_non_deceleration_range", 1.0); - traj_param_->max_dist_for_extending_end_point = - declare_parameter("max_dist_for_extending_end_point", 5.0); - traj_param_->is_avoiding_unknown = declare_parameter("avoiding_object_type.unknown", true); - traj_param_->is_avoiding_car = declare_parameter("avoiding_object_type.car", true); - traj_param_->is_avoiding_truck = declare_parameter("avoiding_object_type.truck", true); - traj_param_->is_avoiding_bus = declare_parameter("avoiding_object_type.bus", true); - traj_param_->is_avoiding_bicycle = declare_parameter("avoiding_object_type.bicycle", true); - traj_param_->is_avoiding_motorbike = declare_parameter("avoiding_object_type.motorbike", true); - traj_param_->is_avoiding_pedestrian = declare_parameter("avoiding_object_type.pedestrian", true); - traj_param_->is_avoiding_animal = declare_parameter("avoiding_object_type.animal", true); - - constrain_param_->is_getting_constraints_close2path_points = - declare_parameter("is_getting_constraints_close2path_points", false); - constrain_param_->clearance_for_straight_line = - declare_parameter("clearance_for_straight_line", 0.05); - constrain_param_->clearance_for_joint = declare_parameter("clearance_for_joint", 0.1); - constrain_param_->range_for_extend_joint = declare_parameter("range_for_extend_joint", 1.6); - constrain_param_->clearance_for_only_smoothing = - declare_parameter("clearance_for_only_smoothing", 0.1); - constrain_param_->clearance_from_object_for_straight = - declare_parameter("clearance_from_object_for_straight", 10.0); - constrain_param_->clearance_from_road = declare_parameter("clearance_from_road", 0.1); - constrain_param_->clearance_from_object = declare_parameter("clearance_from_object", 0.6); - constrain_param_->extra_desired_clearance_from_road = - declare_parameter("extra_desired_clearance_from_road", 0.2); - constrain_param_->min_object_clearance_for_joint = - declare_parameter("min_object_clearance_for_joint", 3.2); - constrain_param_->max_x_constrain_search_range = - declare_parameter("max_x_constrain_search_range", 0.4); - constrain_param_->coef_x_constrain_search_resolution = - declare_parameter("coef_x_constrain_search_resolution", 1.0); - constrain_param_->coef_y_constrain_search_resolution = - declare_parameter("coef_y_constrain_search_resolution", 0.5); - constrain_param_->keep_space_shape_x = declare_parameter("keep_space_shape_x", 3.0); - constrain_param_->keep_space_shape_y = declare_parameter("keep_space_shape_y", 2.0); - constrain_param_->max_lon_space_for_driveable_constraint = - declare_parameter("max_lon_space_for_driveable_constraint", 0.5); - constrain_param_->clearance_for_fixing = 0.0; - - min_delta_dist_for_replan_ = declare_parameter("min_delta_dist_for_replan", 5.0); - min_delta_time_sec_for_replan_ = declare_parameter("min_delta_time_sec_for_replan", 1.0); - distance_for_path_shape_change_detection_ = - declare_parameter("distance_for_path_shape_change_detection", 2.0); - - // vehicle param - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - vehicle_param_->width = vehicle_info.vehicle_width_m; - vehicle_param_->length = vehicle_info.vehicle_length_m; - vehicle_param_->wheelbase = vehicle_info.wheel_base_m; - vehicle_param_->rear_overhang = vehicle_info.rear_overhang_m; - vehicle_param_->front_overhang = vehicle_info.front_overhang_m; - - if (is_using_vehicle_config_) { - double vehicle_width = vehicle_info.vehicle_width_m; - traj_param_->center_line_width = vehicle_width; - constrain_param_->keep_space_shape_y = vehicle_width; - } - constrain_param_->min_object_clearance_for_deceleration = - constrain_param_->clearance_from_object + constrain_param_->keep_space_shape_y * 0.5; - - double max_steer_deg = 0; - max_steer_deg = declare_parameter("max_steer_deg", 30.0); - vehicle_param_->max_steer_rad = max_steer_deg * M_PI / 180.0; - vehicle_param_->steer_tau = declare_parameter("steer_tau", 0.1); - - // mpt param - mpt_param_->is_hard_fixing_terminal_point = - declare_parameter("is_hard_fixing_terminal_point", true); - mpt_param_->num_curvature_sampling_points = declare_parameter("num_curvature_sampling_points", 5); - mpt_param_->base_point_weight = declare_parameter("base_point_weight", 2000.0); - mpt_param_->top_point_weight = declare_parameter("top_point_weight", 1000.0); - mpt_param_->mid_point_weight = declare_parameter("mid_point_weight", 1000.0); - mpt_param_->lat_error_weight = declare_parameter("lat_error_weight", 10.0); - mpt_param_->yaw_error_weight = declare_parameter("yaw_error_weight", 0.0); - mpt_param_->steer_input_weight = declare_parameter("steer_input_weight", 0.1); - mpt_param_->steer_rate_weight = declare_parameter("steer_rate_weight", 100.0); - mpt_param_->steer_acc_weight = declare_parameter("steer_acc_weight", 0.000001); - mpt_param_->terminal_lat_error_weight = declare_parameter("terminal_lat_error_weight", 0.0); - mpt_param_->terminal_yaw_error_weight = declare_parameter("terminal_yaw_error_weight", 100.0); - mpt_param_->terminal_path_lat_error_weight = - declare_parameter("terminal_path_lat_error_weight", 1000.0); - mpt_param_->terminal_path_yaw_error_weight = - declare_parameter("terminal_path_yaw_error_weight", 1000.0); - mpt_param_->zero_ff_steer_angle = declare_parameter("zero_ff_steer_angle", 0.5); - - mpt_param_->clearance_from_road = vehicle_param_->width * 0.5 + - constrain_param_->clearance_from_road + - constrain_param_->extra_desired_clearance_from_road; - mpt_param_->clearance_from_object = - vehicle_param_->width * 0.5 + constrain_param_->clearance_from_object; - mpt_param_->base_point_dist_from_base_link = 0; - mpt_param_->top_point_dist_from_base_link = - (vehicle_param_->length - vehicle_param_->rear_overhang); - mpt_param_->mid_point_dist_from_base_link = - (mpt_param_->base_point_dist_from_base_link + mpt_param_->top_point_dist_from_base_link) * 0.5; - - in_objects_ptr_ = std::make_unique(); + { // vehicle param + vehicle_param_ = VehicleParam{}; + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_param_.width = vehicle_info.vehicle_width_m; + vehicle_param_.length = vehicle_info.vehicle_length_m; + vehicle_param_.wheelbase = vehicle_info.wheel_base_m; + vehicle_param_.rear_overhang = vehicle_info.rear_overhang_m; + vehicle_param_.front_overhang = vehicle_info.front_overhang_m; + } + + { // option parameter + is_publishing_debug_visualization_marker_ = + declare_parameter("option.is_publishing_debug_visualization_marker"); + is_publishing_clearance_map_ = declare_parameter("option.is_publishing_clearance_map"); + is_publishing_object_clearance_map_ = + declare_parameter("option.is_publishing_object_clearance_map"); + is_publishing_area_with_objects_ = + declare_parameter("option.is_publishing_area_with_objects"); + + is_showing_debug_info_ = declare_parameter("option.is_showing_debug_info"); + is_showing_calculation_time_ = declare_parameter("option.is_showing_calculation_time"); + is_stopping_if_outside_drivable_area_ = + declare_parameter("option.is_stopping_if_outside_drivable_area"); + + // drivability check + use_vehicle_circles_for_drivability_ = + declare_parameter("advanced.option.drivability_check.use_vehicle_circles"); + if (use_vehicle_circles_for_drivability_) { + // vehicle_circles + // NOTE: Vehicle shape for drivability check is considered as a set of circles + use_manual_vehicle_circles_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.use_manual_vehicle_circles"); + vehicle_circle_constraints_num_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.num_for_constraints"); + if (use_manual_vehicle_circles_for_drivability_) { // vehicle circles are designated manually + vehicle_circle_longitudinal_offsets_for_drivability_ = + declare_parameter>( + "advanced.option.drivability_check.vehicle_circles.longitudinal_offsets"); + vehicle_circle_radius_for_drivability_ = + declare_parameter("advanced.option.drivability_check.vehicle_circles.radius"); + } else { // vehicle circles are calculated automatically with designated ratio + const int default_radius_num = + std::round(vehicle_param_.length / vehicle_param_.width * 1.5); + + vehicle_circle_radius_num_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.num_for_radius", default_radius_num); + vehicle_circle_radius_ratio_for_drivability_ = declare_parameter( + "advanced.option.drivability_check.vehicle_circles.radius_ratio"); + + std::tie( + vehicle_circle_radius_for_drivability_, + vehicle_circle_longitudinal_offsets_for_drivability_) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_drivability_, + vehicle_circle_radius_num_for_drivability_, + vehicle_circle_radius_ratio_for_drivability_); + } + } + + enable_avoidance_ = declare_parameter("option.enable_avoidance"); + enable_pre_smoothing_ = declare_parameter("option.enable_pre_smoothing"); + skip_optimization_ = declare_parameter("option.skip_optimization"); + reset_prev_optimization_ = declare_parameter("option.reset_prev_optimization"); + } + + { // trajectory parameter + traj_param_ = TrajectoryParam{}; + + // common + traj_param_.num_sampling_points = declare_parameter("common.num_sampling_points"); + traj_param_.trajectory_length = declare_parameter("common.trajectory_length"); + traj_param_.forward_fixing_min_distance = + declare_parameter("common.forward_fixing_min_distance"); + traj_param_.forward_fixing_min_time = + declare_parameter("common.forward_fixing_min_time"); + traj_param_.backward_fixing_distance = + declare_parameter("common.backward_fixing_distance"); + traj_param_.delta_arc_length_for_trajectory = + declare_parameter("common.delta_arc_length_for_trajectory"); + + traj_param_.delta_dist_threshold_for_closest_point = + declare_parameter("common.delta_dist_threshold_for_closest_point"); + traj_param_.delta_yaw_threshold_for_closest_point = + declare_parameter("common.delta_yaw_threshold_for_closest_point"); + traj_param_.delta_yaw_threshold_for_straight = + declare_parameter("common.delta_yaw_threshold_for_straight"); + + traj_param_.num_fix_points_for_extending = + declare_parameter("common.num_fix_points_for_extending"); + traj_param_.max_dist_for_extending_end_point = + declare_parameter("common.max_dist_for_extending_end_point"); + + // object + traj_param_.max_avoiding_ego_velocity_ms = + declare_parameter("object.max_avoiding_ego_velocity_ms"); + traj_param_.max_avoiding_objects_velocity_ms = + declare_parameter("object.max_avoiding_objects_velocity_ms"); + traj_param_.is_avoiding_unknown = + declare_parameter("object.avoiding_object_type.unknown", true); + traj_param_.is_avoiding_car = declare_parameter("object.avoiding_object_type.car", true); + traj_param_.is_avoiding_truck = + declare_parameter("object.avoiding_object_type.truck", true); + traj_param_.is_avoiding_bus = declare_parameter("object.avoiding_object_type.bus", true); + traj_param_.is_avoiding_bicycle = + declare_parameter("object.avoiding_object_type.bicycle", true); + traj_param_.is_avoiding_motorbike = + declare_parameter("object.avoiding_object_type.motorbike", true); + traj_param_.is_avoiding_pedestrian = + declare_parameter("object.avoiding_object_type.pedestrian", true); + traj_param_.is_avoiding_animal = + declare_parameter("object.avoiding_object_type.animal", true); + } + + { // elastic band parameter + eb_param_ = EBParam{}; + + // common + eb_param_.num_joint_buffer_points = + declare_parameter("advanced.eb.common.num_joint_buffer_points"); + eb_param_.num_offset_for_begin_idx = + declare_parameter("advanced.eb.common.num_offset_for_begin_idx"); + eb_param_.delta_arc_length_for_eb = + declare_parameter("advanced.eb.common.delta_arc_length_for_eb"); + eb_param_.num_sampling_points_for_eb = + declare_parameter("advanced.eb.common.num_sampling_points_for_eb"); + + // clearance + eb_param_.clearance_for_straight_line = + declare_parameter("advanced.eb.clearance.clearance_for_straight_line"); + eb_param_.clearance_for_joint = + declare_parameter("advanced.eb.clearance.clearance_for_joint"); + eb_param_.clearance_for_only_smoothing = + declare_parameter("advanced.eb.clearance.clearance_for_only_smoothing"); + + // qp + eb_param_.qp_param.max_iteration = declare_parameter("advanced.eb.qp.max_iteration"); + eb_param_.qp_param.eps_abs = declare_parameter("advanced.eb.qp.eps_abs"); + eb_param_.qp_param.eps_rel = declare_parameter("advanced.eb.qp.eps_rel"); + + // other + eb_param_.clearance_for_fixing = 0.0; + } + + { // mpt param + mpt_param_ = MPTParam{}; + + // option + // TODO(murooka) implement plan_from_ego + mpt_param_.plan_from_ego = false; + // mpt_param_.plan_from_ego = declare_parameter("mpt.option.plan_from_ego"); + mpt_param_.steer_limit_constraint = + declare_parameter("mpt.option.steer_limit_constraint"); + mpt_param_.fix_points_around_ego = declare_parameter("mpt.option.fix_points_around_ego"); + mpt_param_.enable_warm_start = declare_parameter("mpt.option.enable_warm_start"); + mpt_param_.enable_manual_warm_start = + declare_parameter("mpt.option.enable_manual_warm_start"); + mpt_visualize_sampling_num_ = declare_parameter("mpt.option.visualize_sampling_num"); + + // common + mpt_param_.num_curvature_sampling_points = + declare_parameter("mpt.common.num_curvature_sampling_points"); + + mpt_param_.delta_arc_length_for_mpt_points = + declare_parameter("mpt.common.delta_arc_length_for_mpt_points"); + + // kinematics + mpt_param_.max_steer_rad = + declare_parameter("mpt.kinematics.max_steer_deg") * M_PI / 180.0; + + // By default, optimization_center_offset will be vehicle_info.wheel_base * 0.8 + // The 0.8 scale is adopted as it performed the best. + constexpr double default_wheelbase_ratio = 0.8; + mpt_param_.optimization_center_offset = declare_parameter( + "mpt.kinematics.optimization_center_offset", + vehicle_param_.wheelbase * default_wheelbase_ratio); + + // collision free constraints + mpt_param_.l_inf_norm = + declare_parameter("advanced.mpt.collision_free_constraints.option.l_inf_norm"); + mpt_param_.soft_constraint = + declare_parameter("advanced.mpt.collision_free_constraints.option.soft_constraint"); + mpt_param_.hard_constraint = + declare_parameter("advanced.mpt.collision_free_constraints.option.hard_constraint"); + // TODO(murooka) implement two-step soft constraint + mpt_param_.two_step_soft_constraint = false; + // mpt_param_.two_step_soft_constraint = + // declare_parameter("advanced.mpt.collision_free_constraints.option.two_step_soft_constraint"); + + // vehicle_circles + // NOTE: Vehicle shape for collision free constraints is considered as a set of circles + use_manual_vehicle_circles_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.use_manual_vehicle_circles"); + vehicle_circle_constraints_num_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_constraints"); + if (use_manual_vehicle_circles_for_mpt_) { // vehicle circles are designated manually + mpt_param_.vehicle_circle_longitudinal_offsets = declare_parameter>( + "advanced.mpt.collision_free_constraints.vehicle_circles.longitudinal_offsets"); + mpt_param_.vehicle_circle_radius = + declare_parameter("advanced.mpt.collision_free_constraints.vehicle_circles.radius"); + } else { // vehicle circles are calculated automatically with designated ratio + const int default_radius_num = std::round(vehicle_param_.length / vehicle_param_.width * 1.5); + + vehicle_circle_radius_num_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_radius", + default_radius_num); + vehicle_circle_radius_ratio_for_mpt_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.radius_ratio"); + + std::tie(mpt_param_.vehicle_circle_radius, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_mpt_, + vehicle_circle_radius_num_for_mpt_, vehicle_circle_radius_ratio_for_mpt_); + } + + // clearance + mpt_param_.hard_clearance_from_road = + declare_parameter("advanced.mpt.clearance.hard_clearance_from_road"); + mpt_param_.soft_clearance_from_road = + declare_parameter("advanced.mpt.clearance.soft_clearance_from_road"); + mpt_param_.soft_second_clearance_from_road = + declare_parameter("advanced.mpt.clearance.soft_second_clearance_from_road"); + mpt_param_.extra_desired_clearance_from_road = + declare_parameter("advanced.mpt.clearance.extra_desired_clearance_from_road"); + mpt_param_.clearance_from_object = + declare_parameter("advanced.mpt.clearance.clearance_from_object"); + + // weight + mpt_param_.soft_avoidance_weight = + declare_parameter("advanced.mpt.weight.soft_avoidance_weight"); + mpt_param_.soft_second_avoidance_weight = + declare_parameter("advanced.mpt.weight.soft_second_avoidance_weight"); + + mpt_param_.lat_error_weight = declare_parameter("advanced.mpt.weight.lat_error_weight"); + mpt_param_.yaw_error_weight = declare_parameter("advanced.mpt.weight.yaw_error_weight"); + mpt_param_.yaw_error_rate_weight = + declare_parameter("advanced.mpt.weight.yaw_error_rate_weight"); + mpt_param_.steer_input_weight = + declare_parameter("advanced.mpt.weight.steer_input_weight"); + mpt_param_.steer_rate_weight = + declare_parameter("advanced.mpt.weight.steer_rate_weight"); + + mpt_param_.obstacle_avoid_lat_error_weight = + declare_parameter("advanced.mpt.weight.obstacle_avoid_lat_error_weight"); + mpt_param_.obstacle_avoid_yaw_error_weight = + declare_parameter("advanced.mpt.weight.obstacle_avoid_yaw_error_weight"); + mpt_param_.obstacle_avoid_steer_input_weight = + declare_parameter("advanced.mpt.weight.obstacle_avoid_steer_input_weight"); + mpt_param_.near_objects_length = + declare_parameter("advanced.mpt.weight.near_objects_length"); + + mpt_param_.terminal_lat_error_weight = + declare_parameter("advanced.mpt.weight.terminal_lat_error_weight"); + mpt_param_.terminal_yaw_error_weight = + declare_parameter("advanced.mpt.weight.terminal_yaw_error_weight"); + mpt_param_.terminal_path_lat_error_weight = + declare_parameter("advanced.mpt.weight.terminal_path_lat_error_weight"); + mpt_param_.terminal_path_yaw_error_weight = + declare_parameter("advanced.mpt.weight.terminal_path_yaw_error_weight"); + } + + { // replan + max_path_shape_change_dist_for_replan_ = + declare_parameter("replan.max_path_shape_change_dist"); + max_ego_moving_dist_for_replan_ = + declare_parameter("replan.max_ego_moving_dist_for_replan"); + max_delta_time_sec_for_replan_ = + declare_parameter("replan.max_delta_time_sec_for_replan"); + } + + // TODO(murooka) tune this param when avoiding with obstacle_avoidance_planner + traj_param_.center_line_width = vehicle_param_.width; + + objects_ptr_ = std::make_unique(); // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleAvoidancePlanner::paramCallback, this, std::placeholders::_1)); - initialize(); -} + resetPlanning(); -ObstacleAvoidancePlanner::~ObstacleAvoidancePlanner() {} + self_pose_listener_.waitForFirstPose(); +} rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback( const std::vector & parameters) { - auto update_param = [&](const std::string & name, double & v) { - auto it = std::find_if( - parameters.cbegin(), parameters.cend(), - [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); - if (it != parameters.cend()) { - v = it->as_double(); - return true; + using tier4_autoware_utils::updateParam; + + { // option parameter + updateParam( + parameters, "option.is_publishing_debug_visualization_marker", + is_publishing_debug_visualization_marker_); + updateParam( + parameters, "option.is_publishing_clearance_map", is_publishing_clearance_map_); + updateParam( + parameters, "option.is_publishing_object_clearance_map", is_publishing_object_clearance_map_); + updateParam( + parameters, "option.is_publishing_area_with_objects", is_publishing_area_with_objects_); + + updateParam(parameters, "option.is_showing_debug_info", is_showing_debug_info_); + updateParam( + parameters, "option.is_showing_calculation_time", is_showing_calculation_time_); + updateParam( + parameters, "option.is_stopping_if_outside_drivable_area", + is_stopping_if_outside_drivable_area_); + + // drivability check + updateParam( + parameters, "advanced.option.drivability_check.use_vehicle_circles", + use_vehicle_circles_for_drivability_); + if (use_vehicle_circles_for_drivability_) { + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.use_manual_vehicle_circles", + use_manual_vehicle_circles_for_drivability_); + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.num_for_constraints", + vehicle_circle_constraints_num_for_drivability_); + if (use_manual_vehicle_circles_for_drivability_) { + updateParam>( + parameters, "advanced.option.drivability_check.vehicle_circles.longitudinal_offsets", + vehicle_circle_longitudinal_offsets_for_drivability_); + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.radius", + vehicle_circle_radius_for_drivability_); + } else { + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.num_for_radius", + vehicle_circle_radius_num_for_drivability_); + updateParam( + parameters, "advanced.option.drivability_check.vehicle_circles.radius_ratio", + vehicle_circle_radius_ratio_for_drivability_); + + std::tie( + vehicle_circle_radius_for_drivability_, + vehicle_circle_longitudinal_offsets_for_drivability_) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_drivability_, + vehicle_circle_radius_num_for_drivability_, + vehicle_circle_radius_ratio_for_drivability_); + } } - return false; - }; - - // trajectory total/fixing length - update_param("trajectory_length", traj_param_->trajectory_length); - update_param("forward_fixing_distance", traj_param_->forward_fixing_distance); - update_param("backward_fixing_distance", traj_param_->backward_fixing_distance); - - // clearance for unique points - update_param("clearance_for_straight_line", constrain_param_->clearance_for_straight_line); - update_param("clearance_for_joint", constrain_param_->clearance_for_joint); - update_param("clearance_for_only_smoothing", constrain_param_->clearance_for_only_smoothing); - update_param( - "clearance_from_object_for_straight", constrain_param_->clearance_from_object_for_straight); - - // clearance(distance) when generating trajectory - update_param("clearance_from_road", constrain_param_->clearance_from_road); - update_param("clearance_from_object", constrain_param_->clearance_from_object); - update_param("min_object_clearance_for_joint", constrain_param_->min_object_clearance_for_joint); - update_param( - "extra_desired_clearance_from_road", constrain_param_->extra_desired_clearance_from_road); - - // avoiding param - update_param("max_avoiding_objects_velocity_ms", traj_param_->max_avoiding_objects_velocity_ms); - update_param("max_avoiding_ego_velocity_ms", traj_param_->max_avoiding_ego_velocity_ms); - update_param("center_line_width", traj_param_->center_line_width); - update_param( - "acceleration_for_non_deceleration_range", - traj_param_->acceleration_for_non_deceleration_range); - - // mpt param - update_param("base_point_weight", mpt_param_->base_point_weight); - update_param("top_point_weight", mpt_param_->top_point_weight); - update_param("mid_point_weight", mpt_param_->mid_point_weight); - update_param("lat_error_weight", mpt_param_->lat_error_weight); - update_param("yaw_error_weight", mpt_param_->yaw_error_weight); - update_param("steer_input_weight", mpt_param_->steer_input_weight); - update_param("steer_rate_weight", mpt_param_->steer_rate_weight); - update_param("steer_acc_weight", mpt_param_->steer_acc_weight); + + updateParam(parameters, "option.enable_avoidance", enable_avoidance_); + updateParam(parameters, "option.enable_pre_smoothing", enable_pre_smoothing_); + updateParam(parameters, "option.skip_optimization", skip_optimization_); + updateParam(parameters, "option.reset_prev_optimization", reset_prev_optimization_); + } + + { // trajectory parameter + // common + updateParam(parameters, "common.num_sampling_points", traj_param_.num_sampling_points); + updateParam(parameters, "common.trajectory_length", traj_param_.trajectory_length); + updateParam( + parameters, "common.forward_fixing_min_distance", traj_param_.forward_fixing_min_distance); + updateParam( + parameters, "common.forward_fixing_min_time", traj_param_.forward_fixing_min_time); + updateParam( + parameters, "common.backward_fixing_distance", traj_param_.backward_fixing_distance); + updateParam( + parameters, "common.delta_arc_length_for_trajectory", + traj_param_.delta_arc_length_for_trajectory); + + updateParam( + parameters, "common.delta_dist_threshold_for_closest_point", + traj_param_.delta_dist_threshold_for_closest_point); + updateParam( + parameters, "common.delta_yaw_threshold_for_closest_point", + traj_param_.delta_yaw_threshold_for_closest_point); + updateParam( + parameters, "common.delta_yaw_threshold_for_straight", + traj_param_.delta_yaw_threshold_for_straight); + updateParam( + parameters, "common.num_fix_points_for_extending", traj_param_.num_fix_points_for_extending); + updateParam( + parameters, "common.max_dist_for_extending_end_point", + traj_param_.max_dist_for_extending_end_point); + + // object + updateParam( + parameters, "object.max_avoiding_ego_velocity_ms", traj_param_.max_avoiding_ego_velocity_ms); + updateParam( + parameters, "object.max_avoiding_objects_velocity_ms", + traj_param_.max_avoiding_objects_velocity_ms); + updateParam( + parameters, "object.avoiding_object_type.unknown", traj_param_.is_avoiding_unknown); + updateParam(parameters, "object.avoiding_object_type.car", traj_param_.is_avoiding_car); + updateParam( + parameters, "object.avoiding_object_type.truck", traj_param_.is_avoiding_truck); + updateParam(parameters, "object.avoiding_object_type.bus", traj_param_.is_avoiding_bus); + updateParam( + parameters, "object.avoiding_object_type.bicycle", traj_param_.is_avoiding_bicycle); + updateParam( + parameters, "object.avoiding_object_type.motorbike", traj_param_.is_avoiding_motorbike); + updateParam( + parameters, "object.avoiding_object_type.pedestrian", traj_param_.is_avoiding_pedestrian); + updateParam( + parameters, "object.avoiding_object_type.animal", traj_param_.is_avoiding_animal); + } + + { // elastic band parameter + // common + updateParam( + parameters, "advanced.eb.common.num_joint_buffer_points", eb_param_.num_joint_buffer_points); + updateParam( + parameters, "advanced.eb.common.num_offset_for_begin_idx", + eb_param_.num_offset_for_begin_idx); + updateParam( + parameters, "advanced.eb.common.delta_arc_length_for_eb", eb_param_.delta_arc_length_for_eb); + updateParam( + parameters, "advanced.eb.common.num_sampling_points_for_eb", + eb_param_.num_sampling_points_for_eb); + + // clearance + updateParam( + parameters, "advanced.eb.clearance.clearance_for_straight_line", + eb_param_.clearance_for_straight_line); + updateParam( + parameters, "advanced.eb.clearance.clearance_for_joint", eb_param_.clearance_for_joint); + updateParam( + parameters, "advanced.eb.clearance.clearance_for_only_smoothing", + eb_param_.clearance_for_only_smoothing); + + // qp + updateParam(parameters, "advanced.eb.qp.max_iteration", eb_param_.qp_param.max_iteration); + updateParam(parameters, "advanced.eb.qp.eps_abs", eb_param_.qp_param.eps_abs); + updateParam(parameters, "advanced.eb.qp.eps_rel", eb_param_.qp_param.eps_rel); + } + + { // mpt param + // option + // updateParam(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego); + updateParam( + parameters, "mpt.option.steer_limit_constraint", mpt_param_.steer_limit_constraint); + updateParam( + parameters, "mpt.option.fix_points_around_ego", mpt_param_.fix_points_around_ego); + updateParam(parameters, "mpt.option.enable_warm_start", mpt_param_.enable_warm_start); + updateParam( + parameters, "mpt.option.enable_manual_warm_start", mpt_param_.enable_manual_warm_start); + updateParam(parameters, "mpt.option.visualize_sampling_num", mpt_visualize_sampling_num_); + + // common + updateParam( + parameters, "mpt.common.num_curvature_sampling_points", + mpt_param_.num_curvature_sampling_points); + + updateParam( + parameters, "mpt.common.delta_arc_length_for_mpt_points", + mpt_param_.delta_arc_length_for_mpt_points); + + // kinematics + double max_steer_deg = mpt_param_.max_steer_rad * 180.0 / M_PI; + updateParam(parameters, "mpt.kinematics.max_steer_deg", max_steer_deg); + mpt_param_.max_steer_rad = max_steer_deg * M_PI / 180.0; + updateParam( + parameters, "mpt.kinematics.optimization_center_offset", + mpt_param_.optimization_center_offset); + + // collision_free_constraints + updateParam( + parameters, "advanced.mpt.collision_free_constraints.option.l_inf_norm", + mpt_param_.l_inf_norm); + // updateParam( + // parameters, "advanced.mpt.collision_free_constraints.option.two_step_soft_constraint", + // mpt_param_.two_step_soft_constraint); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.option.soft_constraint", + mpt_param_.soft_constraint); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.option.hard_constraint", + mpt_param_.hard_constraint); + + // vehicle_circles + updateParam( + parameters, + "advanced.mpt.collision_free_constraints.vehicle_circles.use_manual_vehicle_circles", + use_manual_vehicle_circles_for_mpt_); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_constraints", + vehicle_circle_constraints_num_for_mpt_); + if (use_manual_vehicle_circles_for_mpt_) { + updateParam>( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.longitudinal_offsets", + mpt_param_.vehicle_circle_longitudinal_offsets); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.radius", + mpt_param_.vehicle_circle_radius); + } else { // vehicle circles are calculated automatically with designated ratio + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.num_for_radius", + vehicle_circle_radius_num_for_mpt_); + updateParam( + parameters, "advanced.mpt.collision_free_constraints.vehicle_circles.radius_ratio", + vehicle_circle_radius_ratio_for_mpt_); + + std::tie(mpt_param_.vehicle_circle_radius, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo( + vehicle_param_, vehicle_circle_constraints_num_for_mpt_, + vehicle_circle_radius_num_for_mpt_, vehicle_circle_radius_ratio_for_mpt_); + } + + // clearance + updateParam( + parameters, "advanced.mpt.clearance.hard_clearance_from_road", + mpt_param_.hard_clearance_from_road); + updateParam( + parameters, "advanced.mpt.clearance.soft_clearance_from_road", + mpt_param_.soft_clearance_from_road); + updateParam( + parameters, "advanced.mpt.clearance.soft_second_clearance_from_road", + mpt_param_.soft_second_clearance_from_road); + updateParam( + parameters, "advanced.mpt.clearance.extra_desired_clearance_from_road", + mpt_param_.extra_desired_clearance_from_road); + updateParam( + parameters, "advanced.mpt.clearance.clearance_from_object", mpt_param_.clearance_from_object); + + // weight + updateParam( + parameters, "advanced.mpt.weight.soft_avoidance_weight", mpt_param_.soft_avoidance_weight); + updateParam( + parameters, "advanced.mpt.weight.soft_second_avoidance_weight", + mpt_param_.soft_second_avoidance_weight); + + updateParam( + parameters, "advanced.mpt.weight.lat_error_weight", mpt_param_.lat_error_weight); + updateParam( + parameters, "advanced.mpt.weight.yaw_error_weight", mpt_param_.yaw_error_weight); + updateParam( + parameters, "advanced.mpt.weight.yaw_error_rate_weight", mpt_param_.yaw_error_rate_weight); + updateParam( + parameters, "advanced.mpt.weight.steer_input_weight", mpt_param_.steer_input_weight); + updateParam( + parameters, "advanced.mpt.weight.steer_rate_weight", mpt_param_.steer_rate_weight); + + updateParam( + parameters, "advanced.mpt.weight.obstacle_avoid_lat_error_weight", + mpt_param_.obstacle_avoid_lat_error_weight); + updateParam( + parameters, "advanced.mpt.weight.obstacle_avoid_yaw_error_weight", + mpt_param_.obstacle_avoid_yaw_error_weight); + updateParam( + parameters, "advanced.mpt.weight.obstacle_avoid_steer_input_weight", + mpt_param_.obstacle_avoid_steer_input_weight); + updateParam( + parameters, "advanced.mpt.weight.near_objects_length", mpt_param_.near_objects_length); + + updateParam( + parameters, "advanced.mpt.weight.terminal_lat_error_weight", + mpt_param_.terminal_lat_error_weight); + updateParam( + parameters, "advanced.mpt.weight.terminal_yaw_error_weight", + mpt_param_.terminal_yaw_error_weight); + updateParam( + parameters, "advanced.mpt.weight.terminal_path_lat_error_weight", + mpt_param_.terminal_path_lat_error_weight); + updateParam( + parameters, "advanced.mpt.weight.terminal_path_yaw_error_weight", + mpt_param_.terminal_path_yaw_error_weight); + } + + { // replan + updateParam( + parameters, "replan.max_path_shape_change_dist", max_path_shape_change_dist_for_replan_); + updateParam( + parameters, "replan.max_ego_moving_dist_for_replan", max_ego_moving_dist_for_replan_); + updateParam( + parameters, "replan.max_delta_time_sec_for_replan", max_delta_time_sec_for_replan_); + } + + resetPlanning(); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -303,21 +809,6 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback return result; } -// ROS callback functions -void ObstacleAvoidancePlanner::pathCallback( - const autoware_auto_planning_msgs::msg::Path::SharedPtr msg) -{ - std::lock_guard lock(mutex_); - current_ego_pose_ptr_ = getCurrentEgoPose(); - if ( - msg->points.empty() || msg->drivable_area.data.empty() || !current_ego_pose_ptr_ || - !current_twist_ptr_) { - return; - } - autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg = generateTrajectory(*msg); - trajectory_pub_->publish(output_trajectory_msg); -} - void ObstacleAvoidancePlanner::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_twist_ptr_ = std::make_unique(); @@ -328,7 +819,7 @@ void ObstacleAvoidancePlanner::odomCallback(const nav_msgs::msg::Odometry::Share void ObstacleAvoidancePlanner::objectsCallback( const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) { - in_objects_ptr_ = std::make_unique(*msg); + objects_ptr_ = std::make_unique(*msg); } void ObstacleAvoidancePlanner::enableAvoidanceCallback( @@ -336,508 +827,630 @@ void ObstacleAvoidancePlanner::enableAvoidanceCallback( { enable_avoidance_ = msg->enable_avoidance; } -// End ROS callback functions -autoware_auto_planning_msgs::msg::Trajectory ObstacleAvoidancePlanner::generateTrajectory( - const autoware_auto_planning_msgs::msg::Path & path) +void ObstacleAvoidancePlanner::resetPlanning() { - auto t_start = std::chrono::high_resolution_clock::now(); + RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Reset planning"); - const auto traj_points = generateOptimizedTrajectory(*current_ego_pose_ptr_, path); + costmap_generator_ptr_ = std::make_unique(); - const auto post_processed_traj = - generatePostProcessedTrajectory(*current_ego_pose_ptr_, path.points, traj_points); + eb_path_optimizer_ptr_ = std::make_unique( + is_showing_debug_info_, traj_param_, eb_param_, vehicle_param_); - auto output = tier4_autoware_utils::convertToTrajectory(post_processed_traj); - output.header = path.header; + mpt_optimizer_ptr_ = + std::make_unique(is_showing_debug_info_, traj_param_, vehicle_param_, mpt_param_); - prev_path_points_ptr_ = - std::make_unique>(path.points); - - auto t_end = std::chrono::high_resolution_clock::now(); - float elapsed_ms = std::chrono::duration(t_end - t_start).count(); - RCLCPP_INFO_EXPRESSION( - get_logger(), is_showing_debug_info_, - "Total time: = %f [ms]\n==========================", elapsed_ms); - return output; + prev_path_points_ptr_ = nullptr; + resetPrevOptimization(); } -std::vector -ObstacleAvoidancePlanner::generateOptimizedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path) +void ObstacleAvoidancePlanner::resetPrevOptimization() { - if (!needReplan( - ego_pose, prev_ego_pose_ptr_, path.points, prev_replanned_time_ptr_, prev_path_points_ptr_, - prev_trajectories_ptr_)) { - return getPrevTrajectory(path.points); - } - prev_ego_pose_ptr_ = std::make_unique(ego_pose); - prev_replanned_time_ptr_ = std::make_unique(this->now()); - - DebugData debug_data; - const auto optional_trajs = eb_path_optimizer_ptr_->generateOptimizedTrajectory( - enable_avoidance_, ego_pose, path, prev_trajectories_ptr_, in_objects_ptr_->objects, - &debug_data); - if (!optional_trajs) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "[Avoidance] Optimization failed, passing previous trajectory"); - const bool is_prev_traj = true; - const auto prev_trajs_inside_area = calcTrajectoryInsideArea( - getPrevTrajs(path.points), path.points, debug_data.clearance_map, path.drivable_area.info, - &debug_data, is_prev_traj); - prev_trajectories_ptr_ = std::make_unique( - makePrevTrajectories(*current_ego_pose_ptr_, path.points, prev_trajs_inside_area.get())); - - const auto prev_traj = util::concatTraj(prev_trajs_inside_area.get()); - publishingDebugData(debug_data, path, prev_traj, *vehicle_param_); - return prev_traj; - } - - const auto trajs_inside_area = getTrajectoryInsideArea( - optional_trajs.get(), path.points, debug_data.clearance_map, path.drivable_area.info, - &debug_data); - - prev_trajectories_ptr_ = std::make_unique( - makePrevTrajectories(*current_ego_pose_ptr_, path.points, trajs_inside_area)); - const auto optimized_trajectory = util::concatTraj(trajs_inside_area); - publishingDebugData(debug_data, path, optimized_trajectory, *vehicle_param_); - return optimized_trajectory; + prev_optimal_trajs_ptr_ = nullptr; + eb_solved_count_ = 0; } -void ObstacleAvoidancePlanner::initialize() +void ObstacleAvoidancePlanner::pathCallback( + const autoware_auto_planning_msgs::msg::Path::SharedPtr path_ptr) { - RCLCPP_WARN(get_logger(), "[ObstacleAvoidancePlanner] Resetting"); - eb_path_optimizer_ptr_ = std::make_unique( - is_showing_debug_info_, *qp_param_, *traj_param_, *constrain_param_, *vehicle_param_, - *mpt_param_); - prev_path_points_ptr_ = nullptr; - prev_trajectories_ptr_ = nullptr; -} + stop_watch_.tic(__func__); -std::unique_ptr ObstacleAvoidancePlanner::getCurrentEgoPose() -{ - geometry_msgs::msg::TransformStamped tf_current_pose; - - try { - tf_current_pose = tf_buffer_ptr_->lookupTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(get_logger(), "[ObstacleAvoidancePlanner] %s", ex.what()); - return nullptr; - } - - geometry_msgs::msg::Pose p; - p.orientation = tf_current_pose.transform.rotation; - p.position.x = tf_current_pose.transform.translation.x; - p.position.y = tf_current_pose.transform.translation.y; - p.position.z = tf_current_pose.transform.translation.z; - std::unique_ptr p_ptr = std::make_unique(p); - return p_ptr; + if (path_ptr->points.empty() || path_ptr->drivable_area.data.empty() || !current_twist_ptr_) { + return; + } + + current_ego_pose_ = self_pose_listener_.getCurrentPose()->pose; + debug_data_ptr_ = std::make_shared(); + debug_data_ptr_->init( + is_showing_calculation_time_, mpt_visualize_sampling_num_, current_ego_pose_, + mpt_param_.vehicle_circle_radius, mpt_param_.vehicle_circle_longitudinal_offsets); + + // generate optimized trajectory + const auto optimized_traj_points = generateOptimizedTrajectory(*path_ptr); + + // generate post processed trajectory + const auto post_processed_traj_points = + generatePostProcessedTrajectory(path_ptr->points, optimized_traj_points); + + // convert to output msg type + auto output_traj_msg = tier4_autoware_utils::convertToTrajectory(post_processed_traj_points); + output_traj_msg.header = path_ptr->header; + + // publish debug data + publishDebugDataInMain(*path_ptr); + + { // print and publish debug msg + debug_data_ptr_->msg_stream << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n" + << "========================================"; + tier4_debug_msgs::msg::StringStamped debug_msg_msg; + debug_msg_msg.stamp = get_clock()->now(); + debug_msg_msg.data = debug_data_ptr_->msg_stream.getString(); + debug_msg_pub_->publish(debug_msg_msg); + } + + // make previous variables + prev_path_points_ptr_ = + std::make_unique>(path_ptr->points); + prev_ego_pose_ptr_ = std::make_unique(current_ego_pose_); + + traj_pub_->publish(output_traj_msg); } std::vector -ObstacleAvoidancePlanner::generatePostProcessedTrajectory( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::vector & optimized_points) const +ObstacleAvoidancePlanner::generateOptimizedTrajectory( + const autoware_auto_planning_msgs::msg::Path & path) { - auto t_start = std::chrono::high_resolution_clock::now(); + stop_watch_.tic(__func__); - std::vector trajectory_points; - if (path_points.empty()) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; - tmp_point.pose = ego_pose; - tmp_point.longitudinal_velocity_mps = 0; - trajectory_points.push_back(tmp_point); - return trajectory_points; + if (reset_prev_optimization_) { + resetPrevOptimization(); } - if (optimized_points.empty()) { - trajectory_points = util::convertPathToTrajectory(path_points); - return trajectory_points; + + // return prev trajectory if replan is not required + if (!checkReplan(path.points)) { + if (prev_optimal_trajs_ptr_) { + return prev_optimal_trajs_ptr_->model_predictive_trajectory; + } + + return points_utils::convertToTrajectoryPoints(path.points); } - trajectory_points = convertPointsToTrajectory(path_points, optimized_points); + latest_replanned_time_ptr_ = std::make_unique(this->now()); + + // create clearance maps + const CVMaps cv_maps = costmap_generator_ptr_->getMaps( + enable_avoidance_, path, objects_ptr_->objects, traj_param_, debug_data_ptr_); + + // calculate trajectory with EB and MPT + auto optimal_trajs = optimizeTrajectory(path, cv_maps); + + // insert 0 velocity when trajectory is over drivable area + if (is_stopping_if_outside_drivable_area_) { + insertZeroVelocityOutsideDrivableArea(optimal_trajs.model_predictive_trajectory, cv_maps); + } + + publishDebugDataInOptimization(path, optimal_trajs.model_predictive_trajectory); - auto t_end = std::chrono::high_resolution_clock::now(); - float elapsed_ms = std::chrono::duration(t_end - t_start).count(); - RCLCPP_INFO_EXPRESSION( - get_logger(), is_showing_debug_info_, "Post processing time: = %f [ms]", elapsed_ms); + // make previous trajectories + prev_optimal_trajs_ptr_ = + std::make_unique(makePrevTrajectories(path.points, optimal_trajs)); - return trajectory_points; + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return optimal_trajs.model_predictive_trajectory; } -bool ObstacleAvoidancePlanner::needReplan( - const geometry_msgs::msg::Pose & ego_pose, - const std::unique_ptr & prev_ego_pose, - const std::vector & path_points, - const std::unique_ptr & prev_replanned_time, - const std::unique_ptr> & - prev_path_points, - std::unique_ptr & prev_trajs) +// check if optimization is required or not. +// NOTE: previous trajectories information will be reset as well in some cases. +bool ObstacleAvoidancePlanner::checkReplan( + const std::vector & path_points) { - if (!prev_ego_pose || !prev_replanned_time || !prev_path_points || !prev_trajs) { + if ( + !prev_ego_pose_ptr_ || !latest_replanned_time_ptr_ || !prev_path_points_ptr_ || + !prev_optimal_trajs_ptr_) { + return true; + } + + const double max_mpt_length = + traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; + if (isPathShapeChanged( + current_ego_pose_, path_points, prev_path_points_ptr_, max_mpt_length, + max_path_shape_change_dist_for_replan_, + traj_param_.delta_yaw_threshold_for_closest_point)) { + RCLCPP_INFO(get_logger(), "Replan since path shape was changed."); return true; } - if (isPathShapeChanged(ego_pose, path_points, prev_path_points)) { - RCLCPP_INFO(get_logger(), "[Avoidance] Path shape is changed, reset prev trajs"); - prev_trajs = nullptr; + if (isPathGoalChanged(current_twist_ptr_->twist.linear.x, path_points, prev_path_points_ptr_)) { + RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed."); + resetPrevOptimization(); return true; } - if (!util::hasValidNearestPointFromEgo( - *current_ego_pose_ptr_, *prev_trajectories_ptr_, *traj_param_)) { + // For when ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(current_ego_pose_.position, prev_ego_pose_ptr_->position); + if (delta_dist > max_ego_moving_dist_for_replan_) { RCLCPP_INFO( - get_logger(), "[Avoidance] Could not find valid nearest point from ego, reset prev trajs"); - prev_trajs = nullptr; + get_logger(), + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + resetPrevOptimization(); return true; } - const double delta_dist = util::calculate2DDistance(ego_pose.position, prev_ego_pose->position); - if (delta_dist > min_delta_dist_for_replan_) { + // For when ego pose moves far from trajectory + if (!hasValidNearestPointFromEgo(current_ego_pose_, *prev_optimal_trajs_ptr_, traj_param_)) { + RCLCPP_INFO( + get_logger(), + "Replan with resetting optimization since valid nearest trajectory point from ego was not " + "found."); + resetPrevOptimization(); return true; } - rclcpp::Duration delta_time = this->now() - *prev_replanned_time; - const double delta_time_sec = delta_time.seconds(); - if (delta_time_sec > min_delta_time_sec_for_replan_) { + const double delta_time_sec = (this->now() - *latest_replanned_time_ptr_).seconds(); + if (delta_time_sec > max_delta_time_sec_for_replan_) { return true; } return false; } -bool ObstacleAvoidancePlanner::isPathShapeChanged( - const geometry_msgs::msg::Pose & ego_pose, - const std::vector & path_points, - const std::unique_ptr> & - prev_path_points) +Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( + const autoware_auto_planning_msgs::msg::Path & path, const CVMaps & cv_maps) { - if (!prev_path_points) { - return true; + stop_watch_.tic(__func__); + + if (skip_optimization_) { + const auto traj = points_utils::convertToTrajectoryPoints(path.points); + Trajectories trajs; + trajs.smoothed_trajectory = traj; + trajs.model_predictive_trajectory = traj; + return trajs; } - const int default_nearest_prev_path_idx = 0; - const int nearest_prev_path_idx = util::getNearestIdx( - *prev_path_points, ego_pose, default_nearest_prev_path_idx, - traj_param_->delta_yaw_threshold_for_closest_point); - const int default_nearest_path_idx = 0; - const int nearest_path_idx = util::getNearestIdx( - path_points, ego_pose, default_nearest_path_idx, - traj_param_->delta_yaw_threshold_for_closest_point); - const auto prev_first = prev_path_points->begin() + nearest_prev_path_idx; - const auto prev_last = prev_path_points->end(); - std::vector truncated_prev_points( - prev_first, prev_last); + // EB: smooth trajectory if enable_pre_smoothing is true + const auto eb_traj = + [&]() -> boost::optional> { + if (enable_pre_smoothing_) { + return eb_path_optimizer_ptr_->getEBTrajectory( + current_ego_pose_, path, prev_optimal_trajs_ptr_, current_twist_ptr_->twist.linear.x, + debug_data_ptr_); + } + return points_utils::convertToTrajectoryPoints(path.points); + }(); + if (!eb_traj) { + return getPrevTrajs(path.points); + } - const auto first = path_points.begin() + nearest_path_idx; - const auto last = path_points.end(); - std::vector truncated_points(first, last); + // EB has to be solved twice before solving MPT with fixed points + // since the result of EB is likely to change with/without fixing (1st/2nd EB) + // that makes MPT fixing points worse. + if (eb_solved_count_ < 2) { + eb_solved_count_++; - for (const auto & prev_point : truncated_prev_points) { - double min_dist = std::numeric_limits::max(); - for (const auto & point : truncated_points) { - const double dist = util::calculate2DDistance(point.pose.position, prev_point.pose.position); - if (dist < min_dist) { - min_dist = dist; - } - } - if (min_dist > distance_for_path_shape_change_detection_) { - return true; + if (prev_optimal_trajs_ptr_) { + prev_optimal_trajs_ptr_->model_predictive_trajectory.clear(); + prev_optimal_trajs_ptr_->mpt_ref_points.clear(); } } - return false; -} - -std::vector -ObstacleAvoidancePlanner::convertPointsToTrajectory( - const std::vector & path_points, - const std::vector & trajectory_points) const -{ - std::vector interpolated_points = - util::getInterpolatedPoints(trajectory_points, traj_param_->delta_arc_length_for_trajectory); - // add discarded point in the process of interpolation - interpolated_points.push_back(trajectory_points.back().pose.position); - if (static_cast(interpolated_points.size()) < min_num_points_for_getting_yaw_) { - return util::convertPathToTrajectory(path_points); - } - std::vector candidate_points = interpolated_points; - geometry_msgs::msg::Pose last_pose; - last_pose.position = candidate_points.back(); - last_pose.orientation = util::getQuaternionFromPoints( - candidate_points.back(), candidate_points[candidate_points.size() - 2]); - const auto extended_point_opt = util::getLastExtendedPoint( - path_points.back(), last_pose, traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_point_opt) { - candidate_points.push_back(extended_point_opt.get()); - } - - const int zero_velocity_idx = util::getZeroVelocityIdx( - is_showing_debug_info_, candidate_points, path_points, prev_trajectories_ptr_, *traj_param_); - - auto traj_points = util::convertPointsToTrajectoryPointsWithYaw(candidate_points); - traj_points = util::fillTrajectoryWithVelocity(traj_points, 1e4); - if (prev_trajectories_ptr_) { - const int max_skip_comparison_velocity_idx_for_optimized_points = - calculateNonDecelerationRange(traj_points, *current_ego_pose_ptr_, current_twist_ptr_->twist); - const auto optimized_trajectory = util::concatTraj(*prev_trajectories_ptr_); - traj_points = util::alignVelocityWithPoints( - traj_points, optimized_trajectory, zero_velocity_idx, - max_skip_comparison_velocity_idx_for_optimized_points); - } - const int max_skip_comparison_idx_for_path_points = -1; - traj_points = util::alignVelocityWithPoints( - traj_points, path_points, zero_velocity_idx, max_skip_comparison_idx_for_path_points); - - return traj_points; -} -void ObstacleAvoidancePlanner::publishingDebugData( - const DebugData & debug_data, const autoware_auto_planning_msgs::msg::Path & path, - const std::vector & traj_points, - const VehicleParam & vehicle_param) -{ - auto traj = tier4_autoware_utils::convertToTrajectory(debug_data.foa_data.avoiding_traj_points); - traj.header = path.header; - avoiding_traj_pub_->publish(traj); - - auto debug_smoothed_points = - tier4_autoware_utils::convertToTrajectory(debug_data.smoothed_points); - debug_smoothed_points.header = path.header; - debug_smoothed_points_pub_->publish(debug_smoothed_points); - - tier4_planning_msgs::msg::IsAvoidancePossible is_avoidance_possible; - is_avoidance_possible.is_avoidance_possible = debug_data.foa_data.is_avoidance_possible; - is_avoidance_possible_pub_->publish(is_avoidance_possible); - - std::vector traj_points_debug = traj_points; - // Add z information for virtual wall - if (!traj_points_debug.empty()) { - const int idx = util::getNearestIdx( - path.points, traj_points.back().pose, 0, traj_param_->delta_yaw_threshold_for_closest_point); - traj_points_debug.back().pose.position.z = path.points.at(idx).pose.position.z + 1.0; - } - - debug_markers_pub_->publish( - getDebugVisualizationMarker(debug_data, traj_points_debug, vehicle_param)); - if (is_publishing_area_with_objects_) { - debug_area_with_objects_pub_->publish( - getDebugCostmap(debug_data.area_with_objects_map, path.drivable_area)); - } - if (is_publishing_clearance_map_) { - debug_clearance_map_pub_->publish( - getDebugCostmap(debug_data.clearance_map, path.drivable_area)); - debug_object_clearance_map_pub_->publish( - getDebugCostmap(debug_data.only_object_clearance_map, path.drivable_area)); + // MPT: optimize trajectory to be kinematically feasible and collision free + const auto mpt_trajs = mpt_optimizer_ptr_->getModelPredictiveTrajectory( + enable_avoidance_, eb_traj.get(), path.points, prev_optimal_trajs_ptr_, cv_maps, + current_ego_pose_, current_twist_ptr_->twist.linear.x, debug_data_ptr_); + if (!mpt_trajs) { + return getPrevTrajs(path.points); } + + // make trajectories, which has all optimized trajectories information + Trajectories trajs; + trajs.smoothed_trajectory = eb_traj.get(); + trajs.mpt_ref_points = mpt_trajs.get().ref_points; + trajs.model_predictive_trajectory = mpt_trajs.get().mpt; + + // debug data + debug_data_ptr_->mpt_traj = mpt_trajs.get().mpt; + debug_data_ptr_->mpt_ref_traj = + points_utils::convertToTrajectoryPoints(mpt_trajs.get().ref_points); + debug_data_ptr_->eb_traj = eb_traj.get(); + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return trajs; } -int ObstacleAvoidancePlanner::calculateNonDecelerationRange( - const std::vector & traj_points, - const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Twist & ego_twist) const +Trajectories ObstacleAvoidancePlanner::getPrevTrajs( + const std::vector & path_points) const { - const int default_idx = 0; - const int nearest_idx = util::getNearestIdx( - traj_points, ego_pose, default_idx, traj_param_->delta_yaw_threshold_for_closest_point); - const double non_decelerating_arc_length = - (ego_twist.linear.x - traj_param_->max_avoiding_ego_velocity_ms) / - traj_param_->acceleration_for_non_deceleration_range; - if (non_decelerating_arc_length < 0 || traj_points.size() < 2) { - return 0; - } - - double accum_arc_length = 0; - for (std::size_t i = nearest_idx + 1; i < traj_points.size(); i++) { - accum_arc_length += - util::calculate2DDistance(traj_points[i].pose.position, traj_points[i - 1].pose.position); - if (accum_arc_length > non_decelerating_arc_length) { - return i; - } + if (prev_optimal_trajs_ptr_) { + return *prev_optimal_trajs_ptr_; } - return 0; -} -Trajectories ObstacleAvoidancePlanner::getTrajectoryInsideArea( - const Trajectories & trajs, - const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data) const -{ - debug_data->current_vehicle_footprints = - util::getVehicleFootprints(trajs.model_predictive_trajectory, *vehicle_param_); - const auto current_trajs_inside_area = - calcTrajectoryInsideArea(trajs, path_points, road_clearance_map, map_info, debug_data); - if (!current_trajs_inside_area) { - RCLCPP_WARN( - get_logger(), - "[Avoidance] Current trajectory is not inside drivable area, passing previous one. " - "Might stop at the end of drivable trajectory."); - auto prev_trajs = getPrevTrajs(path_points); - const bool is_prev_traj = true; - const auto prev_trajs_inside_area = calcTrajectoryInsideArea( - prev_trajs, path_points, road_clearance_map, map_info, debug_data, is_prev_traj); - return prev_trajs_inside_area.get(); - } - return current_trajs_inside_area.get(); + const auto traj = points_utils::convertToTrajectoryPoints(path_points); + Trajectories trajs; + trajs.smoothed_trajectory = traj; + trajs.model_predictive_trajectory = traj; + return trajs; } -boost::optional ObstacleAvoidancePlanner::calcTrajectoryInsideArea( - const Trajectories & trajs, - const std::vector & path_points, - const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info, - DebugData * debug_data, const bool is_prev_traj) const +void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( + std::vector & traj_points, + const CVMaps & cv_maps) { - if (!is_stopping_if_outside_drivable_area_) { - const auto stop_idx = getStopIdx(path_points, trajs, map_info, road_clearance_map, debug_data); - if (stop_idx) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_WARN_THROTTLE( - get_logger(), clock, std::chrono::milliseconds(1000).count(), - "[Avoidance] Expecting over drivable area"); + stop_watch_.tic(__func__); + + const auto & map_info = cv_maps.map_info; + const auto & road_clearance_map = cv_maps.clearance_map; + + const size_t nearest_idx = + tier4_autoware_utils::findNearestIndex(traj_points, current_ego_pose_.position); + + // NOTE: Some end trajectory points will be ignored to check if outside the drivable area + // since these points might be outside drivable area if only end reference points have high + // curvature. + const size_t end_idx = [&]() { + const size_t enough_long_points_num = + static_cast(traj_param_.num_sampling_points * 0.7); + constexpr size_t end_ignored_points_num = 5; + if (traj_points.size() > enough_long_points_num) { + return std::max(static_cast(0), traj_points.size() - end_ignored_points_num); } - return getBaseTrajectory(path_points, trajs); - } - const auto optional_stop_idx = - getStopIdx(path_points, trajs, map_info, road_clearance_map, debug_data); - if (!is_prev_traj && optional_stop_idx) { - return boost::none; - } - - auto tmp_trajs = getBaseTrajectory(path_points, trajs); - if (is_prev_traj) { - tmp_trajs.extended_trajectory = - std::vector{}; - debug_data->is_expected_to_over_drivable_area = true; - } - - if (optional_stop_idx && !prev_trajectories_ptr_) { - if (optional_stop_idx.get() < static_cast(trajs.model_predictive_trajectory.size())) { - tmp_trajs.model_predictive_trajectory = - std::vector{ - trajs.model_predictive_trajectory.begin(), - trajs.model_predictive_trajectory.begin() + optional_stop_idx.get()}; - tmp_trajs.extended_trajectory = - std::vector{}; - debug_data->is_expected_to_over_drivable_area = true; + return traj_points.size(); + }(); + + for (size_t i = nearest_idx; i < end_idx; ++i) { + const auto & traj_point = traj_points.at(i); + + // calculate the first point being outside drivable area + const bool is_outside = [&]() { + if (use_vehicle_circles_for_drivability_) { + return cv_drivable_area_utils::isOutsideDrivableAreaFromCirclesFootprint( + traj_point, road_clearance_map, map_info, + vehicle_circle_longitudinal_offsets_for_drivability_, + vehicle_circle_radius_for_drivability_); + } + return cv_drivable_area_utils::isOutsideDrivableAreaFromRectangleFootprint( + traj_point, road_clearance_map, map_info, vehicle_param_); + }(); + + // only insert zero velocity to the first point outside drivable area + if (is_outside) { + traj_points[i].longitudinal_velocity_mps = 0.0; + debug_data_ptr_->stop_pose_by_drivable_area = traj_points[i].pose; + break; } } - return tmp_trajs; + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; } -Trajectories ObstacleAvoidancePlanner::getPrevTrajs( - const std::vector & path_points) const +void ObstacleAvoidancePlanner::publishDebugDataInOptimization( + const autoware_auto_planning_msgs::msg::Path & path, + const std::vector & traj_points) { - if (!prev_trajectories_ptr_) { - const auto traj = util::convertPathToTrajectory(path_points); - Trajectories trajs; - trajs.smoothed_trajectory = traj; - trajs.model_predictive_trajectory = traj; - return trajs; - } else { - return *prev_trajectories_ptr_; + stop_watch_.tic(__func__); + + { // publish trajectories + auto debug_eb_traj = tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->eb_traj); + debug_eb_traj.header = path.header; + debug_eb_traj_pub_->publish(debug_eb_traj); + + auto debug_mpt_fixed_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_fixed_traj); + debug_mpt_fixed_traj.header = path.header; + debug_mpt_fixed_traj_pub_->publish(debug_mpt_fixed_traj); + + auto debug_mpt_ref_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_ref_traj); + debug_mpt_ref_traj.header = path.header; + debug_mpt_ref_traj_pub_->publish(debug_mpt_ref_traj); + + auto debug_mpt_traj = tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_traj); + debug_mpt_traj.header = path.header; + debug_mpt_traj_pub_->publish(debug_mpt_traj); } -} -std::vector -ObstacleAvoidancePlanner::getPrevTrajectory( - const std::vector & path_points) const -{ - std::vector traj; - const auto & trajs = getPrevTrajs(path_points); - traj.insert( - traj.end(), trajs.model_predictive_trajectory.begin(), trajs.model_predictive_trajectory.end()); - traj.insert(traj.end(), trajs.extended_trajectory.begin(), trajs.extended_trajectory.end()); - return traj; + { // publish markers + if (is_publishing_debug_visualization_marker_) { + stop_watch_.tic("getDebugVisualizationMarker"); + const auto & debug_marker = debug_visualization::getDebugVisualizationMarker( + debug_data_ptr_, traj_points, vehicle_param_, false); + debug_data_ptr_->msg_stream << " getDebugVisualizationMarker:= " + << stop_watch_.toc("getDebugVisualizationMarker") << " [ms]\n"; + + stop_watch_.tic("publishDebugVisualizationMarker"); + debug_markers_pub_->publish(debug_marker); + debug_data_ptr_->msg_stream << " publishDebugVisualizationMarker:= " + << stop_watch_.toc("publishDebugVisualizationMarker") + << " [ms]\n"; + + stop_watch_.tic("getDebugVisualizationWallMarker"); + const auto & debug_wall_marker = + debug_visualization::getDebugVisualizationWallMarker(debug_data_ptr_, vehicle_param_); + debug_data_ptr_->msg_stream << " getDebugVisualizationWallMarker:= " + << stop_watch_.toc("getDebugVisualizationWallMarker") + << " [ms]\n"; + + stop_watch_.tic("publishDebugVisualizationWallMarker"); + debug_wall_markers_pub_->publish(debug_wall_marker); + debug_data_ptr_->msg_stream << " publishDebugVisualizationWallMarker:= " + << stop_watch_.toc("publishDebugVisualizationWallMarker") + << " [ms]\n"; + } + } + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; } Trajectories ObstacleAvoidancePlanner::makePrevTrajectories( - const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, - const Trajectories & trajs) const + const Trajectories & trajs) { + stop_watch_.tic(__func__); + const auto post_processed_smoothed_traj = - generatePostProcessedTrajectory(ego_pose, path_points, trajs.smoothed_trajectory); + generatePostProcessedTrajectory(path_points, trajs.smoothed_trajectory); + + // TODO(murooka) generatePoseProcessedTrajectory may be too large Trajectories trajectories; trajectories.smoothed_trajectory = post_processed_smoothed_traj; trajectories.mpt_ref_points = trajs.mpt_ref_points; trajectories.model_predictive_trajectory = trajs.model_predictive_trajectory; - trajectories.extended_trajectory = trajs.extended_trajectory; + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return trajectories; } -Trajectories ObstacleAvoidancePlanner::getBaseTrajectory( +std::vector +ObstacleAvoidancePlanner::generatePostProcessedTrajectory( const std::vector & path_points, - const Trajectories & trajs) const + const std::vector & optimized_traj_points) { - auto base_trajs = trajs; - if (!trajs.extended_trajectory.empty()) { - const auto extended_point_opt = util::getLastExtendedTrajPoint( - path_points.back(), trajs.extended_trajectory.back().pose, - traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_point_opt) { - base_trajs.extended_trajectory.push_back(extended_point_opt.get()); - } - } else if (!trajs.model_predictive_trajectory.empty()) { - const auto extended_point_opt = util::getLastExtendedTrajPoint( - path_points.back(), trajs.model_predictive_trajectory.back().pose, - traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_point_opt) { - base_trajs.extended_trajectory.push_back(extended_point_opt.get()); - } + stop_watch_.tic(__func__); + + std::vector trajectory_points; + if (path_points.empty()) { + autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; + tmp_point.pose = current_ego_pose_; + tmp_point.longitudinal_velocity_mps = 0.0; + trajectory_points.push_back(tmp_point); + return trajectory_points; } - double prev_velocity = 1e4; - for (auto & p : base_trajs.model_predictive_trajectory) { - if (p.longitudinal_velocity_mps < 1e-6) { - p.longitudinal_velocity_mps = prev_velocity; - } else { - prev_velocity = p.longitudinal_velocity_mps; - } + if (optimized_traj_points.empty()) { + trajectory_points = points_utils::convertToTrajectoryPoints(path_points); + return trajectory_points; } - for (auto & p : base_trajs.extended_trajectory) { - if (p.longitudinal_velocity_mps < 1e-6) { - p.longitudinal_velocity_mps = prev_velocity; - } else { - prev_velocity = p.longitudinal_velocity_mps; + + // calculate extended trajectory that connects to optimized trajectory smoothly + const auto extended_traj_points = getExtendedTrajectory(path_points, optimized_traj_points); + + // concat trajectories + const auto full_traj_points = + points_utils::concatTrajectory(optimized_traj_points, extended_traj_points); + + // NOTE: fine_traj_points has no velocity information + const auto fine_traj_points = generateFineTrajectoryPoints(path_points, full_traj_points); + + const auto fine_traj_points_with_vel = + alignVelocity(fine_traj_points, path_points, full_traj_points); + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return fine_traj_points_with_vel; +} + +std::vector +ObstacleAvoidancePlanner::getExtendedTrajectory( + const std::vector & path_points, + const std::vector & optimized_points) +{ + stop_watch_.tic(__func__); + + assert(!path_points.empty()); + + const double accum_arc_length = tier4_autoware_utils::calcArcLength(optimized_points); + if (accum_arc_length > traj_param_.trajectory_length) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), + "[Avoidance] Not extend trajectory"); + return std::vector{}; + } + + // calculate end idx of optimized points on path points + const auto opt_end_path_idx = tier4_autoware_utils::findNearestIndex( + path_points, optimized_points.back().pose, std::numeric_limits::max(), + traj_param_.delta_yaw_threshold_for_closest_point); + if (!opt_end_path_idx) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), + "[Avoidance] Not extend trajectory since could not find nearest idx from last opt point"); + return std::vector{}; + } + + auto extended_traj_points = + [&]() -> std::vector { + constexpr double non_fixed_traj_length = 5.0; // TODO(murooka) may be better to tune + const size_t non_fixed_begin_path_idx = opt_end_path_idx.get(); + const size_t non_fixed_end_path_idx = + points_utils::findForwardIndex(path_points, non_fixed_begin_path_idx, non_fixed_traj_length); + + if ( + non_fixed_begin_path_idx == path_points.size() - 1 || + non_fixed_begin_path_idx == non_fixed_end_path_idx) { + if (points_utils::isNearLastPathPoint( + optimized_points.back(), path_points, traj_param_.max_dist_for_extending_end_point, + traj_param_.delta_yaw_threshold_for_closest_point)) { + return std::vector{}; + } + const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); + return std::vector{last_traj_point}; + } else if (non_fixed_end_path_idx == path_points.size() - 1) { + // no need to connect smoothly since extended trajectory length is short enough + const auto last_traj_point = points_utils::convertToTrajectoryPoint(path_points.back()); + return std::vector{last_traj_point}; } + + // define non_fixed/fixed_traj_points + const auto begin_point = optimized_points.back(); + const auto end_point = + points_utils::convertToTrajectoryPoint(path_points.at(non_fixed_end_path_idx)); + const std::vector non_fixed_traj_points{ + begin_point, end_point}; + const std::vector fixed_path_points{ + path_points.begin() + non_fixed_end_path_idx + 1, path_points.end()}; + const auto fixed_traj_points = points_utils::convertToTrajectoryPoints(fixed_path_points); + + // spline interpolation to two traj points with end diff constraints + const double begin_yaw = tf2::getYaw(begin_point.pose.orientation); + const double end_yaw = tf2::getYaw(end_point.pose.orientation); + const auto interpolated_non_fixed_traj_points = + interpolation_utils::getConnectedInterpolatedPoints( + non_fixed_traj_points, eb_param_.delta_arc_length_for_eb, begin_yaw, end_yaw); + + // concat interpolated_non_fixed and fixed traj points + auto extended_points = interpolated_non_fixed_traj_points; + extended_points.insert( + extended_points.end(), fixed_traj_points.begin(), fixed_traj_points.end()); + + debug_data_ptr_->extended_non_fixed_traj = interpolated_non_fixed_traj_points; + debug_data_ptr_->extended_fixed_traj = fixed_traj_points; + + return extended_points; + }(); + + // NOTE: Extended points will be concatenated with optimized points. + // Then, minimum velocity of each point is chosen from concatenated points or path points + // since optimized points has zero velocity outside drivable area + constexpr double large_velocity = 1e4; + for (auto & point : extended_traj_points) { + point.longitudinal_velocity_mps = large_velocity; } - return base_trajs; + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return extended_traj_points; +} + +std::vector +ObstacleAvoidancePlanner::generateFineTrajectoryPoints( + const std::vector & path_points, + const std::vector & traj_points) const +{ + stop_watch_.tic(__func__); + + // interpolate x and y + auto interpolated_traj_points = interpolation_utils::getInterpolatedTrajectoryPoints( + traj_points, traj_param_.delta_arc_length_for_trajectory); + + // calculate yaw from x and y + // NOTE: We do not use spline interpolation to yaw in behavior path since the yaw is unstable. + // Currently this implementation is removed since this calculation is heavy (~20ms) + // fillYawInTrajectoryPoint(interpolated_traj_points); + + // compensate last pose + points_utils::compensateLastPose( + path_points.back(), interpolated_traj_points, traj_param_.max_dist_for_extending_end_point, + traj_param_.delta_yaw_threshold_for_closest_point); + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + + return interpolated_traj_points; } -boost::optional ObstacleAvoidancePlanner::getStopIdx( +std::vector +ObstacleAvoidancePlanner::alignVelocity( + const std::vector & fine_traj_points, const std::vector & path_points, - const Trajectories & trajs, const nav_msgs::msg::MapMetaData & map_info, - const cv::Mat & road_clearance_map, DebugData * debug_data) const + const std::vector & traj_points) const { - const int nearest_idx = util::getNearestIdx( - path_points, *current_ego_pose_ptr_, 0, traj_param_->delta_yaw_threshold_for_closest_point); - const double accum_ds = util::getArcLength(path_points, nearest_idx); - - auto target_points = trajs.model_predictive_trajectory; - if (accum_ds < traj_param_->num_sampling_points * traj_param_->delta_arc_length_for_mpt_points) { - target_points.insert( - target_points.end(), trajs.extended_trajectory.begin(), trajs.extended_trajectory.end()); - const auto extended_mpt_point_opt = util::getLastExtendedTrajPoint( - path_points.back(), trajs.model_predictive_trajectory.back().pose, - traj_param_->delta_yaw_threshold_for_closest_point, - traj_param_->max_dist_for_extending_end_point); - if (extended_mpt_point_opt) { - target_points.push_back(extended_mpt_point_opt.get()); + stop_watch_.tic(__func__); + + // search zero velocity index of fine_traj_points + const size_t zero_vel_fine_traj_idx = [&]() { + const size_t zero_vel_path_idx = searchExtendedZeroVelocityIndex(fine_traj_points, path_points); + const size_t zero_vel_traj_idx = + searchExtendedZeroVelocityIndex(fine_traj_points, traj_points); // for outside drivable area + + return std::min(zero_vel_path_idx, zero_vel_traj_idx); + }(); + + auto fine_traj_points_with_vel = fine_traj_points; + size_t prev_begin_idx = 0; + for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { + const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); + + const auto & target_pos = fine_traj_points_with_vel[i].pose.position; + const size_t closest_seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pos); + + // lerp z + fine_traj_points_with_vel[i].pose.position.z = + lerpPoseZ(truncated_points, target_pos, closest_seg_idx); + + // lerp vx + const double target_vel = lerpTwistX(truncated_points, target_pos, closest_seg_idx); + if (i >= zero_vel_fine_traj_idx) { + fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0; + } else if (target_vel < 1e-6) { // NOTE: velocity may be negative due to linear interpolation + const auto prev_idx = std::max(static_cast(i) - 1, 0); + fine_traj_points_with_vel[i].longitudinal_velocity_mps = + fine_traj_points_with_vel[prev_idx].longitudinal_velocity_mps; + } else { + fine_traj_points_with_vel[i].longitudinal_velocity_mps = target_vel; } + + // NOTE: closest_seg_idx is for the clipped trajectory. This operation must be "+=". + prev_begin_idx += closest_seg_idx; + } + + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; + return fine_traj_points_with_vel; +} + +void ObstacleAvoidancePlanner::publishDebugDataInMain( + const autoware_auto_planning_msgs::msg::Path & path) const +{ + stop_watch_.tic(__func__); + + { // publish trajectories + auto debug_extended_fixed_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->extended_fixed_traj); + debug_extended_fixed_traj.header = path.header; + debug_extended_fixed_traj_pub_->publish(debug_extended_fixed_traj); + + auto debug_extended_non_fixed_traj = + tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->extended_non_fixed_traj); + debug_extended_non_fixed_traj.header = path.header; + debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); } - const auto footprints = util::getVehicleFootprints(target_points, *vehicle_param_); - const int debug_nearest_footprint_idx = - util::getNearestIdx(target_points, current_ego_pose_ptr_->position); - std::vector debug_truncated_footprints( - footprints.begin() + debug_nearest_footprint_idx, footprints.end()); - debug_data->vehicle_footprints = debug_truncated_footprints; + { // publish clearance map + stop_watch_.tic("publishClearanceMap"); + + if (is_publishing_area_with_objects_) { // false + debug_area_with_objects_pub_->publish(debug_visualization::getDebugCostmap( + debug_data_ptr_->area_with_objects_map, path.drivable_area)); + } + if (is_publishing_object_clearance_map_) { // false + debug_object_clearance_map_pub_->publish(debug_visualization::getDebugCostmap( + debug_data_ptr_->only_object_clearance_map, path.drivable_area)); + } + if (is_publishing_clearance_map_) { // true + debug_clearance_map_pub_->publish( + debug_visualization::getDebugCostmap(debug_data_ptr_->clearance_map, path.drivable_area)); + } + debug_data_ptr_->msg_stream << " getDebugCostMap * 3:= " + << stop_watch_.toc("publishClearanceMap") << " [ms]\n"; + } - const auto optional_idx = - process_cv::getStopIdx(footprints, *current_ego_pose_ptr_, road_clearance_map, map_info); - return optional_idx; + debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__) + << " [ms]\n"; } -#include +#include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(ObstacleAvoidancePlanner) diff --git a/planning/obstacle_avoidance_planner/src/process_cv.cpp b/planning/obstacle_avoidance_planner/src/process_cv.cpp index 45de56e21804f..f35a9ada665f0 100644 --- a/planning/obstacle_avoidance_planner/src/process_cv.cpp +++ b/planning/obstacle_avoidance_planner/src/process_cv.cpp @@ -14,7 +14,6 @@ #include "obstacle_avoidance_planner/process_cv.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/util.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/util.cpp b/planning/obstacle_avoidance_planner/src/util.cpp deleted file mode 100644 index 527726e5cd801..0000000000000 --- a/planning/obstacle_avoidance_planner/src/util.cpp +++ /dev/null @@ -1,1018 +0,0 @@ -// 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 "obstacle_avoidance_planner/util.hpp" - -#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" - -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include -#include - -namespace util -{ -template -geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const T & point, const geometry_msgs::msg::Pose & origin) -{ - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point relative_p; - const double tmp_x = point.x - origin.position.x; - const double tmp_y = point.y - origin.position.y; - relative_p.x = tmp_x * cos_yaw + tmp_y * sin_yaw; - relative_p.y = -tmp_x * sin_yaw + tmp_y * cos_yaw; - relative_p.z = point.z; - - return relative_p; -} - -template geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const geometry_msgs::msg::Point &, const geometry_msgs::msg::Pose & origin); -template geometry_msgs::msg::Point transformToRelativeCoordinate2D( - const geometry_msgs::msg::Point32 &, const geometry_msgs::msg::Pose & origin); - -geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin) -{ - // NOTE: implement transformation without defining yaw variable - // but directly sin/cos of yaw for fast calculation - const auto & q = origin.orientation; - const double cos_yaw = 1 - 2 * q.z * q.z; - const double sin_yaw = 2 * q.w * q.z; - - geometry_msgs::msg::Point absolute_p; - absolute_p.x = point.x * cos_yaw - point.y * sin_yaw + origin.position.x; - absolute_p.y = point.x * sin_yaw + point.y * cos_yaw + origin.position.y; - absolute_p.z = point.z; - - return absolute_p; -} - -double calculate2DDistance(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) -{ - const double dx = a.x - b.x; - const double dy = a.y - b.y; - return std::sqrt(dx * dx + dy * dy); -} - -double calculateSquaredDistance( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) -{ - const double dx = a.x - b.x; - const double dy = a.y - b.y; - return dx * dx + dy * dy; -} - -double getYawFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) -{ - const double dx = a.x - a_root.x; - const double dy = a.y - a_root.y; - return std::atan2(dy, dx); -} - -geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double yaw) -{ - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - return tf2::toMsg(q); -} - -double normalizeRadian(const double angle) -{ - double n_angle = std::fmod(angle, 2 * M_PI); - n_angle = n_angle > M_PI ? n_angle - 2 * M_PI : n_angle < -M_PI ? 2 * M_PI + n_angle : n_angle; - return n_angle; -} - -geometry_msgs::msg::Quaternion getQuaternionFromPoints( - const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) -{ - const double roll = 0; - const double pitch = 0; - const double yaw = util::getYawFromPoints(a, a_root); - tf2::Quaternion quaternion; - quaternion.setRPY(roll, pitch, yaw); - return tf2::toMsg(quaternion); -} - -template -geometry_msgs::msg::Point transformMapToImage( - const T & map_point, const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - double resolution = occupancy_grid_info.resolution; - double map_y_height = occupancy_grid_info.height; - double map_x_width = occupancy_grid_info.width; - double map_x_in_image_resolution = relative_p.x / resolution; - double map_y_in_image_resolution = relative_p.y / resolution; - geometry_msgs::msg::Point image_point; - image_point.x = map_y_height - map_y_in_image_resolution; - image_point.y = map_x_width - map_x_in_image_resolution; - return image_point; -} -template geometry_msgs::msg::Point transformMapToImage( - const geometry_msgs::msg::Point &, const nav_msgs::msg::MapMetaData & map_info); -template geometry_msgs::msg::Point transformMapToImage( - const geometry_msgs::msg::Point32 &, const nav_msgs::msg::MapMetaData & map_info); - -boost::optional transformMapToOptionalImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - double resolution = occupancy_grid_info.resolution; - double map_y_height = occupancy_grid_info.height; - double map_x_width = occupancy_grid_info.width; - double map_x_in_image_resolution = relative_p.x / resolution; - double map_y_in_image_resolution = relative_p.y / resolution; - double image_x = map_y_height - map_y_in_image_resolution; - double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - geometry_msgs::msg::Point image_point; - image_point.x = image_x; - image_point.y = image_y; - return image_point; - } else { - return boost::none; - } -} - -bool transformMapToImage( - const geometry_msgs::msg::Point & map_point, - const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point) -{ - geometry_msgs::msg::Point relative_p = - transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); - const double map_y_height = occupancy_grid_info.height; - const double map_x_width = occupancy_grid_info.width; - const double scale = 1 / occupancy_grid_info.resolution; - const double map_x_in_image_resolution = relative_p.x * scale; - const double map_y_in_image_resolution = relative_p.y * scale; - const double image_x = map_y_height - map_y_in_image_resolution; - const double image_y = map_x_width - map_x_in_image_resolution; - if ( - image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && - image_y < static_cast(map_x_width)) { - image_point.x = image_x; - image_point.y = image_y; - return true; - } else { - return false; - } -} - -bool interpolate2DPoints( - const std::vector & base_x, const std::vector & base_y, const double resolution, - std::vector & interpolated_points) -{ - if (base_x.empty() || base_y.empty()) { - return false; - } - std::vector base_s = calcEuclidDist(base_x, base_y); - if (base_s.empty() || base_s.size() == 1) { - return false; - } - std::vector new_s; - for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { - new_s.push_back(i); - } - - // spline interpolation - const std::vector interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::slerp(base_s, base_y, new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return false; - } - } - for (size_t i = 0; i < interpolated_x.size(); i++) { - geometry_msgs::msg::Point point; - point.x = interpolated_x[i]; - point.y = interpolated_y[i]; - interpolated_points.push_back(point); - } - return true; -} - -std::vector getInterpolatedPoints( - const std::vector & first_points, - const std::vector & second_points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - std::vector concat_points; - for (const auto point : first_points) { - concat_points.push_back(point.position); - } - for (const auto & point : second_points) { - concat_points.push_back(point.position); - } - - for (std::size_t i = 0; i < concat_points.size(); i++) { - if (i > 0) { - if ( - std::fabs(concat_points[i].x - concat_points[i - 1].x) < 1e-6 && - std::fabs(concat_points[i].y - concat_points[i - 1].y) < 1e-6) { - continue; - } - } - tmp_x.push_back(concat_points[i].x); - tmp_y.push_back(concat_points[i].y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].x - points[i - 1].x) < 1e-6 && - std::fabs(points[i].y - points[i - 1].y) < 1e-6) { - continue; - } - } - tmp_x.push_back(points[i].x); - tmp_y.push_back(points[i].y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].position.x - points[i - 1].position.x) < 1e-6 && - std::fabs(points[i].position.y - points[i - 1].position.y) < 1e-6) { - continue; - } - } - tmp_x.push_back(points[i].position.x); - tmp_y.push_back(points[i].position.y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -std::vector getInterpolatedPoints( - const std::vector & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].p.x - points[i - 1].p.x) < 1e-6 && - std::fabs(points[i].p.y - points[i - 1].p.y) < 1e-6) { - continue; - } - } - tmp_x.push_back(points[i].p.x); - tmp_y.push_back(points[i].p.y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} - -template -std::vector getInterpolatedPoints( - const T & points, const double delta_arc_length) -{ - std::vector tmp_x; - std::vector tmp_y; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - if ( - std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < 1e-6 && - std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < 1e-6) { - continue; - } - } - tmp_x.push_back(points[i].pose.position.x); - tmp_y.push_back(points[i].pose.position.y); - } - std::vector interpolated_points; - util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points); - return interpolated_points; -} -template std::vector -getInterpolatedPoints>( - const std::vector &, const double); -template std::vector -getInterpolatedPoints>( - const std::vector &, const double); - -template -int getNearestIdx( - const T & points, const geometry_msgs::msg::Pose & pose, const int default_idx, - const double delta_yaw_threshold) -{ - double min_dist = std::numeric_limits::max(); - int nearest_idx = default_idx; - const double point_yaw = tf2::getYaw(pose.orientation); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i].pose.position, pose.position); - const double points_yaw = tf2::getYaw(points[i].pose.orientation); - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::Pose &, const int, const double); -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::Pose &, const int, const double); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const int default_idx, const double delta_yaw_threshold, const double delta_dist_threshold) -{ - int nearest_idx = default_idx; - double min_dist = std::numeric_limits::max(); - const double point_yaw = tf2::getYaw(pose.orientation); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i], pose.position); - double points_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; - points_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; - points_yaw = std::atan2(dy, dx); - } - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if ( - dist < min_dist && dist < delta_dist_threshold && - std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} - -int getNearestIdxOverPoint( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, [[maybe_unused]] const int default_idx, - const double delta_yaw_threshold) -{ - double min_dist = std::numeric_limits::max(); - const double point_yaw = tf2::getYaw(pose.orientation); - const double pose_dx = std::cos(point_yaw); - const double pose_dy = std::sin(point_yaw); - int nearest_idx = 0; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - const double dist = util::calculateSquaredDistance(points[i].pose.position, pose.position); - const double points_yaw = - getYawFromPoints(points[i].pose.position, points[i - 1].pose.position); - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - const double dx = points[i].pose.position.x - pose.position.x; - const double dy = points[i].pose.position.y - pose.position.y; - const double ip = dx * pose_dx + dy * pose_dy; - if (dist < min_dist && ip > 0 && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - } - return nearest_idx; -} - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const int default_idx, const double delta_yaw_threshold) -{ - double min_dist = std::numeric_limits::max(); - int nearest_idx = default_idx; - const double point_yaw = tf2::getYaw(pose.orientation); - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - const double dist = calculateSquaredDistance(points[i], pose.position); - const double points_yaw = getYawFromPoints(points[i], points[i - 1]); - const double diff_yaw = points_yaw - point_yaw; - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - min_dist = dist; - nearest_idx = i; - } - } - } - return nearest_idx; -} - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point, const int default_idx) -{ - double min_dist = std::numeric_limits::max(); - int nearest_idx = default_idx; - for (std::size_t i = 0; i < points.size(); i++) { - if (i > 0) { - const double dist = calculateSquaredDistance(points[i - 1].pose.position, point); - const double points_dx = points[i].pose.position.x - points[i - 1].pose.position.x; - const double points_dy = points[i].pose.position.y - points[i - 1].pose.position.y; - const double points2pose_dx = point.x - points[i - 1].pose.position.x; - const double points2pose_dy = point.y - points[i - 1].pose.position.y; - const double ip = points_dx * points2pose_dx + points_dy * points2pose_dy; - if (ip < 0) { - return nearest_idx; - } - if (dist < min_dist && ip > 0) { - min_dist = dist; - nearest_idx = i - 1; - } - } - } - return nearest_idx; -} -template int getNearestIdx>( - const std::vector & points, - const geometry_msgs::msg::Point & point, const int default_idx); -template int getNearestIdx>( - const std::vector & points, - const geometry_msgs::msg::Point & point, const int default_idx); - -int getNearestIdx( - const std::vector & points, const geometry_msgs::msg::Point & point) -{ - int nearest_idx = 0; - double min_dist = std::numeric_limits::max(); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i], point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} - -template -int getNearestIdx(const T & points, const geometry_msgs::msg::Point & point) -{ - int nearest_idx = 0; - double min_dist = std::numeric_limits::max(); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i].pose.position, point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::Point &); -template int getNearestIdx>( - const std::vector &, - const geometry_msgs::msg::Point &); - -int getNearestIdx( - const std::vector & points, const double target_s, const int begin_idx) -{ - double nearest_delta_s = std::numeric_limits::max(); - int nearest_idx = begin_idx; - for (std::size_t i = begin_idx; i < points.size(); i++) { - double diff = std::fabs(target_s - points[i].s); - if (diff < nearest_delta_s) { - nearest_delta_s = diff; - nearest_idx = i; - } - } - return nearest_idx; -} - -template -int getNearestPointIdx(const T & points, const geometry_msgs::msg::Point & point) -{ - int nearest_idx = 0; - double min_dist = std::numeric_limits::max(); - for (std::size_t i = 0; i < points.size(); i++) { - const double dist = calculateSquaredDistance(points[i].p, point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - return nearest_idx; -} -template int getNearestPointIdx>( - const std::vector &, const geometry_msgs::msg::Point &); -template int getNearestPointIdx>( - const std::vector &, const geometry_msgs::msg::Point &); - -std::vector convertPathToTrajectory( - const std::vector & path_points) -{ - std::vector traj_points; - for (const auto & point : path_points) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point; - tmp_point.pose = point.pose; - tmp_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_points.push_back(tmp_point); - } - return traj_points; -} - -std::vector -convertPointsToTrajectoryPointsWithYaw(const std::vector & points) -{ - std::vector traj_points; - for (size_t i = 0; i < points.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose.position = 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); - traj_points.push_back(traj_point); - } - return traj_points; -} - -std::vector fillTrajectoryWithVelocity( - const std::vector & traj_points, - const double velocity) -{ - std::vector traj_with_velocity; - for (const auto & traj_point : traj_points) { - auto tmp_point = traj_point; - tmp_point.longitudinal_velocity_mps = velocity; - traj_with_velocity.push_back(tmp_point); - } - return traj_with_velocity; -} - -template -std::vector alignVelocityWithPoints( - const std::vector & base_traj_points, - const T & points, const int zero_velocity_traj_idx, const int max_skip_comparison_idx) -{ - auto traj_points = base_traj_points; - int prev_begin_idx = 0; - for (std::size_t i = 0; i < traj_points.size(); i++) { - const auto first = points.begin() + prev_begin_idx; - - // TODO(Horibe) could be replaced end() with some reasonable number to reduce computational time - const auto last = points.end(); - const T truncated_points(first, last); - - const size_t closest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(truncated_points, traj_points[i].pose.position); - // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double closest_to_target_dist = tier4_autoware_utils::calcSignedArcLength( - truncated_points, truncated_points.at(closest_seg_idx).pose.position, - traj_points[i].pose.position); - const double seg_dist = tier4_autoware_utils::calcSignedArcLength( - truncated_points, closest_seg_idx, closest_seg_idx + 1); - - // interpolate 1st-nearest (v1) value and 2nd-nearest value (v2) - const auto lerp = [&](const double v1, const double v2, const double ratio) { - return std::abs(seg_dist) < 1e-6 ? v2 : v1 + (v2 - v1) * ratio; - }; - - // z - { - const double closest_z = truncated_points.at(closest_seg_idx).pose.position.z; - const double next_z = truncated_points.at(closest_seg_idx + 1).pose.position.z; - traj_points[i].pose.position.z = lerp(closest_z, next_z, closest_to_target_dist / seg_dist); - } - - // vx - { - const double closest_vel = truncated_points[closest_seg_idx].longitudinal_velocity_mps; - const double next_vel = truncated_points[closest_seg_idx + 1].longitudinal_velocity_mps; - const double target_vel = lerp(closest_vel, next_vel, closest_to_target_dist / seg_dist); - - if (static_cast(i) >= zero_velocity_traj_idx) { - traj_points[i].longitudinal_velocity_mps = 0; - } else if (target_vel < 1e-6) { - const auto idx = std::max(static_cast(i) - 1, 0); - traj_points[i].longitudinal_velocity_mps = traj_points[idx].longitudinal_velocity_mps; - } else if (static_cast(i) <= max_skip_comparison_idx) { - traj_points[i].longitudinal_velocity_mps = target_vel; - } else { - traj_points[i].longitudinal_velocity_mps = - std::fmin(target_vel, traj_points[i].longitudinal_velocity_mps); - } - } - // NOTE: closest_seg_idx is for the clipped trajectory. This operation must be "+=". - prev_begin_idx += closest_seg_idx; - } - return traj_points; -} -template std::vector -alignVelocityWithPoints>( - const std::vector &, - const std::vector &, const int, const int); -template std::vector -alignVelocityWithPoints>( - const std::vector &, - const std::vector &, const int, const int); - -std::vector> getHistogramTable(const std::vector> & input) -{ - std::vector> histogram_table = input; - for (std::size_t i = 0; i < input.size(); i++) { - for (std::size_t j = 0; j < input[i].size(); j++) { - if (input[i][j]) { - histogram_table[i][j] = 0; - } else { - histogram_table[i][j] = (i > 0) ? histogram_table[i - 1][j] + 1 : 1; - } - } - } - return histogram_table; -} - -struct HistogramBin -{ - int height; - int variable_pos; - int original_pos; -}; - -Rectangle getLargestRectangleInRow( - const std::vector & histo, const int current_row, [[maybe_unused]] const int row_size) -{ - std::vector search_histo = histo; - search_histo.push_back(0); - std::stack stack; - Rectangle largest_rect; - for (std::size_t i = 0; i < search_histo.size(); i++) { - HistogramBin bin; - bin.height = search_histo[i]; - bin.variable_pos = i; - bin.original_pos = i; - if (stack.empty()) { - stack.push(bin); - } else { - if (stack.top().height < bin.height) { - stack.push(bin); - } else if (stack.top().height >= bin.height) { - int target_i = i; - while (!stack.empty() && bin.height <= stack.top().height) { - HistogramBin tmp_bin = stack.top(); - stack.pop(); - int area = (i - tmp_bin.variable_pos) * tmp_bin.height; - if (area > largest_rect.area) { - largest_rect.max_y_idx = tmp_bin.variable_pos; - largest_rect.min_y_idx = i - 1; - largest_rect.max_x_idx = current_row - tmp_bin.height + 1; - largest_rect.min_x_idx = current_row; - largest_rect.area = area; - } - - target_i = tmp_bin.variable_pos; - } - bin.variable_pos = target_i; - stack.push(bin); - } - } - } - return largest_rect; -} - -Rectangle getLargestRectangle(const std::vector> & input) -{ - std::vector> histogram_table = getHistogramTable(input); - Rectangle largest_rectangle; - for (std::size_t i = 0; i < histogram_table.size(); i++) { - Rectangle rect = getLargestRectangleInRow(histogram_table[i], i, input.size()); - if (rect.area > largest_rectangle.area) { - largest_rectangle = rect; - } - } - return largest_rectangle; -} - -boost::optional getLastExtendedPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold) -{ - const double dist = calculate2DDistance(path_point.pose.position, pose.position); - const double diff_yaw = tf2::getYaw(path_point.pose.orientation) - tf2::getYaw(pose.orientation); - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if ( - dist > 1e-6 && dist < delta_dist_threshold && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - return path_point.pose.position; - } else { - return boost::none; - } -} - -boost::optional getLastExtendedTrajPoint( - const autoware_auto_planning_msgs::msg::PathPoint & path_point, - const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold, - const double delta_dist_threshold) -{ - const double dist = calculate2DDistance(path_point.pose.position, pose.position); - const double diff_yaw = tf2::getYaw(path_point.pose.orientation) - tf2::getYaw(pose.orientation); - const double norm_diff_yaw = normalizeRadian(diff_yaw); - if ( - dist > 1e-6 && dist < delta_dist_threshold && std::fabs(norm_diff_yaw) < delta_yaw_threshold) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_traj_p; - tmp_traj_p.pose.position = path_point.pose.position; - tmp_traj_p.pose.orientation = - util::getQuaternionFromPoints(path_point.pose.position, pose.position); - tmp_traj_p.longitudinal_velocity_mps = path_point.longitudinal_velocity_mps; - return tmp_traj_p; - } else { - return boost::none; - } -} - -std::vector getVehicleFootprints( - const std::vector & optimized_points, - const VehicleParam & vehicle_param) -{ - const double baselink_to_top = vehicle_param.length - vehicle_param.rear_overhang; - const double half_width = vehicle_param.width * 0.5; - std::vector rel_lon_offset{baselink_to_top, baselink_to_top, 0, 0}; - std::vector rel_lat_offset{half_width, -half_width, -half_width, half_width}; - std::vector rects; - for (std::size_t i = 0; i < optimized_points.size(); i++) { - // for (int i = nearest_idx; i < optimized_points.size(); i++) { - std::vector abs_points; - for (std::size_t j = 0; j < rel_lon_offset.size(); j++) { - geometry_msgs::msg::Point rel_point; - rel_point.x = rel_lon_offset[j]; - rel_point.y = rel_lat_offset[j]; - abs_points.push_back( - util::transformToAbsoluteCoordinate2D(rel_point, optimized_points[i].pose)); - } - Footprint rect; - rect.p = optimized_points[i].pose.position; - rect.top_left = abs_points[0]; - rect.top_right = abs_points[1]; - rect.bottom_right = abs_points[2]; - rect.bottom_left = abs_points[3]; - rects.push_back(rect); - } - return rects; -} - -/* - * calculate distance in x-y 2D space - */ -std::vector calcEuclidDist(const std::vector & x, const std::vector & y) -{ - if (x.size() != y.size()) { - std::cerr << "x y vector size should be the same." << std::endl; - } - - std::vector dist_v; - dist_v.push_back(0.0); - for (unsigned int i = 0; i < x.size() - 1; ++i) { - const double dx = x.at(i + 1) - x.at(i); - const double dy = y.at(i + 1) - y.at(i); - dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); - } - - return dist_v; -} - -bool hasValidNearestPointFromEgo( - const geometry_msgs::msg::Pose & ego_pose, const Trajectories & trajs, - const TrajectoryParam & traj_param) -{ - const auto traj = concatTraj(trajs); - const auto interpolated_points = - util::getInterpolatedPoints(traj, traj_param.delta_arc_length_for_trajectory); - const int default_idx = -1; - const int nearest_idx_with_thres = util::getNearestIdx( - interpolated_points, ego_pose, default_idx, traj_param.delta_yaw_threshold_for_closest_point, - traj_param.delta_dist_threshold_for_closest_point); - if (nearest_idx_with_thres == default_idx) { - return false; - } - return true; -} - -std::vector concatTraj( - const Trajectories & trajs) -{ - std::vector trajectory; - trajectory.insert( - trajectory.end(), trajs.model_predictive_trajectory.begin(), - trajs.model_predictive_trajectory.end()); - trajectory.insert( - trajectory.end(), trajs.extended_trajectory.begin(), trajs.extended_trajectory.end()); - return trajectory; -} - -int getZeroVelocityIdx( - const bool is_showing_debug_info, const std::vector & fine_points, - const std::vector & path_points, - const std::unique_ptr & opt_trajs, const TrajectoryParam & traj_param) -{ - const int default_idx = fine_points.size() - 1; - const int zero_velocity_idx_from_path = - getZeroVelocityIdxFromPoints(path_points, fine_points, default_idx, traj_param); - - int zero_velocity_idx_from_opt_points = zero_velocity_idx_from_path; - if (opt_trajs) { - zero_velocity_idx_from_opt_points = getZeroVelocityIdxFromPoints( - util::concatTraj(*opt_trajs), fine_points, default_idx, traj_param); - } - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("util"), is_showing_debug_info, - "0 m/s idx from path: %d from opt: %d total size %zu", zero_velocity_idx_from_path, - zero_velocity_idx_from_opt_points, fine_points.size()); - const int zero_velocity_idx = - std::min(zero_velocity_idx_from_path, zero_velocity_idx_from_opt_points); - - if (is_showing_debug_info) { - int zero_velocity_path_idx = path_points.size() - 1; - for (std::size_t i = 0; i < path_points.size(); i++) { - if (path_points[i].longitudinal_velocity_mps < 1e-6) { - zero_velocity_path_idx = i; - break; - } - } - const float debug_dist = util::calculate2DDistance( - path_points[zero_velocity_path_idx].pose.position, fine_points[zero_velocity_idx]); - RCLCPP_INFO( - rclcpp::get_logger("util"), "Dist from path 0 velocity point: = %f [m]", debug_dist); - } - return zero_velocity_idx; -} - -template -int getZeroVelocityIdxFromPoints( - const T & points, const std::vector & fine_points, - const int /* default_idx */, const TrajectoryParam & traj_param) -{ - int zero_velocity_points_idx = points.size() - 1; - for (std::size_t i = 0; i < points.size(); i++) { - if (points[i].longitudinal_velocity_mps < 1e-6) { - zero_velocity_points_idx = i; - break; - } - } - const int default_zero_velocity_fine_points_idx = fine_points.size() - 1; - const int zero_velocity_fine_points_idx = util::getNearestIdx( - fine_points, points[zero_velocity_points_idx].pose, default_zero_velocity_fine_points_idx, - traj_param.delta_yaw_threshold_for_closest_point, - traj_param.delta_dist_threshold_for_closest_point); - return zero_velocity_fine_points_idx; -} -template int getZeroVelocityIdxFromPoints>( - const std::vector &, - const std::vector &, const int, const TrajectoryParam &); -template int -getZeroVelocityIdxFromPoints>( - const std::vector &, - const std::vector &, const int, const TrajectoryParam &); - -template -double getArcLength(const T & points, const int initial_idx) -{ - double accum_arc_length = 0; - for (size_t i = initial_idx; i < points.size(); i++) { - if (i > 0) { - const double dist = - util::calculate2DDistance(points[i].pose.position, points[i - 1].pose.position); - accum_arc_length += dist; - } - } - return accum_arc_length; -} -template double getArcLength>( - const std::vector &, const int); -template double getArcLength>( - const std::vector &, const int); - -double getArcLength(const std::vector & points, const int initial_idx) -{ - double accum_arc_length = 0; - for (size_t i = initial_idx; i < points.size(); i++) { - if (i > 0) { - const double dist = util::calculate2DDistance(points[i].position, points[i - 1].position); - accum_arc_length += dist; - } - } - return accum_arc_length; -} - -void logOSQPSolutionStatus(const int solution_status) -{ - /****************** - * Solver Status * - ******************/ - int OSQP_DUAL_INFEASIBLE_INACCURATE = 4; - int OSQP_PRIMAL_INFEASIBLE_INACCURATE = 3; - int OSQP_SOLVED_INACCURATE = 2; - int OSQP_SOLVED = 1; - int OSQP_MAX_ITER_REACHED = -2; - int OSQP_PRIMAL_INFEASIBLE = -3; /* primal infeasible */ - int OSQP_DUAL_INFEASIBLE = -4; /* dual infeasible */ - int OSQP_SIGINT = -5; /* interrupted by user */ - - if (solution_status == OSQP_SOLVED) { - // RCLCPP_INFO(rclcpp::get_logger("util"),"[Avoidance] OSQP solution status: OSQP_SOLVED"); - } else if (solution_status == OSQP_DUAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE"); - } else if (solution_status == OSQP_PRIMAL_INFEASIBLE_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), - "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE"); - } else if (solution_status == OSQP_SOLVED_INACCURATE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SOLVED_INACCURATE"); - } else if (solution_status == OSQP_MAX_ITER_REACHED) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_ITER_REACHED"); - } else if (solution_status == OSQP_PRIMAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE"); - } else if (solution_status == OSQP_DUAL_INFEASIBLE) { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE"); - } else if (solution_status == OSQP_SIGINT) { - RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SIGINT"); - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] Interrupted by user, process will be finished."); - std::exit(0); - } else { - RCLCPP_WARN( - rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: Not defined %d", - solution_status); - } -} - -} // namespace util diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp new file mode 100644 index 0000000000000..3ae8832531da5 --- /dev/null +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -0,0 +1,551 @@ +// 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 "obstacle_avoidance_planner/utils.hpp" + +#include "obstacle_avoidance_planner/eb_path_optimizer.hpp" +#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "tf2/utils.h" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" + +#include "boost/optional.hpp" + +#include +#include +#include +#include +#include + +namespace +{ +std::vector convertEulerAngleToMonotonic(const std::vector & angle) +{ + if (angle.empty()) { + return std::vector{}; + } + + std::vector monotonic_angle{angle.front()}; + for (size_t i = 1; i < angle.size(); ++i) { + const double diff_angle = angle.at(i) - monotonic_angle.back(); + monotonic_angle.push_back( + monotonic_angle.back() + tier4_autoware_utils::normalizeRadian(diff_angle)); + } + + return monotonic_angle; +} + +std::vector calcEuclidDist(const std::vector & x, const std::vector & y) +{ + if (x.size() != y.size()) { + std::cerr << "x y vector size should be the same." << std::endl; + } + + std::vector dist_v; + dist_v.push_back(0.0); + for (unsigned int i = 0; i < x.size() - 1; ++i) { + const double dx = x.at(i + 1) - x.at(i); + const double dy = y.at(i + 1) - y.at(i); + dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); + } + + return dist_v; +} + +std::array, 3> validateTrajectoryPoints( + const std::vector & points) +{ + constexpr double epsilon = 1e-6; + + std::vector x; + std::vector y; + std::vector yaw; + for (size_t i = 0; i < points.size(); i++) { + if (i > 0) { + if ( + std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < epsilon && + std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < epsilon) { + continue; + } + } + x.push_back(points[i].pose.position.x); + y.push_back(points[i].pose.position.y); + yaw.push_back(tf2::getYaw(points[i].pose.orientation)); + } + return {x, y, yaw}; +} + +std::array, 2> validatePoints( + const std::vector & points) +{ + std::vector x; + std::vector y; + for (size_t i = 0; i < points.size(); i++) { + if (i > 0) { + if ( + std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < 1e-6 && + std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < 1e-6) { + continue; + } + } + x.push_back(points[i].pose.position.x); + y.push_back(points[i].pose.position.y); + } + return {x, y}; +} + +// only two points is supported +std::vector slerpTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s) +{ + const double h = base_s.at(1) - base_s.at(0); + + const double c = begin_diff; + const double d = base_x.at(0); + const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); + const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); + + std::vector res; + for (const auto & s : new_s) { + const double ds = s - base_s.at(0); + res.push_back(d + (c + (b + a * ds) * ds) * ds); + } + + return res; +} +} // namespace + +namespace tier4_autoware_utils +{ +template <> +geometry_msgs::msg::Point getPoint(const ReferencePoint & p) +{ + return p.p; +} + +template <> +geometry_msgs::msg::Pose getPose(const ReferencePoint & p) +{ + geometry_msgs::msg::Pose pose; + pose.position = p.p; + pose.orientation = createQuaternionFromYaw(p.yaw); + return pose; +} +} // namespace tier4_autoware_utils + +namespace geometry_utils +{ +geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin) +{ + // NOTE: implement transformation without defining yaw variable + // but directly sin/cos of yaw for fast calculation + const auto & q = origin.orientation; + const double cos_yaw = 1 - 2 * q.z * q.z; + const double sin_yaw = 2 * q.w * q.z; + + geometry_msgs::msg::Point absolute_p; + absolute_p.x = point.x * cos_yaw - point.y * sin_yaw + origin.position.x; + absolute_p.y = point.x * sin_yaw + point.y * cos_yaw + origin.position.y; + absolute_p.z = point.z; + + return absolute_p; +} + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & a_root) +{ + const double yaw = tier4_autoware_utils::calcAzimuthAngle(a_root, a); + return tier4_autoware_utils::createQuaternionFromYaw(yaw); +} + +geometry_msgs::msg::Quaternion getQuaternionFromPoints( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + const double dx = (8.0 * (p3.x - p2.x) - (p4.x - p1.x)) / 12.0; + const double dy = (8.0 * (p3.y - p2.y) - (p4.y - p1.y)) / 12.0; + const double yaw = std::atan2(dy, dx); + + return tier4_autoware_utils::createQuaternionFromYaw(yaw); +} + +boost::optional transformMapToOptionalImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info) +{ + const geometry_msgs::msg::Point relative_p = + transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); + const double resolution = occupancy_grid_info.resolution; + const double map_y_height = occupancy_grid_info.height; + const double map_x_width = occupancy_grid_info.width; + const double map_x_in_image_resolution = relative_p.x / resolution; + const double map_y_in_image_resolution = relative_p.y / resolution; + const double image_x = map_y_height - map_y_in_image_resolution; + const double image_y = map_x_width - map_x_in_image_resolution; + if ( + image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && + image_y < static_cast(map_x_width)) { + geometry_msgs::msg::Point image_point; + image_point.x = image_x; + image_point.y = image_y; + return image_point; + } else { + return boost::none; + } +} + +bool transformMapToImage( + const geometry_msgs::msg::Point & map_point, + const nav_msgs::msg::MapMetaData & occupancy_grid_info, geometry_msgs::msg::Point & image_point) +{ + geometry_msgs::msg::Point relative_p = + transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin); + const double map_y_height = occupancy_grid_info.height; + const double map_x_width = occupancy_grid_info.width; + const double scale = 1 / occupancy_grid_info.resolution; + const double map_x_in_image_resolution = relative_p.x * scale; + const double map_y_in_image_resolution = relative_p.y * scale; + const double image_x = map_y_height - map_y_in_image_resolution; + const double image_y = map_x_width - map_x_in_image_resolution; + if ( + image_x >= 0 && image_x < static_cast(map_y_height) && image_y >= 0 && + image_y < static_cast(map_x_width)) { + image_point.x = image_x; + image_point.y = image_y; + return true; + } else { + return false; + } +} +} // namespace geometry_utils + +namespace interpolation_utils +{ +std::vector interpolate2DPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution, + const double offset = 0.0) +{ + if (base_x.empty() || base_y.empty()) { + return std::vector{}; + } + const std::vector base_s = calcEuclidDist(base_x, base_y); + if (base_s.empty() || base_s.size() == 1) { + return std::vector{}; + } + + std::vector new_s; + for (double i = offset; i < base_s.back() - 1e-6; i += resolution) { + new_s.push_back(i); + } + if (new_s.empty()) { + return std::vector{}; + } + + // spline interpolation + const std::vector interpolated_x = interpolation::slerp(base_s, base_x, new_s); + const std::vector interpolated_y = interpolation::slerp(base_s, base_y, new_s); + for (size_t i = 0; i < interpolated_x.size(); ++i) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } + } + + std::vector interpolated_points; + for (size_t i = 0; i < interpolated_x.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = interpolated_x[i]; + point.y = interpolated_y[i]; + interpolated_points.push_back(point); + } + + return interpolated_points; +} + +std::vector interpolateConnected2DPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution, + const double begin_yaw, const double end_yaw) +{ + if (base_x.empty() || base_y.empty()) { + return std::vector{}; + } + std::vector base_s = calcEuclidDist(base_x, base_y); + if (base_s.empty() || base_s.size() == 1) { + return std::vector{}; + } + std::vector new_s; + for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { + new_s.push_back(i); + } + + // spline interpolation + const auto interpolated_x = + slerpTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); + const auto interpolated_y = + slerpTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); + + for (size_t i = 0; i < interpolated_x.size(); i++) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } + } + + std::vector interpolated_points; + for (size_t i = 0; i < interpolated_x.size(); i++) { + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = interpolated_x[i]; + point.pose.position.y = interpolated_y[i]; + + const size_t front_idx = (i == interpolated_x.size() - 1) ? i - 1 : i; + const double dx = interpolated_x[front_idx + 1] - interpolated_x[front_idx]; + const double dy = interpolated_y[front_idx + 1] - interpolated_y[front_idx]; + const double yaw = std::atan2(dy, dx); + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + + interpolated_points.push_back(point); + } + + return interpolated_points; +} + +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, + const std::vector & base_yaw, const double resolution) +{ + if (base_x.empty() || base_y.empty() || base_yaw.empty()) { + return std::vector{}; + } + std::vector base_s = calcEuclidDist(base_x, base_y); + if (base_s.empty() || base_s.size() == 1) { + return std::vector{}; + } + std::vector new_s; + for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { + new_s.push_back(i); + } + + const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); + + // spline interpolation + const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); + const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); + const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); + + for (size_t i = 0; i < interpolated_x.size(); i++) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } + } + + std::vector interpolated_points; + for (size_t i = 0; i < interpolated_x.size(); i++) { + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = interpolated_x[i]; + point.pose.position.y = interpolated_y[i]; + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); + interpolated_points.push_back(point); + } + + return interpolated_points; +} + +std::vector getInterpolatedTrajectoryPoints( + const std::vector & points, + const double delta_arc_length) +{ + std::array, 3> validated_pose = validateTrajectoryPoints(points); + return interpolation_utils::interpolate2DTrajectoryPoints( + validated_pose.at(0), validated_pose.at(1), validated_pose.at(2), delta_arc_length); +} + +std::vector getConnectedInterpolatedPoints( + const std::vector & points, + const double delta_arc_length, const double begin_yaw, const double end_yaw) +{ + std::array, 2> validated_pose = validatePoints(points); + return interpolation_utils::interpolateConnected2DPoints( + validated_pose.at(0), validated_pose.at(1), delta_arc_length, begin_yaw, end_yaw); +} +} // namespace interpolation_utils + +namespace points_utils +{ +// functions to convert to another type of points +std::vector convertToPosesWithYawEstimation( + const std::vector points) +{ + std::vector poses; + if (points.empty()) { + return poses; + } else if (points.size() == 1) { + geometry_msgs::msg::Pose pose; + pose.position = points.at(0); + poses.push_back(pose); + return poses; + } + + for (size_t i = 0; i < points.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position = points.at(i); + + const size_t front_idx = (i == points.size() - 1) ? i - 1 : i; + const double points_yaw = + tier4_autoware_utils::calcAzimuthAngle(points.at(front_idx), points.at(front_idx + 1)); + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(points_yaw); + + poses.push_back(pose); + } + return poses; +} + +template +ReferencePoint convertToReferencePoint(const T & point) +{ + ReferencePoint ref_point; + + const auto & pose = tier4_autoware_utils::getPose(point); + ref_point.p = pose.position; + ref_point.yaw = tf2::getYaw(pose.orientation); + + return ref_point; +} + +template ReferencePoint convertToReferencePoint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & point); +template ReferencePoint convertToReferencePoint( + const geometry_msgs::msg::Pose & point); +template <> +ReferencePoint convertToReferencePoint(const geometry_msgs::msg::Point & point) +{ + ReferencePoint ref_point; + + ref_point.p = point; + + return ref_point; +} + +std::vector concatTrajectory( + const std::vector & traj_points, + const std::vector & extended_traj_points) +{ + std::vector trajectory; + trajectory.insert(trajectory.end(), traj_points.begin(), traj_points.end()); + trajectory.insert(trajectory.end(), extended_traj_points.begin(), extended_traj_points.end()); + return trajectory; +} + +void compensateLastPose( + const autoware_auto_planning_msgs::msg::PathPoint & last_path_point, + std::vector & traj_points, + const double delta_dist_threshold, const double delta_yaw_threshold) +{ + if (traj_points.empty()) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + return; + } + + const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose; + + const double dist = + tier4_autoware_utils::calcDistance2d(last_path_point.pose.position, last_traj_pose.position); + const double norm_diff_yaw = [&]() { + const double diff_yaw = + tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation); + return tier4_autoware_utils::normalizeRadian(diff_yaw); + }(); + if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) { + traj_points.push_back(convertToTrajectoryPoint(last_path_point)); + } +} + +int getNearestIdx( + const std::vector & points, const double target_s, const int begin_idx) +{ + double nearest_delta_s = std::numeric_limits::max(); + int nearest_idx = begin_idx; + for (size_t i = begin_idx; i < points.size(); i++) { + double diff = std::fabs(target_s - points[i].s); + if (diff < nearest_delta_s) { + nearest_delta_s = diff; + nearest_idx = i; + } + } + return nearest_idx; +} +} // namespace points_utils + +namespace utils +{ +void logOSQPSolutionStatus(const int solution_status) +{ + /****************** + * Solver Status * + ******************/ + const int LOCAL_OSQP_SOLVED = 1; + const int LOCAL_OSQP_SOLVED_INACCURATE = 2; + const int LOCAL_OSQP_MAX_ITER_REACHED = -2; + const int LOCAL_OSQP_PRIMAL_INFEASIBLE = -3; + const int LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE = 3; + const int LOCAL_OSQP_DUAL_INFEASIBLE = -4; + const int LOCAL_OSQP_DUAL_INFEASIBLE_INACCURATE = 4; + const int LOCAL_OSQP_SIGINT = -5; + const int LOCAL_OSQP_TIME_LIMIT_REACHED = -6; + const int LOCAL_OSQP_UNSOLVED = -10; + const int LOCAL_OSQP_NON_CVX = -7; + + if (solution_status == LOCAL_OSQP_SOLVED) { + } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE_INACCURATE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), + "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE"); + } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE_INACCURATE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), + "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE"); + } else if (solution_status == LOCAL_OSQP_SOLVED_INACCURATE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SOLVED_INACCURATE"); + } else if (solution_status == LOCAL_OSQP_MAX_ITER_REACHED) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_ITER_REACHED"); + } else if (solution_status == LOCAL_OSQP_PRIMAL_INFEASIBLE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE"); + } else if (solution_status == LOCAL_OSQP_DUAL_INFEASIBLE) { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE"); + } else if (solution_status == LOCAL_OSQP_SIGINT) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_SIGINT"); + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] Interrupted by user, process will be finished."); + std::exit(0); + } else if (solution_status == LOCAL_OSQP_TIME_LIMIT_REACHED) { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_TIME_LIMIT_REACHED"); + } else if (solution_status == LOCAL_OSQP_UNSOLVED) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_UNSOLVED"); + } else if (solution_status == LOCAL_OSQP_NON_CVX) { + RCLCPP_WARN(rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: OSQP_NON_CVX"); + } else { + RCLCPP_WARN( + rclcpp::get_logger("util"), "[Avoidance] OSQP solution status: Not defined %d", + solution_status); + } +} +} // namespace utils diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index d842036dd6f97..e752c31f242da 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -15,48 +15,53 @@ #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include +#include +#include -KinematicsBicycleModel::KinematicsBicycleModel( - const double wheelbase, const double steer_lim, const double steer_tau) -: VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2) +KinematicsBicycleModel::KinematicsBicycleModel(const double wheel_base, const double steer_limit) +: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheel_base, steer_limit) { - wheelbase_ = wheelbase; - steer_lim_ = steer_lim; - steer_tau_ = steer_tau; } -void KinematicsBicycleModel::calculateDiscreteMatrix( - Eigen::MatrixXd * Ad, Eigen::MatrixXd * Bd, Eigen::MatrixXd * Cd, Eigen::MatrixXd * Wd, - const double ds) +void KinematicsBicycleModel::calculateStateEquationMatrix( + Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double ds) { - auto sign = [](double x) { return (x > 0.0) - (x < 0.0); }; + // const double epsilon = std::numeric_limits::epsilon(); + // constexpr double dt = 0.03; // assuming delta time for steer tau - /* Linearize delta around delta_r (reference delta) */ - double delta_r = atan(wheelbase_ * curvature_); - if (abs(delta_r) >= steer_lim_) { - delta_r = steer_lim_ * static_cast(sign(delta_r)); - } - double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r)); + /* + const double lf = wheel_base_ - center_offset_from_base_; + const double lr = center_offset_from_base_; + */ - // Ad << 0.0, velocity, 0.0, 0.0, 0.0, velocity / wheelbase_ * cos_delta_r_squared_inv, 0.0, 0.0, - // -1.0 / steer_tau_; - // Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_x_, dim_x_); - // Ad = (I - dt * 0.5 * Ad).inverse() * (I + dt * 0.5 * Ad); // bilinear discretization + const double delta_r = std::atan(wheel_base_ * curvature_); + const double cropped_delta_r = std::clamp(delta_r, -steer_limit_, steer_limit_); - // assuming delta time for steer tau - constexpr double dt = 0.03; - *Ad << 1.0, ds, 0, 0.0, 1, ds / (wheelbase_ * cos_delta_r_squared_inv), 0.0, 0, - 1 - dt / steer_tau_; + // NOTE: cos(delta_r) will not be zero since that happens only when curvature is infinity + Ad << 1.0, ds, // + 0.0, 1.0; - *Bd << 0.0, 0.0, dt / steer_tau_; + Bd << 0.0, ds / wheel_base_ / std::pow(std::cos(delta_r), 2.0); - *Cd << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0; + Wd << 0.0, -ds * curvature_ + ds / wheel_base_ * + (std::tan(cropped_delta_r) - + cropped_delta_r / std::pow(std::cos(cropped_delta_r), 2.0)); +} - *Wd << 0.0, - -ds * curvature_ + ds / wheelbase_ * (tan(delta_r) - delta_r * cos_delta_r_squared_inv), 0.0; +void KinematicsBicycleModel::calculateObservationMatrix(Eigen::MatrixXd & Cd) +{ + Cd << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0; +} + +void KinematicsBicycleModel::calculateObservationSparseMatrix( + std::vector> & Cd_vec) +{ + Cd_vec.clear(); + Cd_vec.push_back({0, 0, 1.0}); + Cd_vec.push_back({1, 1, 1.0}); } void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd * Uref) { - (*Uref)(0, 0) = std::atan(wheelbase_ * curvature_); + (*Uref)(0, 0) = std::atan(wheel_base_ * curvature_); } diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp index 50a2d72f841a6..8e6522725c133 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp @@ -14,11 +14,24 @@ #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" -VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y) -: dim_x_(dim_x), dim_u_(dim_u), dim_y_(dim_y) +VehicleModelInterface::VehicleModelInterface( + int dim_x, int dim_u, int dim_y, double wheel_base, double steer_limit) +: dim_x_(dim_x), + dim_u_(dim_u), + dim_y_(dim_y), + wheel_base_(wheel_base), + steer_limit_(steer_limit), + center_offset_from_base_(0.0) { } + int VehicleModelInterface::getDimX() { return dim_x_; } int VehicleModelInterface::getDimU() { return dim_u_; } int VehicleModelInterface::getDimY() { return dim_y_; } + +void VehicleModelInterface::updateCenterOffset(const double center_offset_from_base) +{ + center_offset_from_base_ = center_offset_from_base; +} + void VehicleModelInterface::setCurvature(const double curvature) { curvature_ = curvature; } diff --git a/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp b/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp index c82df6cccedbb..167947c9ff4f7 100644 --- a/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp +++ b/sensing/laserscan_to_occupancy_grid_map/src/laserscan_to_occupancy_grid_map_node.cpp @@ -121,6 +121,7 @@ OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_opti use_height_filter_ = declare_parameter("use_height_filter", true); enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); const double map_length{declare_parameter("map_length", 100.0)}; + const double map_width{declare_parameter("map_width", 100.0)}; const double map_resolution{declare_parameter("map_resolution", 0.5)}; const bool input_obstacle_pointcloud{declare_parameter("input_obstacle_pointcloud", true)}; const bool input_obstacle_and_raw_pointcloud{ @@ -152,7 +153,7 @@ OccupancyGridMapNode::OccupancyGridMapNode(const rclcpp::NodeOptions & node_opti /* Occupancy grid */ occupancy_grid_map_updater_ptr_ = std::make_shared( - map_length / map_resolution, map_length / map_resolution, map_resolution); + map_length / map_resolution, map_width / map_resolution, map_resolution); } PointCloud2::SharedPtr OccupancyGridMapNode::convertLaserscanToPointCLoud2( diff --git a/system/dummy_diag_publisher/README.md b/system/dummy_diag_publisher/README.md index d553885f8ca70..0c424b9d0699d 100644 --- a/system/dummy_diag_publisher/README.md +++ b/system/dummy_diag_publisher/README.md @@ -16,11 +16,12 @@ This package outputs a dummy diagnostic data for debugging and developing. ### Node Parameters -| Name | Type | Default Value | Explanation | -| ------------- | ------ | ------------- | ------------------------------------- | -| `update_rate` | int | `10` | Timer callback period [Hz] | -| `diag_name` | string | `diag_name` | Diag_name set by dummy diag publisher | -| `is_active` | bool | `true` | Force update or not | +| Name | Type | Default Value | Explanation | Reconfigurable | +| ------------- | ------------------------------ | ------------- | --------------------------------------- | -------------- | +| `update_rate` | int | `10` | Timer callback period [Hz] | false | +| `diag_name` | string | `diag_name` | Diag_name set by dummy diag publisher | false | +| `is_active` | bool | `true` | Force update or not | true | +| `status` | DummyDiagPublisherNode::Status | `0 (OK)` | diag status set by dummy diag publisher | true | ## Assumptions / Known limits diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml index 26fab9cf3ebd5..ae1042fc006bf 100644 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml @@ -2,6 +2,7 @@ + @@ -10,6 +11,7 @@ + diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 2cea6cc97cb3e..2cc4bca27724a 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -20,26 +20,6 @@ contains: [": emergency_stop_operation"] timeout: 1.0 - driving_recorder: - type: diagnostic_aggregator/AnalyzerGroup - path: driving_recorder - analyzers: - storage_error: - type: diagnostic_aggregator/GenericAnalyzer - path: storage_error - contains: ["bagkeeper"] - timeout: 3.0 - - debug_data_logger: - type: diagnostic_aggregator/AnalyzerGroup - path: debug_data_logger - analyzers: - storage_error: - type: diagnostic_aggregator/GenericAnalyzer - path: storage_error - contains: ["bagpacker"] - timeout: 3.0 - resource_monitoring: type: diagnostic_aggregator/AnalyzerGroup path: resource_monitoring diff --git a/system/system_error_monitor/launch/system_error_monitor.launch.xml b/system/system_error_monitor/launch/system_error_monitor.launch.xml index 981fc8395a545..c35761d994262 100644 --- a/system/system_error_monitor/launch/system_error_monitor.launch.xml +++ b/system/system_error_monitor/launch/system_error_monitor.launch.xml @@ -17,6 +17,7 @@ + @@ -34,6 +35,7 @@ + diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 12fdafce41708..1a0b696bb36f4 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -28,6 +28,35 @@ namespace { +enum class DebugLevel { DEBUG, INFO, WARN, ERROR, FATAL }; + +template +void logThrottledNamed( + const std::string & logger_name, const rclcpp::Clock::SharedPtr clock, const double duration_ms, + const std::string & message) +{ + static std::unordered_map last_output_time; + if (last_output_time.count(logger_name) != 0) { + const auto time_from_last_output = clock->now() - last_output_time.at(logger_name); + if (time_from_last_output.seconds() * 1000.0 < duration_ms) { + return; + } + } + + last_output_time[logger_name] = clock->now(); + if constexpr (debug_level == DebugLevel::DEBUG) { + RCLCPP_DEBUG(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::INFO) { + RCLCPP_INFO(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::WARN) { + RCLCPP_WARN(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::ERROR) { + RCLCPP_ERROR(rclcpp::get_logger(logger_name), message.c_str()); + } else if constexpr (debug_level == DebugLevel::FATAL) { + RCLCPP_FATAL(rclcpp::get_logger(logger_name), message.c_str()); + } +} + std::vector split(const std::string & str, const char delim) { std::vector elems; @@ -111,9 +140,17 @@ diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( diag_array.status.push_back(decorateDiag(hazard_diag, "[Safe Fault]")); } for (const auto & hazard_diag : hazard_status.diag_latent_fault) { + const std::string logger_name = "system_error_monitor " + hazard_diag.name; + logThrottledNamed( + logger_name, clock, 5000, "[Latent Fault]: " + hazard_diag.message); + diag_array.status.push_back(decorateDiag(hazard_diag, "[Latent Fault]")); } for (const auto & hazard_diag : hazard_status.diag_single_point_fault) { + const std::string logger_name = "system_error_monitor " + hazard_diag.name; + logThrottledNamed( + logger_name, clock, 5000, "[Single Point Fault]: " + hazard_diag.message); + diag_array.status.push_back(decorateDiag(hazard_diag, "[Single Point Fault]")); }