From 9bc020d216365bef6ae0adc4e01aa3d4b9bfba26 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 8 Jul 2024 22:55:42 +0900 Subject: [PATCH 1/7] feat(autoware_universe_utils): add TimeKeeper to track function's processing time (#7754) Signed-off-by: Takayuki Murooka --- common/autoware_universe_utils/CMakeLists.txt | 25 ++ common/autoware_universe_utils/README.md | 249 ++++++++++++++++++ .../examples/example_scoped_time_track.cpp | 61 +++++ .../examples/example_time_keeper.cpp | 64 +++++ .../universe_utils/system/time_keeper.hpp | 202 ++++++++++++++ .../src/system/time_keeper.cpp | 175 ++++++++++++ .../test/src/system/test_time_keeper.cpp | 53 ++++ .../path_optimizer/common_structs.hpp | 52 ---- .../autoware/path_optimizer/mpt_optimizer.hpp | 5 +- .../include/autoware/path_optimizer/node.hpp | 7 +- .../state_equation_generator.hpp | 7 +- .../src/mpt_optimizer.cpp | 63 ++--- planning/autoware_path_optimizer/src/node.cpp | 71 +++-- .../src/state_equation_generator.cpp | 3 +- 14 files changed, 893 insertions(+), 144 deletions(-) create mode 100644 common/autoware_universe_utils/examples/example_scoped_time_track.cpp create mode 100644 common/autoware_universe_utils/examples/example_time_keeper.cpp create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp create mode 100644 common/autoware_universe_utils/src/system/time_keeper.cpp create mode 100644 common/autoware_universe_utils/test/src/system/test_time_keeper.cpp diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 9e86ddeb60692..2fdeef2119ab8 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,11 +1,15 @@ cmake_minimum_required(VERSION 3.14) project(autoware_universe_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) +find_package(fmt REQUIRED) + ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp @@ -16,6 +20,11 @@ ament_auto_add_library(autoware_universe_utils SHARED src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp src/system/backtrace.cpp + src/system/time_keeper.cpp +) + +target_link_libraries(autoware_universe_utils + fmt::fmt ) if(BUILD_TESTING) @@ -30,4 +39,20 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_universe_utils + ) + install(TARGETS ${example_name} + DESTINATION lib/${PROJECT_NAME} + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 22b9355b09635..2d24f84423299 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -7,3 +7,252 @@ This package contains many common functions used by other packages, so please re ## For developers `autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. + +## `autoware::universe_utils` + +### `systems` + +#### `autoware::universe_utils::TimeKeeper` + +##### Constructor + +```cpp +template +explicit TimeKeeper(Reporters... reporters); +``` + +- Initializes the `TimeKeeper` with a list of reporters. + +##### Methods + +- `void add_reporter(std::ostream * os);` + + - Adds a reporter to output processing times to an `ostream`. + - `os`: Pointer to the `ostream` object. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher with `std_msgs::msg::String`. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void start_track(const std::string & func_name);` + + - Starts tracking the processing time of a function. + - `func_name`: Name of the function to be tracked. + +- `void end_track(const std::string & func_name);` + - Ends tracking the processing time of a function. + - `func_name`: Name of the function to end tracking. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` + +#### `autoware::universe_utils::ScopedTimeTrack` + +##### Description + +Class for automatically tracking the processing time of a function within a scope. + +##### Constructor + +```cpp +ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); +``` + +- `func_name`: Name of the function to be tracked. +- `time_keeper`: Reference to the `TimeKeeper` object. + +##### Destructor + +```cpp +~ScopedTimeTrack(); +``` + +- Destroys the `ScopedTimeTrack` object, ending the tracking of the function. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp new file mode 100644 index 0000000000000..010cc58aba8ae --- /dev/null +++ b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp @@ -0,0 +1,61 @@ +// Copyright 2024 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp new file mode 100644 index 0000000000000..3f6037e68daac --- /dev/null +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp new file mode 100644 index 0000000000000..96070f0f30b77 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -0,0 +1,202 @@ +// Copyright 2024 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 AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ + +#include "autoware/universe_utils/system/stop_watch.hpp" + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware::universe_utils +{ +/** + * @brief Class representing a node in the time tracking tree + */ +class ProcessingTimeNode : public std::enable_shared_from_this +{ +public: + /** + * @brief Construct a new ProcessingTimeNode object + * + * @param name Name of the node + */ + explicit ProcessingTimeNode(const std::string & name); + + /** + * @brief Add a child node + * + * @param name Name of the child node + * @return std::shared_ptr Shared pointer to the newly added child node + */ + std::shared_ptr add_child(const std::string & name); + + /** + * @brief Get the result string representing the node and its children in a tree structure + * + * @return std::string Result string representing the node and its children + */ + std::string to_string() const; + + /** + * @brief Construct a ProcessingTimeTree message from the node and its children + * + * @return tier4_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree message + */ + tier4_debug_msgs::msg::ProcessingTimeTree to_msg() const; + + /** + * @brief Get the parent node + * + * @return std::shared_ptr Shared pointer to the parent node + */ + std::shared_ptr get_parent_node() const; + + /** + * @brief Get the child nodes + * + * @return std::vector> Vector of shared pointers to the child + * nodes + */ + std::vector> get_child_nodes() const; + + /** + * @brief Set the processing time for the node + * + * @param processing_time Processing time to be set + */ + void set_time(const double processing_time); + + /** + * @brief Get the name of the node + * + * @return std::string Name of the node + */ + std::string get_name() const; + +private: + const std::string name_; //!< Name of the node + double processing_time_{0.0}; //!< Processing time of the node + std::shared_ptr parent_node_{nullptr}; //!< Shared pointer to the parent node + std::vector> + child_nodes_; //!< Vector of shared pointers to the child nodes +}; + +using ProcessingTimeDetail = + tier4_debug_msgs::msg::ProcessingTimeTree; //!< Alias for the ProcessingTimeTree message + +/** + * @brief Class for tracking and reporting the processing time of various functions + */ +class TimeKeeper +{ +public: + template + explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) + { + reporters_.reserve(sizeof...(Reporters)); + (add_reporter(reporters), ...); + } + + /** + * @brief Add a reporter to output processing times to an ostream + * + * @param os Pointer to the ostream object + */ + void add_reporter(std::ostream * os); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher with + * std_msgs::msg::String + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Start tracking the processing time of a function + * + * @param func_name Name of the function to be tracked + */ + void start_track(const std::string & func_name); + + /** + * @brief End tracking the processing time of a function + * + * @param func_name Name of the function to end tracking + */ + void end_track(const std::string & func_name); + +private: + /** + * @brief Report the processing times to all registered reporters + */ + void report(); + + std::shared_ptr + current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr root_node_; //!< Shared pointer to the root time node + autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; //!< StopWatch object for tracking the processing time + + std::vector &)>> + reporters_; //!< Vector of functions for reporting the processing times +}; + +/** + * @brief Class for automatically tracking the processing time of a function within a scope + */ +class ScopedTimeTrack +{ +public: + /** + * @brief Construct a new ScopedTimeTrack object + * + * @param func_name Name of the function to be tracked + * @param time_keeper Reference to the TimeKeeper object + */ + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + + /** + * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function + */ + ~ScopedTimeTrack(); + +private: + const std::string func_name_; //!< Name of the function being tracked + TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp new file mode 100644 index 0000000000000..58bed6677227c --- /dev/null +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -0,0 +1,175 @@ +// Copyright 2024 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include +#include + +namespace autoware::universe_utils +{ + +ProcessingTimeNode::ProcessingTimeNode(const std::string & name) : name_(name) +{ +} + +std::shared_ptr ProcessingTimeNode::add_child(const std::string & name) +{ + auto new_child_node = std::make_shared(name); + new_child_node->parent_node_ = shared_from_this(); + child_nodes_.push_back(new_child_node); + return new_child_node; +} + +std::string ProcessingTimeNode::to_string() const +{ + std::function + construct_string = [&]( + const ProcessingTimeNode & node, std::ostringstream & oss, + const std::string & prefix, bool is_last, bool is_root) { + if (!is_root) { + oss << prefix << (is_last ? "└── " : "├── "); + } + oss << node.name_ << " (" << node.processing_time_ << "ms)\n"; + for (size_t i = 0; i < node.child_nodes_.size(); ++i) { + const auto & child = node.child_nodes_[i]; + construct_string( + *child, oss, prefix + (is_last ? " " : "│ "), i == node.child_nodes_.size() - 1, + false); + } + }; + + std::ostringstream oss; + construct_string(*this, oss, "", true, true); + return oss.str(); +} + +tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const +{ + tier4_debug_msgs::msg::ProcessingTimeTree time_tree_msg; + + std::function + construct_msg = [&]( + const ProcessingTimeNode & node, + tier4_debug_msgs::msg::ProcessingTimeTree & tree_msg, int parent_id) { + tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; + time_node_msg.name = node.name_; + time_node_msg.processing_time = node.processing_time_; + time_node_msg.id = tree_msg.nodes.size() + 1; + time_node_msg.parent_id = parent_id; + tree_msg.nodes.emplace_back(time_node_msg); + + for (const auto & child : node.child_nodes_) { + construct_msg(*child, tree_msg, time_node_msg.id); + } + }; + construct_msg(*this, time_tree_msg, 0); + + return time_tree_msg; +} + +std::shared_ptr ProcessingTimeNode::get_parent_node() const +{ + return parent_node_; +} +std::vector> ProcessingTimeNode::get_child_nodes() const +{ + return child_nodes_; +} +void ProcessingTimeNode::set_time(const double processing_time) +{ + processing_time_ = processing_time; +} +std::string ProcessingTimeNode::get_name() const +{ + return name_; +} + +void TimeKeeper::add_reporter(std::ostream * os) +{ + reporters_.emplace_back([os](const std::shared_ptr & node) { + *os << "==========================" << std::endl; + *os << node->to_string() << std::endl; + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + publisher->publish(node->to_msg()); + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + std_msgs::msg::String msg; + msg.data = node->to_string(); + publisher->publish(msg); + }); +} + +void TimeKeeper::start_track(const std::string & func_name) +{ + if (current_time_node_ == nullptr) { + current_time_node_ = std::make_shared(func_name); + root_node_ = current_time_node_; + } else { + current_time_node_ = current_time_node_->add_child(func_name); + } + stop_watch_.tic(func_name); +} + +void TimeKeeper::end_track(const std::string & func_name) +{ + if (current_time_node_->get_name() != func_name) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but end_track({}) is called", + current_time_node_->get_name(), func_name)); + } + const double processing_time = stop_watch_.toc(func_name); + current_time_node_->set_time(processing_time); + current_time_node_ = current_time_node_->get_parent_node(); + + if (current_time_node_ == nullptr) { + report(); + } +} + +void TimeKeeper::report() +{ + if (current_time_node_ != nullptr) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but report() is called", current_time_node_->get_name())); + } + for (const auto & reporter : reporters_) { + reporter(root_node_); + } + current_time_node_.reset(); + root_node_.reset(); +} + +ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper) +: func_name_(func_name), time_keeper_(time_keeper) +{ + time_keeper_.start_track(func_name_); +} + +ScopedTimeTrack::~ScopedTimeTrack() +{ + time_keeper_.end_track(func_name_); +} + +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp new file mode 100644 index 0000000000000..71ca7cc74bec5 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -0,0 +1,53 @@ +// Copyright 2024 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 "autoware/universe_utils/system/time_keeper.hpp" + +#include +#include + +#include + +#include +#include +#include + +TEST(system, TimeKeeper) +{ + using autoware::universe_utils::ScopedTimeTrack; + using autoware::universe_utils::TimeKeeper; + + rclcpp::Node node{"sample_node"}; + + auto publisher = node.create_publisher( + "~/debug/processing_time_tree", 1); + + TimeKeeper time_keeper(&std::cerr, publisher); + + ScopedTimeTrack st{"main_func", time_keeper}; + + { // funcA + ScopedTimeTrack st{"funcA", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + { // funcB + ScopedTimeTrack st{"funcB", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + { // funcC + ScopedTimeTrack st{"funcC", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } +} diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 7f2688ffaad95..399262f93e19d 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -17,7 +17,6 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -43,57 +42,6 @@ struct PlannerData double ego_vel{}; }; -struct TimeKeeper -{ - void init() - { - accumulated_msg = "\n"; - accumulated_time = 0.0; - } - - template - TimeKeeper & operator<<(const T & msg) - { - latest_stream << msg; - return *this; - } - - void endLine() - { - const auto msg = latest_stream.str(); - accumulated_msg += msg + "\n"; - latest_stream.str(""); - - if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); - } - } - - void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - - void toc(const std::string & func_name, const std::string & white_spaces) - { - const double elapsed_time = stop_watch_.toc(func_name); - *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; - accumulated_time = elapsed_time; - endLine(); - } - - std::string getLog() const { return accumulated_msg; } - - bool enable_calculation_time_info; - std::string accumulated_msg = "\n"; - std::stringstream latest_stream; - - double getAccumulatedTime() const { return accumulated_time; } - - double accumulated_time{0.0}; - - autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; -}; - struct DebugData { // settting diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 4303e5c7e05b4..3443ab663b642 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -19,6 +19,7 @@ #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" @@ -107,7 +108,7 @@ class MPTOptimizer rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr); + const std::shared_ptr time_keeper_); std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; @@ -222,7 +223,7 @@ class MPTOptimizer autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; rclcpp::Logger logger_; MPTParam mpt_param_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 359c20f2a4d29..6f75c649e02b2 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -22,10 +22,11 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "rclcpp/rclcpp.hpp" #include +#include #include #include @@ -65,7 +66,7 @@ class PathOptimizer : public rclcpp::Node // argument variables autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; - mutable std::shared_ptr time_keeper_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; @@ -98,6 +99,8 @@ class PathOptimizer : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index b8162f5ef2d32..1d23d7788dca1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -18,6 +18,7 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -39,9 +40,9 @@ class StateEquationGenerator StateEquationGenerator() = default; StateEquationGenerator( const double wheel_base, const double max_steer_rad, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), - time_keeper_ptr_(time_keeper_ptr) + time_keeper_(time_keeper) { } @@ -56,7 +57,7 @@ class StateEquationGenerator private: std::unique_ptr vehicle_model_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index a5da50862c9fb..1d49b6cb77d59 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -395,13 +395,13 @@ MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), vehicle_info_(vehicle_info), traj_param_(traj_param), debug_data_ptr_(debug_data_ptr), - time_keeper_ptr_(time_keeper_ptr), + time_keeper_(time_keeper), logger_(node->get_logger().get_child("mpt_optimizer")) { // initialize mpt param @@ -411,7 +411,7 @@ MPTOptimizer::MPTOptimizer( // state equation generator state_equation_generator_ = - StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_ptr_); + StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -520,8 +520,6 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData // 8. publish trajectories for debug publishDebugTrajectories(p.header, ref_points, *mpt_traj_points); - time_keeper_ptr_->toc(__func__, " "); - debug_data_ptr_->ref_points = ref_points; prev_ref_points_ptr_ = std::make_shared>(ref_points); prev_optimized_traj_points_ptr_ = @@ -541,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; @@ -549,14 +547,14 @@ std::vector MPTOptimizer::calcReferencePoints( const double backward_traj_length = traj_param_.output_backward_traj_length; // 1. resample and convert smoothed points type from trajectory points to reference points - time_keeper_ptr_->tic("resampleReferencePoints"); + time_keeper_->start_track("resampleReferencePoints"); auto ref_points = [&]() { const auto resampled_smoothed_points = trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( smoothed_points, mpt_param_.delta_arc_length); return trajectory_utils::convertToReferencePoints(resampled_smoothed_points); }(); - time_keeper_ptr_->toc("resampleReferencePoints", " "); + time_keeper_->end_track("resampleReferencePoints"); // 2. crop forward and backward with margin, and calculate spline interpolation // NOTE: Margin is added to calculate orientation, curvature, etc precisely. @@ -611,8 +609,6 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points.resize(mpt_param_.num_points); } - time_keeper_ptr_->toc(__func__, " "); - return ref_points; } @@ -639,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!prev_ref_points_ptr_) { // no fixed point @@ -681,8 +677,6 @@ void MPTOptimizer::updateFixedPoint(std::vector & ref_points) co ref_points.front().curvature = front_point.curvature; ref_points.front().fixed_kinematic_state = front_point.optimized_kinematic_state; } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points) const @@ -697,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -786,8 +780,6 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c } } } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateBounds( @@ -796,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -844,8 +836,6 @@ void MPTOptimizer::updateBounds( } } */ - - time_keeper_ptr_->toc(__func__, " "); return; } @@ -1106,7 +1096,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1162,8 +1152,6 @@ void MPTOptimizer::updateVehicleBounds( ref_points.at(p_idx).pose_on_constraints.push_back(vehicle_bounds_pose); } } - - time_keeper_ptr_->toc(__func__, " "); } // cost function: J = x' Q x + u' R u @@ -1171,7 +1159,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1230,7 +1218,6 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); - time_keeper_ptr_->toc(__func__, " "); return ValueMatrix{Q_sparse_mat, R_sparse_mat}; } @@ -1238,7 +1225,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1288,7 +1275,6 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( obj_matrix.hessian = H; obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1299,7 +1285,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1453,7 +1439,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - time_keeper_ptr_->toc(__func__, " "); return ConstraintMatrix{A, lb, ub}; } @@ -1479,7 +1464,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1512,7 +1497,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const auto lower_bound = toStdVector(updated_const_mat.lower_bound); // initialize or update solver according to warm start - time_keeper_ptr_->tic("initOsqp"); + time_keeper_->start_track("initOsqp"); const autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); @@ -1532,13 +1517,12 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( } prev_mat_n_ = H.rows(); prev_mat_m_ = A.rows(); - - time_keeper_ptr_->toc("initOsqp", " "); + time_keeper_->end_track("initOsqp"); // solve qp - time_keeper_ptr_->tic("solveOsqp"); + time_keeper_->start_track("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); - time_keeper_ptr_->toc("solveOsqp", " "); + time_keeper_->end_track("solveOsqp"); // check solution status const int solution_status = std::get<3>(result); @@ -1565,8 +1549,6 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const Eigen::VectorXd optimized_variables = Eigen::Map(&optimization_result[0], N_v); - time_keeper_ptr_->toc(__func__, " "); - if (u0) { // manual warm start return static_cast(optimized_variables + *u0); } @@ -1649,7 +1631,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1702,7 +1684,6 @@ std::optional> MPTOptimizer::calcMPTPoints( traj_points.push_back(traj_point); } - time_keeper_ptr_->toc(__func__, " "); return traj_points; } @@ -1710,7 +1691,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( @@ -1725,8 +1706,6 @@ void MPTOptimizer::publishDebugTrajectories( // mpt points const auto mpt_traj = autoware::motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); - - time_keeper_ptr_->toc(__func__, " "); } std::vector MPTOptimizer::extractFixedPoints( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 3899867a9dcce..7c73d1ad1fee3 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -23,6 +23,7 @@ #include "rclcpp/time.hpp" #include +#include #include namespace autoware::path_optimizer @@ -38,7 +39,8 @@ std::vector concatVectors(const std::vector & prev_vector, const std::vect return concatenated_vector; } -StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) +[[maybe_unused]] StringStamped createStringStamped( + const rclcpp::Time & now, const std::string & data) { StringStamped msg; msg.stamp = now; @@ -46,7 +48,7 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } -Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +[[maybe_unused]] Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) { Float64Stamped msg; msg.stamp = now; @@ -85,8 +87,7 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), - debug_data_ptr_(std::make_shared()), - time_keeper_ptr_(std::make_shared()) + debug_data_ptr_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -102,6 +103,9 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); debug_calculation_time_float_pub_ = create_publisher("~/debug/processing_time_ms", 1); + debug_processing_time_detail_pub_ = + create_publisher( + "~/debug/processing_time_detail_ms", 1); { // parameters // parameter for option @@ -120,8 +124,6 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); - time_keeper_ptr_->enable_calculation_time_info = - declare_parameter("option.debug.enable_calculation_time_info"); vehicle_stop_margin_outside_drivable_area_ = declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); @@ -133,11 +135,14 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) traj_param_ = TrajectoryParam(this); } + time_keeper_ = + std::make_shared(debug_processing_time_detail_pub_); + // create core algorithm pointers with parameter declaration replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); mpt_optimizer_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, vehicle_info_, traj_param_, debug_data_ptr_, - time_keeper_ptr_); + time_keeper_); // reset planners // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been @@ -177,9 +182,6 @@ rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( // parameters for debug info updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); - updateParam( - parameters, "option.debug.enable_calculation_time_info", - time_keeper_ptr_->enable_calculation_time_info); updateParam( parameters, "common.vehicle_stop_margin_outside_drivable_area", @@ -220,8 +222,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - time_keeper_ptr_->init(); - time_keeper_ptr_->tic(__func__); + time_keeper_->start_track(__func__); // check if input path is valid if (!checkInputPath(*path_ptr, *get_clock())) { @@ -267,21 +268,21 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) // 5. publish debug data publishDebugData(planner_data.header); - time_keeper_ptr_->toc(__func__, ""); - *time_keeper_ptr_ << "========================================"; - time_keeper_ptr_->endLine(); - // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time + /* const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); debug_calculation_time_str_pub_->publish(calculation_time_msg); debug_calculation_time_float_pub_->publish( createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); + */ const auto output_traj_msg = autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); + + time_keeper_->end_track(__func__); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const @@ -319,7 +320,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_traj_points = planner_data.traj_points; @@ -339,13 +340,12 @@ std::vector PathOptimizer::generateOptimizedTrajectory( // 4. publish debug marker publishDebugMarkerOfOptimization(optimized_traj_points); - time_keeper_ptr_->toc(__func__, " "); return optimized_traj_points; } std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -372,7 +372,6 @@ std::vector PathOptimizer::optimizeTrajectory(const PlannerData // with model predictive trajectory const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); - time_keeper_ptr_->toc(__func__, " "); return mpt_traj; } @@ -391,7 +390,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -486,14 +485,12 @@ void PathOptimizer::applyInputVelocity( trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (optimized_traj_points.empty()) { return; @@ -553,13 +550,11 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( } else { debug_data_ptr_->stop_pose_by_drivable_area = std::nullopt; } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -569,36 +564,33 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos } virtual_wall_pub_->publish(virtual_wall_marker); - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!enable_pub_debug_marker_) { return; } - time_keeper_ptr_->tic(__func__); - // debug marker - time_keeper_ptr_->tic("getDebugMarker"); + time_keeper_->start_track("getDebugMarker"); const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_); - time_keeper_ptr_->toc("getDebugMarker", " "); + time_keeper_->end_track("getDebugMarker"); - time_keeper_ptr_->tic("publishDebugMarker"); + time_keeper_->start_track("publishDebugMarker"); debug_markers_pub_->publish(debug_marker); - time_keeper_ptr_->toc("publishDebugMarker", " "); - - time_keeper_ptr_->toc(__func__, " "); + time_keeper_->end_track("publishDebugMarker"); } std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -654,20 +646,17 @@ std::vector PathOptimizer::extendTrajectory( // debug_data_ptr_->extended_traj_points = // extended_traj_points ? *extended_traj_points : std::vector(); - time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } void PathOptimizer::publishDebugData(const Header & header) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // publish trajectories const auto debug_extended_traj = autoware::motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); - - time_keeper_ptr_->toc(__func__, " "); } } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 5aa2ab4ffbfa8..74033c5834db2 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); @@ -60,7 +60,6 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( W.segment(i * D_x, D_x) = Wd; } - time_keeper_ptr_->toc(__func__, " "); return Matrix{A, B, W}; } From fbc6cc34996c3643919a77869f00ca4d3ceb71eb Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 12 Jul 2024 16:26:25 +0900 Subject: [PATCH 2/7] chore(autoware_universe_utils): update document (#7907) * update readme Signed-off-by: Y.Hisaki * refactoring Signed-off-by: Y.Hisaki * remove string reporter Signed-off-by: Y.Hisaki * fix readme.md Signed-off-by: Y.Hisaki * change node name of example Signed-off-by: Y.Hisaki * update readme Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- common/autoware_universe_utils/README.md | 223 ++++++------------ .../examples/example_scoped_time_track.cpp | 61 ----- .../examples/example_time_keeper.cpp | 78 +++--- .../universe_utils/system/time_keeper.hpp | 16 +- .../src/system/time_keeper.cpp | 14 +- 5 files changed, 127 insertions(+), 265 deletions(-) delete mode 100644 common/autoware_universe_utils/examples/example_scoped_time_track.cpp diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 2d24f84423299..cd5e366c520fc 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -49,56 +49,80 @@ explicit TimeKeeper(Reporters... reporters); - Ends tracking the processing time of a function. - `func_name`: Name of the function to end tracking. +##### Note + +- It's possible to start and end time measurements using `start_track` and `end_track` as shown below: + + ```cpp + time_keeper.start_track("example_function"); + // Your function code here + time_keeper.end_track("example_function"); + ``` + +- For safety and to ensure proper tracking, it is recommended to use `ScopedTimeTrack`. + ##### Example ```cpp -#include "autoware/universe_utils/system/time_keeper.hpp" - #include +#include + +#include #include #include +#include + +class ExampleNode : public rclcpp::Node +{ +public: + ExampleNode() : Node("time_keeper_example") + { + publisher_ = + create_publisher("processing_time", 1); + + time_keeper_ = std::make_shared(publisher_, &std::cerr); + // You can also add a reporter later by add_reporter. + // time_keeper_->add_reporter(publisher_); + // time_keeper_->add_reporter(&std::cerr); + + timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); + } + +private: + std::shared_ptr time_keeper_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_str_; + rclcpp::TimerBase::SharedPtr timer_; + + void func_a() + { + // Start constructing ProcessingTimeTree (because func_a is the root function) + autoware::universe_utils::ScopedTimeTrack st("func_a", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + func_b(); + // End constructing ProcessingTimeTree. After this, the tree will be reported (publishing + // message and outputting to std::cerr) + } + + void func_b() + { + autoware::universe_utils::ScopedTimeTrack st("func_b", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + func_c(); + } + + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } +}; int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("time_keeper_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - time_keeper->start_track("funcA"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - time_keeper->end_track("funcA"); - }; - - auto funcB = [&time_keeper, &funcA]() { - time_keeper->start_track("funcB"); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - time_keeper->end_track("funcB"); - }; - - auto funcC = [&time_keeper, &funcB]() { - time_keeper->start_track("funcC"); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - time_keeper->end_track("funcC"); - }; - - funcC(); - + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; @@ -109,38 +133,28 @@ int main(int argc, char ** argv) ```text ========================== - funcC (6000.7ms) - └── funcB (3000.44ms) - └── funcA (1000.19ms) + func_a (6.382ms) + └── func_b (5.243ms) + └── func_c (3.146ms) ``` - Output (`ros2 topic echo /processing_time`) ```text + --- nodes: - id: 1 - name: funcC - processing_time: 6000.659 + name: func_a + processing_time: 6.397 parent_id: 0 - id: 2 - name: funcB - processing_time: 3000.415 + name: func_b + processing_time: 5.263 parent_id: 1 - id: 3 - name: funcA - processing_time: 1000.181 + name: func_c + processing_time: 3.129 parent_id: 2 - --- - ``` - -- Output (`ros2 topic echo /processing_time_str --field data`) - - ```text - funcC (6000.67ms) - └── funcB (3000.42ms) - └── funcA (1000.19ms) - - --- ``` #### `autoware::universe_utils::ScopedTimeTrack` @@ -165,94 +179,3 @@ ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); ``` - Destroys the `ScopedTimeTrack` object, ending the tracking of the function. - -##### Example - -```cpp -#include "autoware/universe_utils/system/time_keeper.hpp" - -#include - -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("scoped_time_track_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(1)); - }; - - auto funcB = [&time_keeper, &funcA]() { - autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - }; - - auto funcC = [&time_keeper, &funcB]() { - autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - }; - - funcC(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} -``` - -- Output (console) - - ```text - ========================== - funcC (6000.7ms) - └── funcB (3000.44ms) - └── funcA (1000.19ms) - ``` - -- Output (`ros2 topic echo /processing_time`) - - ```text - nodes: - - id: 1 - name: funcC - processing_time: 6000.659 - parent_id: 0 - - id: 2 - name: funcB - processing_time: 3000.415 - parent_id: 1 - - id: 3 - name: funcA - processing_time: 1000.181 - parent_id: 2 - --- - ``` - -- Output (`ros2 topic echo /processing_time_str --field data`) - - ```text - funcC (6000.67ms) - └── funcB (3000.42ms) - └── funcA (1000.19ms) - - --- - ``` diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp deleted file mode 100644 index 010cc58aba8ae..0000000000000 --- a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2024 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 "autoware/universe_utils/system/time_keeper.hpp" - -#include - -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("scoped_time_track_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(1)); - }; - - auto funcB = [&time_keeper, &funcA]() { - autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - }; - - auto funcC = [&time_keeper, &funcB]() { - autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - }; - - funcC(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp index 3f6037e68daac..c50aa1e1a8342 100644 --- a/common/autoware_universe_utils/examples/example_time_keeper.cpp +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -15,49 +15,63 @@ #include +#include + +#include #include #include +#include -int main(int argc, char ** argv) +class ExampleNode : public rclcpp::Node { - rclcpp::init(argc, argv); - auto node = std::make_shared("time_keeper_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); +public: + ExampleNode() : Node("time_keeper_example") + { + publisher_ = + create_publisher("processing_time", 1); - auto publisher = - node->create_publisher("processing_time", 10); + time_keeper_ = std::make_shared(publisher_, &std::cerr); + // You can also add a reporter later by add_reporter. + // time_keeper_->add_reporter(publisher_); + // time_keeper_->add_reporter(&std::cerr); - time_keeper->add_reporter(publisher); + timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); + } - auto publisher_str = node->create_publisher("processing_time_str", 10); +private: + std::shared_ptr time_keeper_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_str_; + rclcpp::TimerBase::SharedPtr timer_; - time_keeper->add_reporter(publisher_str); + void func_a() + { + // Start constructing ProcessingTimeTree (because func_a is the root function) + autoware::universe_utils::ScopedTimeTrack st("func_a", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + func_b(); + // End constructing ProcessingTimeTree. After this, the tree will be reported (publishing + // message and outputting to std::cerr) + } - auto funcA = [&time_keeper]() { - time_keeper->start_track("funcA"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - time_keeper->end_track("funcA"); - }; + void func_b() + { + autoware::universe_utils::ScopedTimeTrack st("func_b", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + func_c(); + } - auto funcB = [&time_keeper, &funcA]() { - time_keeper->start_track("funcB"); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - time_keeper->end_track("funcB"); - }; - - auto funcC = [&time_keeper, &funcB]() { - time_keeper->start_track("funcC"); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - time_keeper->end_track("funcC"); - }; - - funcC(); + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } +}; +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 96070f0f30b77..c635138416aa5 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -17,14 +17,11 @@ #include "autoware/universe_utils/system/stop_watch.hpp" #include -#include #include #include #include -#include - #include #include #include @@ -134,14 +131,6 @@ class TimeKeeper */ void add_reporter(rclcpp::Publisher::SharedPtr publisher); - /** - * @brief Add a reporter to publish processing times to an rclcpp publisher with - * std_msgs::msg::String - * - * @param publisher Shared pointer to the rclcpp publisher - */ - void add_reporter(rclcpp::Publisher::SharedPtr publisher); - /** * @brief Start tracking the processing time of a function * @@ -187,6 +176,11 @@ class ScopedTimeTrack */ ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + ScopedTimeTrack(const ScopedTimeTrack &) = delete; + ScopedTimeTrack & operator=(const ScopedTimeTrack &) = delete; + ScopedTimeTrack(ScopedTimeTrack &&) = delete; + ScopedTimeTrack & operator=(ScopedTimeTrack &&) = delete; + /** * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function */ diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 58bed6677227c..429f063dfc62e 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -14,7 +14,8 @@ #include "autoware/universe_utils/system/time_keeper.hpp" -#include +#include + #include namespace autoware::universe_utils @@ -67,7 +68,7 @@ tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; time_node_msg.name = node.name_; time_node_msg.processing_time = node.processing_time_; - time_node_msg.id = tree_msg.nodes.size() + 1; + time_node_msg.id = static_cast(tree_msg.nodes.size() + 1); time_node_msg.parent_id = parent_id; tree_msg.nodes.emplace_back(time_node_msg); @@ -112,15 +113,6 @@ void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr }); } -void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) -{ - reporters_.emplace_back([publisher](const std::shared_ptr & node) { - std_msgs::msg::String msg; - msg.data = node->to_string(); - publisher->publish(msg); - }); -} - void TimeKeeper::start_track(const std::string & func_name) { if (current_time_node_ == nullptr) { From 5023173eb1bb087def663de88104aa9854808cd6 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 12 Jul 2024 20:06:30 +0900 Subject: [PATCH 3/7] feat(autoware_behavior_path_planner_common,autoware_behavior_path_lane_change_module): add time_keeper to bpp (#8004) * feat(autoware_behavior_path_planner_common,autoware_behavior_path_lane_change_module): add time_keeper to bpp Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- .../interface.hpp | 2 ++ .../utils/base_class.hpp | 21 +++++++++++++++++-- .../src/interface.cpp | 6 ++++++ .../src/scene.cpp | 17 +++++++++++++++ .../behavior_path_planner/planner_manager.hpp | 2 ++ .../interface/scene_module_interface.hpp | 8 ++++++- .../scene_module_manager_interface.hpp | 5 +++++ 7 files changed, 58 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 9c014b77a7e82..bd309dd35a260 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include #include #include @@ -108,6 +109,7 @@ class LaneChangeInterface : public SceneModuleInterface const double start_distance, const double finish_distance, const bool safe, const uint8_t & state) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { ptr->updateCooperateStatus( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 229f46f89377f..49c8056681e2f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include #include @@ -50,7 +51,10 @@ class LaneChangeBase LaneChangeBase( std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) - : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type} + : lane_change_parameters_{std::move(parameters)}, + direction_{direction}, + type_{type}, + time_keeper_(std::make_shared()) { } @@ -151,7 +155,18 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } - void setData(const std::shared_ptr & data) { planner_data_ = data; } + void setData(const std::shared_ptr & data) + { + planner_data_ = data; + if (!common_data_ptr_->bpp_param_ptr) { + common_data_ptr_->bpp_param_ptr = + std::make_shared(data->parameters); + } + common_data_ptr_->self_odometry_ptr = data->self_odometry; + common_data_ptr_->route_handler_ptr = data->route_handler; + common_data_ptr_->lc_param_ptr = lane_change_parameters_; + common_data_ptr_->direction = direction_; + } void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } @@ -237,6 +252,8 @@ class LaneChangeBase rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 0a5b31f9e32d2..b55b41828081e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -21,6 +21,7 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include +#include #include #include @@ -43,6 +44,7 @@ LaneChangeInterface::LaneChangeInterface( module_type_{std::move(module_type)}, prev_approved_path_{std::make_unique()} { + module_type_->setTimeKeeper(getTimeKeeper()); steering_factor_interface_ptr_ = std::make_unique(&node, name); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } @@ -71,6 +73,7 @@ bool LaneChangeInterface::isExecutionReady() const void LaneChangeInterface::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->updateSpecialData(); @@ -94,6 +97,7 @@ void LaneChangeInterface::postProcess() BehaviorModuleOutput LaneChangeInterface::plan() { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); resetPathCandidate(); resetPathReference(); @@ -165,6 +169,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() CandidateOutput LaneChangeInterface::planCandidate() const { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto selected_path = module_type_->getLaneChangePath(); if (selected_path.path.points.empty()) { @@ -340,6 +345,7 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto steering_factor_direction = std::invoke([&]() { if (module_type_->getDirection() == Direction::LEFT) { return SteeringFactor::LEFT; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index ebfa1e7ac260c..3fc64736731f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include #include @@ -55,6 +56,7 @@ NormalLaneChange::NormalLaneChange( void NormalLaneChange::updateLaneChangeStatus() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); @@ -263,6 +265,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const BehaviorModuleOutput NormalLaneChange::generateOutput() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); return prev_module_output_; @@ -308,6 +311,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( @@ -327,6 +331,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c void NormalLaneChange::insertStopPoint( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { return; } @@ -468,6 +473,7 @@ void NormalLaneChange::insertStopPoint( PathWithLaneId NormalLaneChange::getReferencePath() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { @@ -482,6 +488,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const std::optional NormalLaneChange::extendPath() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; @@ -560,6 +567,7 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & pose = getEgoPose(); const auto & current_lanes = status_.current_lanes; const auto & shift_line = status_.lane_change_path.info.shift_line; @@ -584,6 +592,7 @@ lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { return {}; } @@ -1832,6 +1841,7 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = planner_data_->route_handler; const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -1869,6 +1879,7 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && @@ -1882,6 +1893,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) bool NormalLaneChange::calcAbortPath() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = @@ -2025,6 +2037,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); PathSafetyStatus path_safety_status; if (collision_check_objects.empty()) { @@ -2108,6 +2121,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( bool NormalLaneChange::isVehicleStuck( const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // Ego is still moving, not in stuck if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); @@ -2163,6 +2177,7 @@ bool NormalLaneChange::isVehicleStuck( double NormalLaneChange::get_max_velocity_for_safety_check() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity; if (external_velocity_limit_ptr) { return std::min( @@ -2174,6 +2189,7 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { lane_change_debug_.is_stuck = false; return false; // can not check @@ -2208,6 +2224,7 @@ void NormalLaneChange::setStopPose(const Pose & stop_pose) void NormalLaneChange::updateStopTime() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_vel = getEgoVelocity(); if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index a76284b70b7d8..3a9aa9cafbcca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -21,6 +21,7 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include #include @@ -277,6 +278,7 @@ class PlannerManager const SceneModulePtr & module_ptr, const std::shared_ptr & planner_data, const BehaviorModuleOutput & previous_module_output) const { + universe_utils::ScopedTimeTrack st(module_ptr->name() + "=>run", *module_ptr->getTimeKeeper()); stop_watch_.tic(module_ptr->name()); module_ptr->setData(planner_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index d11c3181526d2..95eb82e0b4c23 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -97,7 +98,8 @@ class SceneModuleInterface objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), steering_factor_interface_ptr_( - std::make_unique(&node, utils::convertToSnakeCase(name))) + std::make_unique(&node, utils::convertToSnakeCase(name))), + time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { uuid_map_.emplace(module_name, generateUUID()); @@ -244,6 +246,8 @@ class SceneModuleInterface previous_module_output_ = previous_module_output; } + std::shared_ptr getTimeKeeper() const { return time_keeper_; } + /** * @brief set planner data */ @@ -641,6 +645,8 @@ class SceneModuleInterface mutable MarkerArray debug_marker_; mutable MarkerArray drivable_lanes_marker_; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 5b9c0389e29c5..82a364a9c37c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -85,6 +85,7 @@ class SceneModuleManagerInterface observer.lock()->setData(planner_data_); observer.lock()->setPreviousModuleOutput(previous_module_output); + observer.lock()->getTimeKeeper()->add_reporter(this->pub_processing_time_); observer.lock()->onEntry(); observers_.push_back(observer); @@ -310,6 +311,8 @@ class SceneModuleManagerInterface pub_debug_marker_ = node->create_publisher("~/debug/" + name_, 20); pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name_, 20); pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); + pub_processing_time_ = node->create_publisher( + "~/processing_time/" + name_, 20); } // misc @@ -330,6 +333,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; + rclcpp::Publisher::SharedPtr pub_processing_time_; + std::string name_; std::shared_ptr planner_data_; From 2d9db5c6ea6885c314ae89d00948f55bbce869dc Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 13 Jul 2024 14:27:11 +0900 Subject: [PATCH 4/7] feat(velocity_smoother): add time_keeper (#8026) Signed-off-by: Takayuki Murooka --- .../autoware/velocity_smoother/node.hpp | 5 +++ .../analytical_jerk_constrained_smoother.hpp | 6 +++- .../smoother/jerk_filtered_smoother.hpp | 5 ++- .../smoother/l2_pseudo_jerk_smoother.hpp | 5 ++- .../smoother/linf_pseudo_jerk_smoother.hpp | 5 ++- .../smoother/smoother_base.hpp | 6 +++- .../autoware_velocity_smoother/src/node.cpp | 35 ++++++++++++++++--- .../analytical_jerk_constrained_smoother.cpp | 5 +-- .../src/smoother/jerk_filtered_smoother.cpp | 20 ++++++++++- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 4 ++- .../smoother/linf_pseudo_jerk_smoother.cpp | 4 ++- .../src/smoother/smoother_base.cpp | 8 ++++- 12 files changed, 93 insertions(+), 15 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 0dd18615be5ef..26918cce24549 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -23,6 +23,7 @@ #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/ros/self_pose_listener.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" @@ -260,6 +261,8 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_closest_jerk_; rclcpp::Publisher::SharedPtr debug_calculation_time_; rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_; // For Jerk Filtered Algorithm Debug rclcpp::Publisher::SharedPtr pub_forward_filtered_trajectory_; @@ -275,6 +278,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; + + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 42d5a520c9e44..bbc3828bb1b15 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" @@ -24,6 +25,7 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include #include #include @@ -65,7 +67,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase } backward; }; - explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node); + explicit AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper = + std::make_shared()); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 14d0d8ab7ff84..06a6f2da7f30a 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -40,7 +42,8 @@ class JerkFilteredSmoother : public SmootherBase double jerk_filter_ds; }; - explicit JerkFilteredSmoother(rclcpp::Node & node); + explicit JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 1e2918d8e313b..a2a07ec6909aa 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -38,7 +40,8 @@ class L2PseudoJerkSmoother : public SmootherBase double over_a_weight; }; - explicit L2PseudoJerkSmoother(rclcpp::Node & node); + explicit L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index b88cd26eb4467..7c46a53431608 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -38,7 +40,8 @@ class LinfPseudoJerkSmoother : public SmootherBase double over_a_weight; }; - explicit LinfPseudoJerkSmoother(rclcpp::Node & node); + explicit LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index be7baf694d361..6671eaa3eabe7 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -15,12 +15,14 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include +#include #include namespace autoware::velocity_smoother @@ -54,7 +56,8 @@ class SmootherBase resampling::ResampleParam resample_param; }; - explicit SmootherBase(rclcpp::Node & node); + explicit SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper); virtual ~SmootherBase() = default; virtual bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -85,6 +88,7 @@ class SmootherBase protected: BaseParam base_param_; + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index aefbd9ef20f84..3640c93d2d807 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -46,6 +46,13 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); + // create time_keeper and its publisher + // NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother. + debug_processing_time_detail_ = create_publisher( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = + std::make_shared(debug_processing_time_detail_); + // create smoother setupSmoother(wheelbase_); @@ -99,7 +106,7 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase) { switch (node_param_.algorithm_type) { case AlgorithmType::JERK_FILTERED: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); // Set Publisher for jerk filtered algorithm pub_forward_filtered_trajectory_ = @@ -113,15 +120,15 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase) break; } case AlgorithmType::L2: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::LINF: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::ANALYTICAL: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } default: @@ -309,6 +316,8 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory void VelocitySmootherNode::calcExternalVelocityLimit() { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!external_velocity_limit_ptr_) { return; } @@ -414,6 +423,8 @@ bool VelocitySmootherNode::checkData() const void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); @@ -505,6 +516,8 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr void VelocitySmootherNode::updateDataForExternalVelocityLimit() { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (prev_output_.empty()) { return; } @@ -522,6 +535,8 @@ void VelocitySmootherNode::updateDataForExternalVelocityLimit() TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( const TrajectoryPoints & traj_input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length @@ -569,6 +584,8 @@ bool VelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.empty()) { return false; // cannot apply smoothing } @@ -675,6 +692,8 @@ bool VelocitySmootherNode::smoothVelocity( void VelocitySmootherNode::insertBehindVelocity( const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const bool keep_closest_vel_for_behind = (type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN || type == InitializeType::ENGAGING); @@ -737,6 +756,8 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto std::pair VelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x; const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps); @@ -820,6 +841,8 @@ std::pair VelocitySmootherNode::ca void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input); if (!stop_idx) { return; @@ -866,6 +889,8 @@ void VelocitySmootherNode::overwriteStopPoint( void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (traj.size() < 1) { return; } @@ -905,6 +930,8 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 7f263fdea5e36..3906222454d35 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -66,8 +66,9 @@ bool applyMaxVelocity( namespace autoware::velocity_smoother { -AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) -: SmootherBase(node) +AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.resample.ds_resample = node.declare_parameter("resample.ds_resample"); diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index cab8e49a3b45d..d458c688d060c 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -29,7 +29,9 @@ namespace autoware::velocity_smoother { -JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) +JerkFilteredSmoother::JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.jerk_weight = node.declare_parameter("jerk_weight"); @@ -59,6 +61,8 @@ bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, std::vector & debug_trajectories) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + output = input; if (input.empty()) { @@ -102,6 +106,8 @@ bool JerkFilteredSmoother::apply( const auto initial_traj_pose = filtered.front().pose; const auto resample = [&](const auto & trajectory) { + autoware::universe_utils::ScopedTimeTrack st("resample", *time_keeper_); + return resampling::resampleTrajectory( trajectory, v0, initial_traj_pose, std::numeric_limits::max(), std::numeric_limits::max(), base_param_.resample_param); @@ -152,6 +158,7 @@ bool JerkFilteredSmoother::apply( v_max_arr.at(i) = opt_resampled_trajectory.at(i).longitudinal_velocity_mps; } + time_keeper_->start_track("initOptimization"); /* * x = [ * b[0], b[1], ..., b[N], : 0~N @@ -290,9 +297,12 @@ bool JerkFilteredSmoother::apply( lower_bound[constr_idx] = a0; ++constr_idx; } + time_keeper_->end_track("initOptimization"); // execute optimization + time_keeper_->start_track("optimize"); const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + time_keeper_->end_track("optimize"); const std::vector optval = std::get<0>(result); const int status_val = std::get<3>(result); if (status_val != 1) { @@ -356,6 +366,8 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( const double v0, const double a0, const double a_max, const double a_start, const double j_max, const TrajectoryPoints & input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) { double v_lim = input.at(i).longitudinal_velocity_mps; static constexpr double ep = 1.0e-5; @@ -408,6 +420,8 @@ TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter( const double v0, const double a0, const double a_min, const double a_stop, const double j_min, const TrajectoryPoints & input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + auto input_rev = input; std::reverse(input_rev.begin(), input_rev.end()); auto filtered = forwardJerkFilter( @@ -423,6 +437,8 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + TrajectoryPoints merged; merged = forward_filtered; @@ -475,6 +491,8 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, const double nearest_yaw_threshold) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + return resampling::resampleTrajectory( input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, smoother_param_.jerk_filter_ds); diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index b489c994b0495..f379b217187c9 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +L2PseudoJerkSmoother::L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index cb8451dba8302..2ab3d6dd80dfc 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +LinfPseudoJerkSmoother::LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 700cf45b7eb9d..46faf10fe4a62 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -60,7 +60,9 @@ TrajectoryPoints applyPreProcess( } } // namespace -SmootherBase::SmootherBase(rclcpp::Node & node) +SmootherBase::SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: time_keeper_(time_keeper) { auto & p = base_param_; p.max_accel = node.declare_parameter("normal.max_acc"); @@ -130,6 +132,8 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate lateral acc. do nothing. } @@ -198,6 +202,8 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate the desired velocity. do nothing. } From 780f9789de715848e57cfe3747fcdc7d81910f95 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 16 Jul 2024 19:30:24 +0900 Subject: [PATCH 5/7] feat(static_obstacle_avoidance): integrate time keeper to major functions (#8044) Signed-off-by: satoshi-ota Signed-off-by: Y.Hisaki --- .../src/scene.cpp | 42 ++++++++++++++++++- 1 file changed, 40 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 824466a1ad3ad..1d8e7ea6ce503 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -24,6 +24,7 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include #include #include @@ -194,6 +195,7 @@ bool StaticObstacleAvoidanceModule::canTransitSuccessState() void StaticObstacleAvoidanceModule::fillFundamentalData( AvoidancePlanningData & data, DebugData & debug) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference pose data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_->getPreviousSplineShiftPath()); @@ -308,8 +310,6 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using utils::static_obstacle_avoidance::fillAvoidanceNecessity; - using utils::static_obstacle_avoidance::fillObjectStoppableJudge; using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -371,9 +371,25 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( } } +void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const +{ + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + + // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); + std::for_each(objects.begin(), objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); + o.to_stop_line = calcDistanceToStopLine(o); + fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); + }); +} + ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using boost::geometry::return_centroid; const auto & path_points = data.reference_path.points; @@ -415,6 +431,7 @@ ObjectData StaticObstacleAvoidanceModule::createObjectData( bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_->isShifted()) { RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated."); @@ -468,6 +485,7 @@ bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData void StaticObstacleAvoidanceModule::fillShiftLine( AvoidancePlanningData & data, DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto path_shifter = path_shifter_; /** @@ -520,6 +538,7 @@ void StaticObstacleAvoidanceModule::fillShiftLine( void StaticObstacleAvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); data.state = getCurrentModuleState(data); /** @@ -614,6 +633,7 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( void StaticObstacleAvoidanceModule::fillDebugData( const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!data.stop_target_object) { return; } @@ -694,6 +714,7 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( bool StaticObstacleAvoidanceModule::isSafePath( ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data_->parameters; if (!parameters_->enable_safety_check) { @@ -802,6 +823,7 @@ bool StaticObstacleAvoidanceModule::isSafePath( PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( const PathWithLaneId & original_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto previous_path = helper_->getPreviousReferencePath(); const auto longest_dist_to_shift_point = [&]() { @@ -864,6 +886,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; resetPathCandidate(); @@ -1021,6 +1044,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; CandidateOutput output; @@ -1060,6 +1084,7 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); BehaviorModuleOutput out = plan(); if (path_shifter_.getShiftLines().empty()) { @@ -1116,6 +1141,7 @@ void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shi void StaticObstacleAvoidanceModule::addNewShiftLines( PathShifter & path_shifter, const AvoidLineArray & new_shift_lines) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); ShiftLineArray future = utils::static_obstacle_avoidance::toShiftLineArray(new_shift_lines); size_t min_start_idx = std::numeric_limits::max(); @@ -1182,6 +1208,7 @@ void StaticObstacleAvoidanceModule::addNewShiftLines( bool StaticObstacleAvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (shift_lines.empty()) { return true; } @@ -1257,6 +1284,7 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( void StaticObstacleAvoidanceModule::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::toShiftedPath; helper_->setData(planner_data_); @@ -1341,6 +1369,7 @@ void StaticObstacleAvoidanceModule::initRTCStatus() void StaticObstacleAvoidanceModule::updateRTCData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; updateRegisteredRTCStatus(helper_->getPreviousSplineShiftPath().path); @@ -1376,6 +1405,8 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { + using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray; + using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); @@ -1386,6 +1417,7 @@ void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData void StaticObstacleAvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); debug_marker_.markers.clear(); debug_marker_ = utils::static_obstacle_avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); @@ -1410,6 +1442,7 @@ void StaticObstacleAvoidanceModule::updateAvoidanceDebugData( double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = parameters_; const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -1451,6 +1484,7 @@ double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & void StaticObstacleAvoidanceModule::insertReturnDeadLine( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; if (data.to_return_point > planner_data_->parameters.forward_path_length) { @@ -1528,6 +1562,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( void StaticObstacleAvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // If avoidance path is NOT valid, don't insert any stop points. @@ -1576,6 +1611,7 @@ void StaticObstacleAvoidanceModule::insertWaitPoint( void StaticObstacleAvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; if (data.safe) { @@ -1623,6 +1659,7 @@ void StaticObstacleAvoidanceModule::insertStopPoint( void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // If avoidance path is NOT safe, don't insert any slow down sections. @@ -1735,6 +1772,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // do nothing if no shift line is approved. From b49bb5af5a10fd0c813452e62b4c55aae5679b5b Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 17 Jul 2024 16:35:37 +0900 Subject: [PATCH 6/7] feat(autoware_behavior_velocity_planner_common,autoware_behavior_velocity_stop_line_module): add time_keeper to bvp (#8070) Signed-off-by: Y.Hisaki --- .../scene_module_interface.hpp | 14 ++++++++++++++ .../src/scene_module_interface.cpp | 13 ++++++++++++- .../src/scene.cpp | 10 +++++++++- 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index b09f00ce367dc..29f3794516ef5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,7 @@ #include // Debug +#include #include #include @@ -106,6 +108,13 @@ class SceneModuleInterface infrastructure_command_ = command; } + void setTimeKeeper(const std::shared_ptr & time_keeper) + { + time_keeper_ = time_keeper; + } + + std::shared_ptr getTimeKeeper() { return time_keeper_; } + std::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } void setActivation(const bool activated) { activated_ = activated; } @@ -132,6 +141,7 @@ class SceneModuleInterface std::optional first_stop_path_point_index_; autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; + mutable std::shared_ptr time_keeper_; void setSafe(const bool safe) { @@ -215,6 +225,10 @@ class SceneModuleManagerInterface pub_infrastructure_commands_; std::shared_ptr processing_time_publisher_; + + rclcpp::Publisher::SharedPtr pub_processing_time_detail_; + + std::shared_ptr time_keeper_; }; class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index a0f30cd3e33cb..350bc439a4a41 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -17,9 +17,11 @@ #include #include #include +#include #include #include +#include namespace autoware::behavior_velocity_planner { @@ -33,7 +35,8 @@ SceneModuleInterface::SceneModuleInterface( safe_(false), distance_(std::numeric_limits::lowest()), logger_(logger), - clock_(clock) + clock_(clock), + time_keeper_(std::shared_ptr()) { } @@ -71,6 +74,11 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( "~/output/infrastructure_commands", 1); processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); } size_t SceneModuleManagerInterface::findEgoSegmentIndex( @@ -94,6 +102,8 @@ void SceneModuleManagerInterface::updateSceneModuleInstances( void SceneModuleManagerInterface::modifyPathVelocity( tier4_planning_msgs::msg::PathWithLaneId * path) { + universe_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; @@ -177,6 +187,7 @@ void SceneModuleManagerInterface::registerModule( RCLCPP_DEBUG( logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_module->setTimeKeeper(time_keeper_); scene_modules_.insert(scene_module); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 6075706a672d0..e3b585af4fb8b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -40,6 +41,8 @@ StopLineModule::StopLineModule( bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { + universe_utils::ScopedTimeTrack st( + std::string(__func__) + " (lane_id:=" + std::to_string(module_id_) + ")", *getTimeKeeper()); debug_data_ = DebugData(); if (path->points.empty()) return true; const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -50,11 +53,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop const LineString2d stop_line = planning_utils::extendLine( stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + time_keeper_->start_track("createTargetPoint"); // Calculate stop pose and insert index const auto stop_point = arc_lane_utils::createTargetPoint( *path, stop_line, lane_id_, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); - + time_keeper_->end_track("createTargetPoint"); // If no collision found, do nothing if (!stop_point) { RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); @@ -70,12 +74,16 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop * |----------------------------| * s---ego----------x--|--------g */ + time_keeper_->start_track( + "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength"); const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( path->points, stop_pose.position, stop_point_idx); const size_t current_seg_idx = findEgoSegmentIndex(path->points); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, current_seg_idx, stop_pose.position, stop_line_seg_idx); + time_keeper_->end_track( + "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength"); switch (state_) { case State::APPROACH: { // Insert stop pose From 843add6a73a6152f0be2e72b4ce31df18dce9e97 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Fri, 19 Jul 2024 17:09:59 +0900 Subject: [PATCH 7/7] resolve errors Signed-off-by: Y.Hisaki --- .../utils/base_class.hpp | 14 ++++---------- .../src/scene.cpp | 19 ++----------------- 2 files changed, 6 insertions(+), 27 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 49c8056681e2f..be1f328fd89df 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -155,17 +155,11 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } - void setData(const std::shared_ptr & data) + void setData(const std::shared_ptr & data) { planner_data_ = data; } + + void setTimeKeeper(const std::shared_ptr & time_keeper) { - planner_data_ = data; - if (!common_data_ptr_->bpp_param_ptr) { - common_data_ptr_->bpp_param_ptr = - std::make_shared(data->parameters); - } - common_data_ptr_->self_odometry_ptr = data->self_odometry; - common_data_ptr_->route_handler_ptr = data->route_handler; - common_data_ptr_->lc_param_ptr = lane_change_parameters_; - common_data_ptr_->direction = direction_; + time_keeper_ = time_keeper; } void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 1d8e7ea6ce503..b7ab41ec99abd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -310,6 +310,8 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -371,21 +373,6 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( } } -void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const -{ - using utils::static_obstacle_avoidance::fillAvoidanceNecessity; - using utils::static_obstacle_avoidance::fillObjectStoppableJudge; - - // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); - std::for_each(objects.begin(), objects.end(), [&, this](auto & o) { - fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); - o.to_stop_line = calcDistanceToStopLine(o); - fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); - }); -} - ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { @@ -1405,8 +1392,6 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { - using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray; - using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear();