diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 74e4de15becf3..7c921dac7db4c 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -13,12 +13,8 @@ knownConditionTrueFalse missingInclude missingIncludeSystem noConstructor -noExplicitConstructor -noValidConfiguration passedByValue -preprocessorErrorDirective redundantInitialization -shadowFunction shadowVariable // cspell: ignore uninit uninitMemberVar diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1f25372c14d71..8f947ce2d2a13 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -192,10 +192,10 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_modu planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp index 4175a04aadadc..e1220bf2c5879 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp @@ -32,7 +32,7 @@ struct VehicleKinematics using Message = autoware_adapi_v1_msgs::msg::VehicleKinematics; static constexpr char name[] = "/api/vehicle/kinematics"; static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; }; 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..efcb18844408c 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -7,3 +7,94 @@ 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. + +### TimeKeeper and ScopedTimeTrack + +#### `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 +#include +#include "autoware_universe_utils/time_keeper.hpp" + + +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("time_topic", 10); + time_keeper->add_reporter(publisher); + + time_keeper->start_track("example_function"); + // Do some processing here + time_keeper->end_track("example_function"); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +#### `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. 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..cfe8eb089729e --- /dev/null +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -0,0 +1,55 @@ +// 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); + + 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"); + }; + + funcB(); + + 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/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index 163bd761407c5..77e9294cb48c2 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -96,9 +96,7 @@ TEST_F(FakeNodeFixture, Test) } INSTANTIATE_TEST_SUITE_P( - FakeNodeFixtureTests, FakeNodeFixtureParametrized, - // cppcheck-suppress syntaxError // cppcheck doesn't like the trailing comma. - ::testing::Values(-5, 0, 42)); + FakeNodeFixtureTests, FakeNodeFixtureParametrized, ::testing::Values(-5, 0, 42)); /// @test Test that we can use a parametrized test. TEST_P(FakeNodeFixtureParametrized, Test) diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index caa89c79d08ea..4644ec6c3e019 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -37,7 +37,6 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { using autoware::common::osqp::calCSCMatrix; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index e5d7041469289..5ce1f0eb064a1 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -37,7 +37,6 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { using qp::calCSCMatrix; diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index 96466665d5172..c28678e5c872a 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -38,7 +38,6 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestProxqpInterface, BasicQp) { auto check_result = [](const auto & solution) { diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 74523398c3a77..91dc0386ae6c6 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -70,7 +70,8 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief callback on receiving a trajectory * @param [in] traj_msg received trajectory message */ - void onTrajectory(const Trajectory::ConstSharedPtr traj_msg); + void onTrajectory( + const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief callback on receiving a reference trajectory @@ -88,7 +89,9 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief callback on receiving a modified goal * @param [in] modified_goal_msg received modified goal message */ - void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg); + void onModifiedGoal( + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, + const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief publish the given metric statistic @@ -99,26 +102,43 @@ class PlanningEvaluatorNode : public rclcpp::Node /** * @brief publish current ego lane info */ - DiagnosticStatus generateLaneletDiagnosticStatus(); + DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief publish current ego kinematic state */ DiagnosticStatus generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped); + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); private: static bool isFinite(const TrajectoryPoint & p); - void publishModifiedGoalDeviationMetrics(); - // update Route Handler + + /** + * @brief update route handler data + */ void getRouteData(); + /** + * @brief fetch data and publish diagnostics + */ + void onTimer(); + + /** + * @brief fetch topic data + */ + void fetchData(); + // ROS - rclcpp::Subscription::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr ref_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr modified_goal_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ + this, "~/input/trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber ref_sub_{ + this, "~/input/reference_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ + this, "~/input/objects"}; + autoware::universe_utils::InterProcessPollingSubscriber modified_goal_sub_{ + this, "~/input/modified_goal"}; + autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ + this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ this, "~/input/route", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ @@ -131,6 +151,7 @@ class PlanningEvaluatorNode : public rclcpp::Node std::unique_ptr tf_buffer_; autoware::route_handler::RouteHandler route_handler_; + DiagnosticArray metrics_msg_; // Parameters std::string output_file_str_; std::string ego_frame_str_; @@ -142,8 +163,7 @@ class PlanningEvaluatorNode : public rclcpp::Node std::deque stamps_; std::array>, static_cast(Metric::SIZE)> metric_stats_; - Odometry::ConstSharedPtr ego_state_ptr_; - PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; + rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 5e959aed123af..9b65dbbc0b89e 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -35,26 +35,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op : Node("planning_evaluator", node_options) { using std::placeholders::_1; - - traj_sub_ = create_subscription( - "~/input/trajectory", 1, std::bind(&PlanningEvaluatorNode::onTrajectory, this, _1)); - - ref_sub_ = create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&PlanningEvaluatorNode::onReferenceTrajectory, this, _1)); - - objects_sub_ = create_subscription( - "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1)); - - modified_goal_sub_ = create_subscription( - "~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1)); - - odom_sub_ = create_subscription( - "~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1)); - tf_buffer_ = std::make_unique(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); + // Timer callback to publish evaluator diagnostics + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&PlanningEvaluatorNode::onTimer, this)); // Parameters metrics_calculator_.parameters.trajectory.min_point_dist_m = declare_parameter("trajectory.min_point_dist_m"); @@ -133,9 +120,10 @@ void PlanningEvaluatorNode::getRouteData() } } -DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus() +DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus( + const Odometry::ConstSharedPtr ego_state_ptr) { - const auto & ego_pose = ego_state_ptr_->pose.pose; + const auto & ego_pose = ego_state_ptr->pose.pose; const auto current_lanelets = [&]() { lanelet::ConstLanelet closest_route_lanelet; route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); @@ -166,14 +154,14 @@ DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus() } DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped) + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr) { DiagnosticStatus status; status.name = "kinematic_state"; status.level = status.OK; diagnostic_msgs::msg::KeyValue key_value; key_value.key = "vel"; - key_value.value = std::to_string(ego_state_ptr_->twist.twist.linear.x); + key_value.value = std::to_string(ego_state_ptr->twist.twist.linear.x); status.values.push_back(key_value); key_value.key = "acc"; const auto & acc = accel_stamped.accel.accel.linear.x; @@ -220,9 +208,40 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( return status; } -void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) +void PlanningEvaluatorNode::onTimer() +{ + metrics_msg_.header.stamp = now(); + + const auto ego_state_ptr = odometry_sub_.takeData(); + onOdometry(ego_state_ptr); + { + const auto objects_msg = objects_sub_.takeData(); + onObjects(objects_msg); + } + + { + const auto ref_traj_msg = ref_sub_.takeData(); + onReferenceTrajectory(ref_traj_msg); + } + + { + const auto traj_msg = traj_sub_.takeData(); + onTrajectory(traj_msg, ego_state_ptr); + } + { + const auto modified_goal_msg = modified_goal_sub_.takeData(); + onModifiedGoal(modified_goal_msg, ego_state_ptr); + } + if (!metrics_msg_.status.empty()) { + metrics_pub_->publish(metrics_msg_); + } + metrics_msg_ = DiagnosticArray{}; +} + +void PlanningEvaluatorNode::onTrajectory( + const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr) { - if (!ego_state_ptr_) { + if (!ego_state_ptr || !traj_msg) { return; } @@ -231,8 +250,6 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m stamps_.push_back(traj_msg->header.stamp); } - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); if (!metric_stat) { @@ -244,88 +261,71 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m } if (metric_stat->count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } + metrics_calculator_.setPreviousTrajectory(*traj_msg); auto runtime = (now() - start).seconds(); RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3); } void PlanningEvaluatorNode::onModifiedGoal( - const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg) + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, + const Odometry::ConstSharedPtr ego_state_ptr) { - modified_goal_ptr_ = modified_goal_msg; - if (ego_state_ptr_) { - publishModifiedGoalDeviationMetrics(); - } -} - -void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) -{ - ego_state_ptr_ = odometry_msg; - metrics_calculator_.setEgoPose(odometry_msg->pose.pose); - { - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); - - getRouteData(); - if (route_handler_.isHandlerReady() && ego_state_ptr_) { - metrics_msg.status.push_back(generateLaneletDiagnosticStatus()); - } - - const auto acc = accel_sub_.takeData(); - - if (acc && ego_state_ptr_) { - metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*acc)); - } - - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } - } - - if (modified_goal_ptr_) { - publishModifiedGoalDeviationMetrics(); + if (!modified_goal_msg || !ego_state_ptr) { + return; } -} - -void PlanningEvaluatorNode::publishModifiedGoalDeviationMetrics() -{ auto start = now(); - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { const auto metric_stat = metrics_calculator_.calculate( - Metric(metric), modified_goal_ptr_->pose, ego_state_ptr_->pose.pose); + Metric(metric), modified_goal_msg->pose, ego_state_ptr->pose.pose); if (!metric_stat) { continue; } metric_stats_[static_cast(metric)].push_back(*metric_stat); if (metric_stat->count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } auto runtime = (now() - start).seconds(); RCLCPP_DEBUG( get_logger(), "Planning evaluation modified goal deviation calculation time: %2.2f ms", runtime * 1e3); } +void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) +{ + if (!odometry_msg) return; + metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + { + getRouteData(); + if (route_handler_.isHandlerReady() && odometry_msg) { + metrics_msg_.status.push_back(generateLaneletDiagnosticStatus(odometry_msg)); + } + + const auto acc_msg = accel_sub_.takeData(); + if (acc_msg && odometry_msg) { + metrics_msg_.status.push_back(generateKinematicStateDiagnosticStatus(*acc_msg, odometry_msg)); + } + } +} + void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg) { + if (!traj_msg) { + return; + } metrics_calculator_.setReferenceTrajectory(*traj_msg); } void PlanningEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { + if (!objects_msg) { + return; + } metrics_calculator_.setPredictedObjects(*objects_msg); } diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index f23d9f74986e6..d8f3ef55b15dd 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -216,7 +216,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 373e343ad64fa..b3280fee47364 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -13,6 +13,7 @@ + @@ -163,6 +164,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 4c288aa632182..fba0144e6cee1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -4,6 +4,7 @@ + @@ -95,14 +96,24 @@ + + + + + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 684ecee757aad..ba0b058d8c00b 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -332,7 +332,7 @@ def create_time_series_outlier_filter_components(input_topic, output_topic): components.append( ComposableNode( package="occupancy_grid_map_outlier_filter", - plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", + plugin="autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", name="occupancy_grid_based_outlier_filter", remappings=[ ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 5d8d7563e7a7c..6c428a5adea0b 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -80,6 +80,7 @@ + + diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 48ce49d2bcaa9..1881deab23d05 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -241,18 +241,26 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching ### Abstract -Calculate 2D covariance (xx, xy, yx, yy) in real time using the NDT convergence from multiple initial poses. -The arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. +Initially, the covariance of NDT scan matching is a fixed value(FIXED_VALUE mode). +So, three modes are provided for 2D covariance (xx, xy, yx, yy) estimation in real time: LAPLACE_APPROXIMATION, MULTI_NDT, and MULTI_NDT_SCORE. +LAPLACE_APPROXIMATION calculates the inverse matrix of the XY (2x2) part of the Hessian obtained by NDT scan matching and uses it as the covariance matrix. +On the other hand, MULTI_NDT, and MULTI_NDT_SCORE use NDT convergence from multiple initial poses to obtain 2D covariance. +Ideally, the arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. In this implementation, the number of initial positions is fixed to simplify the code. +To obtain the covariance, MULTI_NDT computes until convergence at each initial position, while MULTI_NDT_SCORE uses the nearest voxel transformation likelihood. The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2. [original paper](https://www.fujipress.jp/jrm/rb/robot003500020435/). +drawing + Note that this function may spoil healthy system behavior if it consumes much calculation resources. ### Parameters +There are three types in the calculation of 2D covariance in real time.You can select the method by changing covariance_estimation_type. initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. +In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature. {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index f62329b8bdb6d..616cb108baf4a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -107,13 +107,18 @@ # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) covariance_estimation: - enable: false + # Covariance estimation type + # 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE + covariance_estimation_type: 0 # Offset arrangement in covariance estimation [m] # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance + temperature: 0.1 + dynamic_map_loading: # Dynamic map loading distance diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index e488b49393d48..d6bd975c36bf3 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -29,6 +29,13 @@ enum class ConvergedParamType { NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 }; +enum class CovarianceEstimationType { + FIXED_VALUE = 0, + LAPLACE_APPROXIMATION = 1, + MULTI_NDT = 2, + MULTI_NDT_SCORE = 3, +}; + struct HyperParameters { struct Frame @@ -80,8 +87,10 @@ struct HyperParameters struct CovarianceEstimation { - bool enable{}; - std::vector initial_pose_offset_model{}; + CovarianceEstimationType covariance_estimation_type{}; + std::vector initial_pose_offset_model_x{}; + std::vector initial_pose_offset_model_y{}; + double temperature{}; } covariance_estimation{}; } covariance{}; @@ -148,33 +157,27 @@ struct HyperParameters for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { covariance.output_pose_covariance[i] = output_pose_covariance[i]; } - covariance.covariance_estimation.enable = - node->declare_parameter("covariance.covariance_estimation.enable"); - if (covariance.covariance_estimation.enable) { - std::vector initial_pose_offset_model_x = - node->declare_parameter>( - "covariance.covariance_estimation.initial_pose_offset_model_x"); - std::vector initial_pose_offset_model_y = - node->declare_parameter>( - "covariance.covariance_estimation.initial_pose_offset_model_y"); - - if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { - const size_t size = initial_pose_offset_model_x.size(); - covariance.covariance_estimation.initial_pose_offset_model.resize(size); - for (size_t i = 0; i < size; i++) { - covariance.covariance_estimation.initial_pose_offset_model[i].x() = - initial_pose_offset_model_x[i]; - covariance.covariance_estimation.initial_pose_offset_model[i].y() = - initial_pose_offset_model_y[i]; - } - } else { - std::stringstream message; - message << "Invalid initial pose offset model parameters." - << "Please make sure that the number of elements in " - << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same."; - throw std::runtime_error(message.str()); - } + const int64_t covariance_estimation_type_tmp = node->declare_parameter( + "covariance.covariance_estimation.covariance_estimation_type"); + covariance.covariance_estimation.covariance_estimation_type = + static_cast(covariance_estimation_type_tmp); + covariance.covariance_estimation.initial_pose_offset_model_x = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_x"); + covariance.covariance_estimation.initial_pose_offset_model_y = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_y"); + if ( + covariance.covariance_estimation.initial_pose_offset_model_x.size() != + covariance.covariance_estimation.initial_pose_offset_model_y.size()) { + std::stringstream message; + message << "Invalid initial pose offset model parameters." + << "Please make sure that the number of elements in " + << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same."; + throw std::runtime_error(message.str()); } + covariance.covariance_estimation.temperature = + node->declare_parameter("covariance.covariance_estimation.temperature"); dynamic_map_loading.update_distance = node->declare_parameter("dynamic_map_loading.update_distance"); diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 2f3eb82b1c217..119c3534cab16 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -131,7 +131,7 @@ class NDTScanMatcher : public rclcpp::Node static int count_oscillation(const std::vector & result_pose_msg_array); - std::array estimate_covariance( + Eigen::Matrix2d estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); diff --git a/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png b/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png new file mode 100644 index 0000000000000..f6e25d04c0fd6 Binary files /dev/null and b/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png differ diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json index d978e5bcf7357..cbee3b5da72aa 100644 --- a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -5,10 +5,12 @@ "covariance_estimation": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).", - "default": false + "covariance_estimation_type": { + "type": "number", + "description": "2D Real-time Converged estimation type. 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE (FIXED_VALUE mode does not perform 2D Real-time Converged estimation)", + "default": 0, + "minimum": 0, + "maximum": 3 }, "initial_pose_offset_model_x": { "type": "array", @@ -19,9 +21,20 @@ "type": "array", "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + }, + "temperature": { + "type": "number", + "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance", + "default": 0.1, + "exclusiveMinimum": 0 } }, - "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"], + + "required": [ + "covariance_estimation_type", + "initial_pose_offset_model_x", + "initial_pose_offset_model_y" + ], "additionalProperties": false } } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index ca486a320d9dc..6d34f666b997b 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -21,6 +21,7 @@ #include #include +#include #include @@ -566,11 +567,17 @@ bool NDTScanMatcher::callback_sensor_points_main( std::array ndt_covariance = rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); - - if (is_converged && param_.covariance.covariance_estimation.enable) { - const auto estimated_covariance = + if ( + param_.covariance.covariance_estimation.covariance_estimation_type != + CovarianceEstimationType::FIXED_VALUE) { + const Eigen::Matrix2d estimated_covariance_2d = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); - ndt_covariance = estimated_covariance; + const Eigen::Matrix2d estimated_covariance_2d_adj = + pclomp::adjust_diagonal_covariance(estimated_covariance_2d, ndt_result.pose, 0.0225, 0.0225); + ndt_covariance[0 + 6 * 0] = estimated_covariance_2d_adj(0, 0); + ndt_covariance[1 + 6 * 1] = estimated_covariance_2d_adj(1, 1); + ndt_covariance[1 + 6 * 0] = estimated_covariance_2d_adj(1, 0); + ndt_covariance[0 + 6 * 1] = estimated_covariance_2d_adj(0, 1); } // check distance_initial_to_result @@ -807,7 +814,7 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } -std::array NDTScanMatcher::estimate_covariance( +Eigen::Matrix2d NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) { @@ -819,19 +826,9 @@ std::array NDTScanMatcher::estimate_covariance( std::stringstream message; message << "Error in Eigen solver: " << e.what(); RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - - return param_.covariance.output_pose_covariance; + return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0]; } - // first result is added to mean - const int n = - static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; - const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); - Eigen::Vector2d mean = ndt_pose_2d; - std::vector ndt_pose_2d_vec; - ndt_pose_2d_vec.reserve(n); - ndt_pose_2d_vec.emplace_back(ndt_pose_2d); - geometry_msgs::msg::PoseArray multi_ndt_result_msg; geometry_msgs::msg::PoseArray multi_initial_pose_msg; multi_ndt_result_msg.header.stamp = sensor_ros_time; @@ -841,47 +838,45 @@ std::array NDTScanMatcher::estimate_covariance( multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); - // multiple searches - for (const auto & pose_offset : - param_.covariance.covariance_estimation.initial_pose_offset_model) { - const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; - - Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); - sub_initial_pose_matrix = ndt_result.pose; - sub_initial_pose_matrix(0, 3) += static_cast(rotated_pose_offset_2d.x()); - sub_initial_pose_matrix(1, 3) += static_cast(rotated_pose_offset_2d.y()); - - auto sub_output_cloud = std::make_shared>(); - ndt_ptr_->align(*sub_output_cloud, sub_initial_pose_matrix); - const Eigen::Matrix4f sub_ndt_result = ndt_ptr_->getResult().pose; - - const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_result.topRightCorner<2, 1>().cast(); - mean += sub_ndt_pose_2d; - ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); - - multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(sub_ndt_result)); - multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); - } - - // calculate the covariance matrix - mean /= n; - Eigen::Matrix2d pca_covariance = Eigen::Matrix2d::Zero(); - for (const auto & temp_ndt_pose_2d : ndt_pose_2d_vec) { - const Eigen::Vector2d diff_2d = temp_ndt_pose_2d - mean; - pca_covariance += diff_2d * diff_2d.transpose(); + if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::LAPLACE_APPROXIMATION) { + return pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian); + } else if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::MULTI_NDT) { + const std::vector poses_to_search = pclomp::propose_poses_to_search( + ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x, + param_.covariance.covariance_estimation.initial_pose_offset_model_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_multi_ndt_covariance_estimation = + estimate_xy_covariance_by_multi_ndt(ndt_result, ndt_ptr_, poses_to_search); + for (size_t i = 0; i < result_of_multi_ndt_covariance_estimation.ndt_initial_poses.size(); + i++) { + multi_ndt_result_msg.poses.push_back( + matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_results[i].pose)); + multi_initial_pose_msg.poses.push_back( + matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_initial_poses[i])); + } + multi_ndt_pose_pub_->publish(multi_ndt_result_msg); + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + return result_of_multi_ndt_covariance_estimation.covariance; + } else if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::MULTI_NDT_SCORE) { + const std::vector poses_to_search = pclomp::propose_poses_to_search( + ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x, + param_.covariance.covariance_estimation.initial_pose_offset_model_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation + result_of_multi_ndt_score_covariance_estimation = estimate_xy_covariance_by_multi_ndt_score( + ndt_result, ndt_ptr_, poses_to_search, param_.covariance.covariance_estimation.temperature); + for (const auto & sub_initial_pose_matrix : poses_to_search) { + multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); + } + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + return result_of_multi_ndt_score_covariance_estimation.covariance; + } else { + return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0]; } - pca_covariance /= (n - 1); // unbiased covariance - - std::array ndt_covariance = param_.covariance.output_pose_covariance; - ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); - ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); - ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); - ndt_covariance[1 + 6 * 1] += pca_covariance(1, 1); - - multi_ndt_pose_pub_->publish(multi_ndt_result_msg); - multi_initial_pose_pub_->publish(multi_initial_pose_msg); - - return ndt_covariance; } void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 46f5165c5f4e2..4c33377c8ed0f 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -53,14 +53,13 @@ class UndistortNode : public rclcpp::Node qos = rclcpp::QoS(10).durability_volatile().best_effort(); } - auto on_image = std::bind(&UndistortNode::on_image, this, _1); - auto on_compressed_image = std::bind(&UndistortNode::on_compressed_image, this, _1); - auto on_info = std::bind(&UndistortNode::on_info, this, _1); - sub_image_ = create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_image_ = create_subscription( + "~/input/image_raw", qos, std::bind(&UndistortNode::on_image, this, _1)); sub_compressed_image_ = create_subscription( - "~/input/image_raw/compressed", qos, std::move(on_compressed_image)); - - sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); + "~/input/image_raw/compressed", qos, + std::bind(&UndistortNode::on_compressed_image, this, _1)); + sub_info_ = create_subscription( + "~/input/camera_info", qos, std::bind(&UndistortNode::on_info, this, _1)); pub_info_ = create_publisher("~/output/resized_info", 10); pub_image_ = create_publisher("~/output/resized_image", 10); diff --git a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp b/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp index 95ca3f3dfbea0..46baffa270108 100644 --- a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp +++ b/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp @@ -31,8 +31,12 @@ using geometry_msgs::msg::Point; class MapHeightFitter final { public: - MapHeightFitter(rclcpp::Node * node); + explicit MapHeightFitter(rclcpp::Node * node); ~MapHeightFitter(); + MapHeightFitter(const MapHeightFitter &) = delete; + MapHeightFitter & operator=(const MapHeightFitter &) = delete; + MapHeightFitter(MapHeightFitter &&) = delete; + MapHeightFitter & operator=(MapHeightFitter &&) = delete; std::optional fit(const Point & position, const std::string & frame); private: diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index d6a20a636733d..c9e199ad88422 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -29,6 +29,8 @@ #include #include +#include + namespace map_height_fitter { @@ -107,7 +109,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n void MapHeightFitter::Impl::on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { map_frame_ = msg->header.frame_id; - map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); + map_cloud_ = std::make_shared>(); pcl::fromROSMsg(*msg, *map_cloud_); } @@ -125,8 +127,8 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } const auto req = std::make_shared(); - req->area.center_x = point.x; - req->area.center_y = point.y; + req->area.center_x = static_cast(point.x); + req->area.center_y = static_cast(point.y); req->area.radius = 50; RCLCPP_DEBUG(logger, "Send request to map_loader"); @@ -157,7 +159,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } } map_frame_ = res->header.frame_id; - map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); + map_cloud_ = std::make_shared>(); pcl::fromROSMsg(pcd_msg, *map_cloud_); return true; } @@ -276,9 +278,7 @@ MapHeightFitter::MapHeightFitter(rclcpp::Node * node) impl_ = std::make_unique(node); } -MapHeightFitter::~MapHeightFitter() -{ -} +MapHeightFitter::~MapHeightFitter() = default; std::optional MapHeightFitter::fit(const Point & position, const std::string & frame) { diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 4e85ddec056c1..8bfdbc5d5560c 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -27,9 +27,6 @@ #include #include -using autoware_map_msgs::msg::LaneletMapBin; -using tier4_map_msgs::msg::MapProjectorInfo; - class Lanelet2MapLoaderNode : public rclcpp::Node { public: @@ -38,7 +35,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static LaneletMapBin create_map_bin_msg( + static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +45,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index cb640e4dc83d5..049d714ec452a 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -34,7 +34,7 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node bool viz_lanelets_centerline_; - void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); + void on_map_bin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp index 225445d17bfa1..d73ec0d1ee06e 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp @@ -30,7 +30,7 @@ class LocalProjector : public Projector return BasicPoint3d{0.0, 0.0, gps.ele}; } - GPSPoint reverse(const BasicPoint3d & point) const override + [[nodiscard]] GPSPoint reverse(const BasicPoint3d & point) const override { return GPSPoint{0.0, 0.0, point.z()}; } diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 7b712c7c281a2..e2b2a052619e4 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -51,6 +51,9 @@ #include +using autoware_map_msgs::msg::LaneletMapBin; +using tier4_map_msgs::msg::MapProjectorInfo; + Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) { @@ -105,13 +108,13 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { std::unique_ptr projector = geography_utils::get_lanelet2_projector(projector_info); - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; } } else { const lanelet::projection::LocalProjector projector; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); if (!errors.empty()) { for (const auto & error : errors) { @@ -150,7 +153,8 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { - std::string format_version{}, map_version{}; + std::string format_version{}; + std::string map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 87319222001d9..4bff12c640c30 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -51,18 +51,18 @@ namespace { -void insertMarkerArray( +void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) { a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) { - cl->r = r; - cl->g = g; - cl->b = b; - cl->a = a; + cl->r = static_cast(r); + cl->g = static_cast(g); + cl->b = static_cast(b); + cl->a = static_cast(a); } } // namespace @@ -75,13 +75,13 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), - std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); + std::bind(&Lanelet2MapVisualizationNode::on_map_bin, this, _1)); pub_marker_ = this->create_publisher( "output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local()); } -void Lanelet2MapVisualizationNode::onMapBin( +void Lanelet2MapVisualizationNode::on_map_bin( const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); @@ -103,8 +103,6 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); std::vector stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); - std::vector tl_reg_elems = - lanelet::utils::query::trafficLights(all_lanelets); std::vector aw_tl_reg_elems = lanelet::utils::query::autowareTrafficLights(all_lanelets); std::vector da_reg_elems = @@ -133,140 +131,158 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::noParkingAreas(all_lanelets); lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); - std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, - cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, - cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, - cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones, cl_intersection_area; - setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); - setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); - setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); - setColor(&cl_partitions, 0.25, 0.25, 0.25, 0.999); - setColor(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); - setColor(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); - setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); - setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); - setColor(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); - setColor(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); - setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); - setColor(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); - setColor(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); - setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); - setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); - setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); - setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); - setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); - setColor(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); + std_msgs::msg::ColorRGBA cl_road; + std_msgs::msg::ColorRGBA cl_shoulder; + std_msgs::msg::ColorRGBA cl_cross; + std_msgs::msg::ColorRGBA cl_partitions; + std_msgs::msg::ColorRGBA cl_pedestrian_markings; + std_msgs::msg::ColorRGBA cl_ll_borders; + std_msgs::msg::ColorRGBA cl_shoulder_borders; + std_msgs::msg::ColorRGBA cl_stoplines; + std_msgs::msg::ColorRGBA cl_trafficlights; + std_msgs::msg::ColorRGBA cl_detection_areas; + std_msgs::msg::ColorRGBA cl_speed_bumps; + std_msgs::msg::ColorRGBA cl_crosswalks; + std_msgs::msg::ColorRGBA cl_parking_lots; + std_msgs::msg::ColorRGBA cl_parking_spaces; + std_msgs::msg::ColorRGBA cl_lanelet_id; + std_msgs::msg::ColorRGBA cl_obstacle_polygons; + std_msgs::msg::ColorRGBA cl_no_stopping_areas; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area_for_run_out; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_area; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_line; + std_msgs::msg::ColorRGBA cl_no_parking_areas; + std_msgs::msg::ColorRGBA cl_curbstones; + std_msgs::msg::ColorRGBA cl_intersection_area; + set_color(&cl_road, 0.27, 0.27, 0.27, 0.999); + set_color(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); + set_color(&cl_cross, 0.27, 0.3, 0.27, 0.5); + set_color(&cl_partitions, 0.25, 0.25, 0.25, 0.999); + set_color(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); + set_color(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); + set_color(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); + set_color(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); + set_color(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); + set_color(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); + set_color(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); + set_color(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); + set_color(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); + set_color(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); + set_color(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); + set_color(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); + set_color(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + set_color(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); + set_color(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( pedestrian_polygon_markings, cl_pedestrian_markings)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( pedestrian_line_markings, cl_pedestrian_markings)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "walkway_lanelets", walkway_lanelets, cl_cross)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( road_lanelets, cl_ll_borders, viz_lanelets_centerline_)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( road_lanelets, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( crosswalk_lanelets, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker( crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( intersection_areas, cl_intersection_area)); diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index a8d380fd81b86..da42389dcc69f 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -14,20 +14,22 @@ #include "differential_map_loader_module.hpp" +#include + DifferentialMapLoaderModule::DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_differential_pcd_maps_service_ = node->create_service( "service/get_differential_pcd_map", std::bind( - &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, + &DifferentialMapLoaderModule::on_service_get_differential_point_cloud_map, this, std::placeholders::_1, std::placeholders::_2)); } -void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo & area, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const +void DifferentialMapLoaderModule::differential_area_load( + const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, + const GetDifferentialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids std::vector should_remove(static_cast(cached_ids.size()), true); @@ -36,18 +38,18 @@ void DifferentialMapLoaderModule::differentialAreaLoad( PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = path; // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; + if (!is_grid_within_queried_area(area_info, metadata)) continue; auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); if (id_in_cached_list != cached_ids.end()) { - int index = id_in_cached_list - cached_ids.begin(); + int index = static_cast(id_in_cached_list - cached_ids.begin()); should_remove[index] = false; } else { autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -63,19 +65,19 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } } -bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( +bool DifferentialMapLoaderModule::on_service_get_differential_point_cloud_map( GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res) + GetDifferentialPointCloudMap::Response::SharedPtr res) const { auto area = req->area; std::vector cached_ids = req->cached_ids; - differentialAreaLoad(area, cached_ids, res); + differential_area_load(area, cached_ids, res); res->header.frame_id = "map"; return true; } autoware_map_msgs::msg::PointCloudMapCellWithID -DifferentialMapLoaderModule::loadPointCloudMapCellWithID( +DifferentialMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 7069e1dbdf45c..690ffeca653c8 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -38,7 +38,7 @@ class DifferentialMapLoaderModule public: explicit DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -46,13 +46,13 @@ class DifferentialMapLoaderModule std::map all_pcd_file_metadata_dict_; rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; - bool onServiceGetDifferentialPointCloudMap( + [[nodiscard]] bool on_service_get_differential_point_cloud_map( GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res); - void differentialAreaLoad( + GetDifferentialPointCloudMap::Response::SharedPtr res) const; + void differential_area_load( const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 8c9378e9dfadb..62e165dd1005b 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -14,19 +14,22 @@ #include "partial_map_loader_module.hpp" +#include + PartialMapLoaderModule::PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_partial_pcd_maps_service_ = node->create_service( - "service/get_partial_pcd_map", std::bind( - &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); + "service/get_partial_pcd_map", + std::bind( + &PartialMapLoaderModule::on_service_get_partial_point_cloud_map, this, std::placeholders::_1, + std::placeholders::_2)); } -void PartialMapLoaderModule::partialAreaLoad( +void PartialMapLoaderModule::partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const + const GetPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids for (const auto & ele : all_pcd_file_metadata_dict_) { @@ -34,13 +37,13 @@ void PartialMapLoaderModule::partialAreaLoad( PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = path; // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; + if (!is_grid_within_queried_area(area, metadata)) continue; autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -50,16 +53,18 @@ void PartialMapLoaderModule::partialAreaLoad( } } -bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( - GetPartialPointCloudMap::Request::SharedPtr req, GetPartialPointCloudMap::Response::SharedPtr res) +bool PartialMapLoaderModule::on_service_get_partial_point_cloud_map( + GetPartialPointCloudMap::Request::SharedPtr req, + GetPartialPointCloudMap::Response::SharedPtr res) const { auto area = req->area; - partialAreaLoad(area, res); + partial_area_load(area, res); res->header.frame_id = "map"; return true; } -autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( +autoware_map_msgs::msg::PointCloudMapCellWithID +PartialMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 4d97ab90667ec..ec97661366419 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -38,7 +38,7 @@ class PartialMapLoaderModule public: explicit PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -46,13 +46,13 @@ class PartialMapLoaderModule std::map all_pcd_file_metadata_dict_; rclcpp::Service::SharedPtr get_partial_pcd_maps_service_; - bool onServiceGetPartialPointCloudMap( + [[nodiscard]] bool on_service_get_partial_point_cloud_map( GetPartialPointCloudMap::Request::SharedPtr req, - GetPartialPointCloudMap::Response::SharedPtr res); - void partialAreaLoad( + GetPartialPointCloudMap::Response::SharedPtr res) const; + void partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const GetPartialPointCloudMap::Response::SharedPtr & response) const; + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index a66f9ee99534c..1754d0b7629a2 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -50,10 +50,10 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( sensor_msgs::msg::PointCloud2 pcd; if (use_downsample) { - const float leaf_size = node->declare_parameter("leaf_size"); - pcd = loadPCDFiles(pcd_paths, leaf_size); + const float leaf_size = static_cast(node->declare_parameter("leaf_size")); + pcd = load_pcd_files(pcd_paths, leaf_size); } else { - pcd = loadPCDFiles(pcd_paths, boost::none); + pcd = load_pcd_files(pcd_paths, boost::none); } if (pcd.width == 0) { @@ -65,7 +65,7 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( pub_pointcloud_map_->publish(pcd); } -sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( +sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::load_pcd_files( const std::vector & pcd_paths, const boost::optional leaf_size) const { sensor_msgs::msg::PointCloud2 whole_pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 6a87643b57bff..44f23ded70e37 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -39,7 +39,7 @@ class PointcloudMapLoaderModule rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - sensor_msgs::msg::PointCloud2 loadPCDFiles( + [[nodiscard]] sensor_msgs::msg::PointCloud2 load_pcd_files( const std::vector & pcd_paths, const boost::optional leaf_size) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 349fc2954fe28..c718b25c56694 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -28,7 +28,7 @@ namespace fs = std::filesystem; namespace { -bool isPcdFile(const std::string & p) +bool is_pcd_file(const std::string & p) { if (fs::is_directory(p)) { return false; @@ -36,11 +36,7 @@ bool isPcdFile(const std::string & p) const std::string ext = fs::path(p).extension(); - if (ext != ".pcd" && ext != ".PCD") { - return false; - } - - return true; + return !(ext != ".pcd" && ext != ".PCD"); } } // namespace @@ -48,7 +44,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt : Node("pointcloud_map_loader", options) { const auto pcd_paths = - getPcdPaths(declare_parameter>("pcd_paths_or_directory")); + get_pcd_paths(declare_parameter>("pcd_paths_or_directory")); std::string pcd_metadata_path = declare_parameter("pcd_metadata_path"); bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); @@ -68,7 +64,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } // Parse the metadata file and get the map of (absolute pcd path, pcd file metadata) - auto pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + auto pcd_metadata_dict = get_pcd_metadata(pcd_metadata_path, pcd_paths); if (enable_partial_load) { partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); @@ -81,14 +77,14 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } } -std::map PointCloudMapLoaderNode::getPCDMetadata( +std::map PointCloudMapLoaderNode::get_pcd_metadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const { if (fs::exists(pcd_metadata_path)) { std::set missing_pcd_names; - auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); + auto pcd_metadata_dict = load_pcd_metadata(pcd_metadata_path); - pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths, missing_pcd_names); + pcd_metadata_dict = replace_with_absolute_path(pcd_metadata_dict, pcd_paths, missing_pcd_names); // Warning if some segments are missing if (!missing_pcd_names.empty()) { @@ -96,7 +92,7 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( oss << "The following segment(s) are missing from the input PCDs: "; - for (auto & fname : missing_pcd_names) { + for (const auto & fname : missing_pcd_names) { oss << std::endl << fname; } @@ -105,7 +101,9 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( } return pcd_metadata_dict; - } else if (pcd_paths.size() == 1) { + } + + if (pcd_paths.size() == 1) { // An exception when using a single file PCD map so that the users do not have to provide // a metadata file. // Note that this should ideally be avoided and thus eventually be removed by someone, until @@ -119,12 +117,11 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( PCDFileMetadata metadata = {}; pcl::getMinMax3D(single_pcd, metadata.min, metadata.max); return std::map{{pcd_path, metadata}}; - } else { - throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); } + throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); } -std::vector PointCloudMapLoaderNode::getPcdPaths( +std::vector PointCloudMapLoaderNode::get_pcd_paths( const std::vector & pcd_paths_or_directory) const { std::vector pcd_paths; @@ -133,14 +130,14 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); } - if (isPcdFile(p)) { + if (is_pcd_file(p)) { pcd_paths.push_back(p); } if (fs::is_directory(p)) { for (const auto & file : fs::directory_iterator(p)) { const auto filename = file.path().string(); - if (isPcdFile(filename)) { + if (is_pcd_file(filename)) { pcd_paths.push_back(filename); } } diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index fd9b297a9e3f4..dbc0d584d347b 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -45,9 +45,9 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::unique_ptr differential_map_loader_; std::unique_ptr selected_map_loader_; - std::vector getPcdPaths( + std::vector get_pcd_paths( const std::vector & pcd_paths_or_directory) const; - std::map getPCDMetadata( + std::map get_pcd_metadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp index 3e7b046f9d178..76b56341b8632 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp @@ -13,9 +13,11 @@ // limitations under the License. #include "selected_map_loader_module.hpp" + +#include namespace { -autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( +autoware_map_msgs::msg::PointCloudMapMetaData create_metadata( const std::map & pcd_file_metadata_dict) { autoware_map_msgs::msg::PointCloudMapMetaData metadata_msg; @@ -23,11 +25,10 @@ autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( metadata_msg.header.stamp = rclcpp::Clock().now(); for (const auto & ele : pcd_file_metadata_dict) { - std::string path = ele.first; PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = ele.first; autoware_map_msgs::msg::PointCloudMapCellMetaDataWithID cell_metadata_with_id; cell_metadata_with_id.cell_id = map_id; @@ -44,23 +45,24 @@ autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( } // namespace SelectedMapLoaderModule::SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_selected_pcd_maps_service_ = node->create_service( - "service/get_selected_pcd_map", std::bind( - &SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); + "service/get_selected_pcd_map", + std::bind( + &SelectedMapLoaderModule::on_service_get_selected_point_cloud_map, this, + std::placeholders::_1, std::placeholders::_2)); // publish the map metadata rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); pub_metadata_ = node->create_publisher( "output/pointcloud_map_metadata", durable_qos); - pub_metadata_->publish(createMetadata(all_pcd_file_metadata_dict_)); + pub_metadata_->publish(create_metadata(all_pcd_file_metadata_dict_)); } -bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( +bool SelectedMapLoaderModule::on_service_get_selected_point_cloud_map( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const { @@ -76,11 +78,11 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( const std::string path = requested_selected_map_iterator->first; // assume that the map ID = map path (for now) - const std::string map_id = path; + const std::string & map_id = path; PCDFileMetadata metadata = requested_selected_map_iterator->second; autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -93,7 +95,7 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( } autoware_map_msgs::msg::PointCloudMapCellWithID -SelectedMapLoaderModule::loadPointCloudMapCellWithID( +SelectedMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp index f44d549a0f576..eea8b8c1950ae 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp @@ -39,7 +39,7 @@ class SelectedMapLoaderModule public: explicit SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -49,10 +49,10 @@ class SelectedMapLoaderModule rclcpp::Publisher::SharedPtr pub_metadata_; - bool onServiceGetSelectedPointCloudMap( + [[nodiscard]] bool on_service_get_selected_point_cloud_map( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 96f9d114ed265..ea2c2e7033014 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -20,7 +20,7 @@ #include #include -std::map loadPCDMetadata(const std::string & pcd_metadata_path) +std::map load_pcd_metadata(const std::string & pcd_metadata_path) { YAML::Node config = YAML::LoadFile(pcd_metadata_path); @@ -33,22 +33,22 @@ std::map loadPCDMetadata(const std::string & pcd_m continue; } - std::string key = node.first.as(); - std::vector values = node.second.as>(); + auto key = node.first.as(); + auto values = node.second.as>(); - PCDFileMetadata fileMetadata; - fileMetadata.min.x = values[0]; - fileMetadata.min.y = values[1]; - fileMetadata.max.x = values[0] + config["x_resolution"].as(); - fileMetadata.max.y = values[1] + config["y_resolution"].as(); + PCDFileMetadata file_metadata; + file_metadata.min.x = static_cast(values[0]); + file_metadata.min.y = static_cast(values[1]); + file_metadata.max.x = static_cast(values[0]) + config["x_resolution"].as(); + file_metadata.max.y = static_cast(values[1]) + config["y_resolution"].as(); - metadata[key] = fileMetadata; + metadata[key] = file_metadata; } return metadata; } -std::map replaceWithAbsolutePath( +std::map replace_with_absolute_path( const std::map & pcd_metadata_path, const std::vector & pcd_paths, std::set & missing_pcd_names) { @@ -75,7 +75,7 @@ std::map replaceWithAbsolutePath( return absolute_path_map; } -bool cylinderAndBoxOverlapExists( +bool cylinder_and_box_overlap_exists( const double center_x, const double center_y, const double radius, const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point) { @@ -92,22 +92,18 @@ bool cylinderAndBoxOverlapExists( const double dy0 = center_y - box_min_point.y; const double dy1 = center_y - box_max_point.y; - if ( - std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius || - std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius) { - return true; - } - - return false; + return std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius || + std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius; } -bool isGridWithinQueriedArea( +bool is_grid_within_queried_area( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) { // Currently, the area load only supports cylindrical area double center_x = area.center_x; double center_y = area.center_y; double radius = area.radius; - bool res = cylinderAndBoxOverlapExists(center_x, center_y, radius, metadata.min, metadata.max); + bool res = + cylinder_and_box_overlap_exists(center_x, center_y, radius, metadata.min, metadata.max); return res; } diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 29d9a24d7b0e7..07e8ade5c6f7c 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -37,15 +37,15 @@ struct PCDFileMetadata } }; -std::map loadPCDMetadata(const std::string & pcd_metadata_path); -std::map replaceWithAbsolutePath( +std::map load_pcd_metadata(const std::string & pcd_metadata_path); +std::map replace_with_absolute_path( const std::map & pcd_metadata_path, const std::vector & pcd_paths, std::set & missing_pcd_names); -bool cylinderAndBoxOverlapExists( +bool cylinder_and_box_overlap_exists( const double center_x, const double center_y, const double radius, - const pcl::PointXYZ position_min, const pcl::PointXYZ position_max); -bool isGridWithinQueriedArea( + const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point); +bool is_grid_within_queried_area( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata); #endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ diff --git a/map/map_loader/test/test_cylinder_box_overlap.cpp b/map/map_loader/test/test_cylinder_box_overlap.cpp index 4c9b18a3dbd8e..d8ca2ca9f8734 100644 --- a/map/map_loader/test/test_cylinder_box_overlap.cpp +++ b/map/map_loader/test/test_cylinder_box_overlap.cpp @@ -35,7 +35,7 @@ TEST(CylinderAndBoxOverlapExists, NoOverlap) p_max.y = 1.0; p_max.z = 1.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_FALSE(result); } @@ -57,7 +57,7 @@ TEST(CylinderAndBoxOverlapExists, Overlap1) p_max.y = 1.0; p_max.z = 1.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_TRUE(result); } @@ -79,6 +79,6 @@ TEST(CylinderAndBoxOverlapExists, Overlap2) p_max.y = 1.0; p_max.z = -99.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_TRUE(result); } diff --git a/map/map_loader/test/test_load_pcd_metadata.cpp b/map/map_loader/test/test_load_pcd_metadata.cpp index a832489b6db99..fcec100f389c5 100644 --- a/map/map_loader/test/test_load_pcd_metadata.cpp +++ b/map/map_loader/test/test_load_pcd_metadata.cpp @@ -21,7 +21,7 @@ using ::testing::ContainerEq; -std::string createYAMLFile() +std::string create_yaml_file() { std::filesystem::path tmp_path = std::filesystem::temp_directory_path() / "temp_metadata.yaml"; @@ -37,14 +37,14 @@ std::string createYAMLFile() TEST(LoadPCDMetadataTest, BasicFunctionality) { - std::string yaml_file_path = createYAMLFile(); + std::string yaml_file_path = create_yaml_file(); std::map expected = { {"file1.pcd", {{1, 2, 0}, {6, 8, 0}}}, {"file2.pcd", {{3, 4, 0}, {8, 10, 0}}}, }; - auto result = loadPCDMetadata(yaml_file_path); + auto result = load_pcd_metadata(yaml_file_path); ASSERT_THAT(result, ContainerEq(expected)); } diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp index 2b686dc0fe8c3..5667f476b4dab 100644 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -26,8 +26,6 @@ #include #include -using std::chrono_literals::operator""ms; - class TestPointcloudMapLoaderModule : public ::testing::Test { protected: @@ -61,6 +59,8 @@ class TestPointcloudMapLoaderModule : public ::testing::Test TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) { + using namespace std::literals::chrono_literals; + // Prepare PCD paths std::vector pcd_paths = {temp_pcd_path}; diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/map/map_loader/test/test_replace_with_absolute_path.cpp index f61dd188f0679..03d533d41cf18 100644 --- a/map/map_loader/test/test_replace_with_absolute_path.cpp +++ b/map/map_loader/test/test_replace_with_absolute_path.cpp @@ -37,7 +37,7 @@ TEST(ReplaceWithAbsolutePathTest, BasicFunctionality) }; std::set missing_pcd_names; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); + auto result = replace_with_absolute_path(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } @@ -56,7 +56,7 @@ TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles) std::map expected = {}; std::set missing_pcd_names; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); + auto result = replace_with_absolute_path(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 2b8358eb52e4e..4f223d5c99bd6 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -1309,10 +1309,14 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { - const lanelet::ConstLineStrings3d & all_fences = - lanelet::utils::query::getAllFences(lanelet_map_ptr_); - for (const auto & fence_line : all_fences) { - if (doesPathCrossFence(predicted_path, fence_line)) { + lanelet::BasicLineString2d predicted_path_ls; + for (const auto & p : predicted_path.path) + predicted_path_ls.emplace_back(p.position.x, p.position.y); + const auto candidates = + lanelet_map_ptr_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls)); + for (const auto & candidate : candidates) { + const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "fence" && doesPathCrossFence(predicted_path, candidate)) { return true; } } diff --git a/perception/detected_object_feature_remover/CMakeLists.txt b/perception/detected_object_feature_remover/CMakeLists.txt index b3dd3c35e19d5..e0ff35d75e64a 100644 --- a/perception/detected_object_feature_remover/CMakeLists.txt +++ b/perception/detected_object_feature_remover/CMakeLists.txt @@ -4,13 +4,13 @@ project(detected_object_feature_remover) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(detected_object_feature_remover_node SHARED - src/detected_object_feature_remover.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/detected_object_feature_remover_node.cpp ) -rclcpp_components_register_node(detected_object_feature_remover_node - PLUGIN "detected_object_feature_remover::DetectedObjectFeatureRemover" - EXECUTABLE detected_object_feature_remover +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::detected_object_feature_remover::DetectedObjectFeatureRemover" + EXECUTABLE detected_object_feature_remover_node ) ament_auto_package( diff --git a/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml b/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml index 2e7fa4605d656..d86d75ce297be 100644 --- a/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml +++ b/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp similarity index 86% rename from perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp rename to perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp index 3fe9e6fde8683..47fe67de9d9aa 100644 --- a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "detected_object_feature_remover_node.hpp" -namespace detected_object_feature_remover +namespace autoware::detected_object_feature_remover { DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOptions & node_options) : Node("detected_object_feature_remover", node_options) @@ -45,7 +45,8 @@ void DetectedObjectFeatureRemover::convert( } } -} // namespace detected_object_feature_remover +} // namespace autoware::detected_object_feature_remover #include -RCLCPP_COMPONENTS_REGISTER_NODE(detected_object_feature_remover::DetectedObjectFeatureRemover) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_feature_remover::DetectedObjectFeatureRemover) diff --git a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp similarity index 71% rename from perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp rename to perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp index 02e4c9c3ca29a..2877d38259f83 100644 --- a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ -#define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ +#ifndef DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ +#define DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ + +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" #include -namespace detected_object_feature_remover +namespace autoware::detected_object_feature_remover { using autoware_perception_msgs::msg::DetectedObjects; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; @@ -41,6 +42,6 @@ class DetectedObjectFeatureRemover : public rclcpp::Node void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs); }; -} // namespace detected_object_feature_remover +} // namespace autoware::detected_object_feature_remover -#endif // DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ +#endif // DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index b6dfbaf402cc8..6be7defe422c3 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -172,6 +172,13 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_geometry test/test_geometry.cpp ) + # test needed cuda, tensorRT and cudnn + if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + ament_auto_add_gtest(test_pointpainting + test/test_pointpainting_fusion.cpp + ) + endif() + endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 2120a909cd672..fdabb0c7055d8 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -1,39 +1,24 @@ /**: ros__parameters: # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: - [ - true, # road - true, # sidewalk - true, # building - true, # wall - true, # fence - true, # pole - true, # traffic_light - true, # traffic_sign - true, # vegetation - true, # terrain - true, # sky - false, # person - false, # ride - false, # car - false, # truck - false, # bus - false, # train - false, # motorcycle - false, # bicycle - false, # others - ] - # the maximum distance of pointcloud to be applied filter, - # this is selected based on semantic segmentation model accuracy, - # calibration accuracy and unknown reaction distance - filter_distance_threshold: 60.0 + UNKNOWN: false + BUILDING: true + WALL: true + OBSTACLE: false + TRAFFIC_LIGHT: false + TRAFFIC_SIGN: false + PERSON: false + VEHICLE: false + BIKE: false + ROAD: true + SIDEWALK: false + ROAD_PAINT: false + CURBSTONE: false + CROSSWALK: false + VEGETATION: true + SKY: false - # debug - debug_mode: false - filter_scope_min_x: -100.0 - filter_scope_max_x: 100.0 - filter_scope_min_y: -100.0 - filter_scope_max_y: 100.0 - filter_scope_min_z: -100.0 - filter_scope_max_z: 100.0 + # the maximum distance of pointcloud to be applied filter + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md index d59e804f1228c..3c469ac15c6e7 100644 --- a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -32,9 +32,7 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud ### Core Parameters -| Name | Type | Description | -| ------------- | ---- | ------------------------ | -| `rois_number` | int | the number of input rois | +{{ json_to_markdown("perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }} ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index a4e6f315a36d6..756cb4224bc20 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,6 +33,14 @@ namespace image_projection_based_fusion { using Label = autoware_perception_msgs::msg::ObjectClassification; +inline bool isInsideBbox( + float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) +{ + // z_c is scaling to normalize projection point + return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && + proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; +} + class PointPaintingFusionNode : public FusionNode { diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 26bcfd5ed802a..89b33775f7898 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #if __has_include() @@ -36,7 +37,13 @@ class SegmentPointCloudFusionNode : public FusionNode::SharedPtr pub_pointcloud_ptr_; std::vector filter_semantic_label_target_; float filter_distance_threshold_; - /* data */ + // declare list of semantic label target, depend on trained data of yolox segmentation model + std::vector> filter_semantic_label_target_list_ = { + {"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false}, + {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false}, + {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false}, + {"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}}; + public: explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json new file mode 100644 index 0000000000000..f5ab1be5eac34 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json @@ -0,0 +1,134 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Segmentation Pointcloud Fusion Node", + "type": "object", + "definitions": { + "segmentation_pointcloud_fusion": { + "type": "object", + "properties": { + "filter_semantic_label_target": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "description": "If true, UNKNOWN class of semantic will be filtered.", + "default": false + }, + "BUILDING": { + "type": "boolean", + "description": "If true, BUILDING class of semantic will be filtered.", + "default": true + }, + "WALL": { + "type": "boolean", + "description": "If true, WALL class of semantic will be filtered.", + "default": true + }, + "OBSTACLE": { + "type": "boolean", + "description": "If true, OBSTACLE class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_LIGHT": { + "type": "boolean", + "description": "If true, TRAFFIC_LIGHT class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_SIGN": { + "type": "boolean", + "description": "If true, TRAFFIC_SIGN class of semantic will be filtered.", + "default": false + }, + "PERSON": { + "type": "boolean", + "description": "If true, PERSON class of semantic will be filtered.", + "default": false + }, + "VEHICLE": { + "type": "boolean", + "description": "If true, VEHICLE class of semantic will be filtered.", + "default": false + }, + "BIKE": { + "type": "boolean", + "description": "If true, BIKE class of semantic will be filtered.", + "default": false + }, + "ROAD": { + "type": "boolean", + "description": "If true, ROAD class of semantic will be filtered.", + "default": true + }, + "SIDEWALK": { + "type": "boolean", + "description": "If true, SIDEWALK class of semantic will be filtered.", + "default": false + }, + "ROAD_PAINT": { + "type": "boolean", + "description": "If true, ROAD_PAINT class of semantic will be filtered.", + "default": false + }, + "CURBSTONE": { + "type": "boolean", + "description": "If true, CURBSTONE class of semantic will be filtered.", + "default": false + }, + "CROSSWALK": { + "type": "boolean", + "description": "If true, CROSSWALK class of semantic will be filtered.", + "default": false + }, + "VEGETATION": { + "type": "boolean", + "description": "If true, VEGETATION class of semantic will be filtered.", + "default": true + }, + "SKY": { + "type": "boolean", + "description": "If true, SKY class of semantic will be filtered.", + "default": false + } + }, + "required": [ + "UNKNOWN", + "BUILDING", + "WALL", + "OBSTACLE", + "TRAFFIC_LIGHT", + "TRAFFIC_SIGN", + "PERSON", + "VEHICLE", + "BIKE", + "ROAD", + "SIDEWALK", + "ROAD_PAINT", + "CURBSTONE", + "CROSSWALK", + "VEGETATION", + "SKY" + ] + }, + "filter_distance_threshold": { + "type": "number", + "description": "A maximum distance of pointcloud to apply filter [m].", + "default": 60.0, + "minimum": 0.0 + } + }, + "required": ["filter_semantic_label_target", "filter_distance_threshold"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/segmentation_pointcloud_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index a7c83ac7614a8..0f3a50628ccbc 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -45,13 +45,6 @@ Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) namespace image_projection_based_fusion { -inline bool isInsideBbox( - float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) -{ - return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && - proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; -} - inline bool isVehicle(int label2d) { return label2d == autoware_perception_msgs::msg::ObjectClassification::CAR || diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index a3117125a46b1..9a983252af436 100644 --- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -31,8 +31,13 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio : FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); - filter_semantic_label_target_ = - declare_parameter>("filter_semantic_label_target"); + for (auto & item : filter_semantic_label_target_list_) { + item.second = declare_parameter("filter_semantic_label_target." + item.first); + } + for (const auto & item : filter_semantic_label_target_list_) { + RCLCPP_INFO( + this->get_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second); + } } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) @@ -129,12 +134,12 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( // skip filtering pointcloud where semantic id out of the defined list uint8_t semantic_id = mask.at( static_cast(projected_point.y()), static_cast(projected_point.x())); - if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + if (static_cast(semantic_id) >= filter_semantic_label_target_list_.size()) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } - if (!filter_semantic_label_target_.at(semantic_id)) { + if (!filter_semantic_label_target_list_.at(semantic_id).second) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); } diff --git a/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp new file mode 100644 index 0000000000000..1c69273667997 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp @@ -0,0 +1,56 @@ +// 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 + +#include + +sensor_msgs::msg::RegionOfInterest createRoi( + const int32_t x_offset, const int32_t y_offset, const int32_t width, const int32_t height) +{ + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = x_offset; + roi.y_offset = y_offset; + roi.width = width; + roi.height = height; + return roi; +} + +TEST(isInsideBboxTest, Inside) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(25.0, 25.0, roi, 1.0); + EXPECT_TRUE(result); +} + +TEST(isInsideBboxTest, Border) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(20.0, 30.0, roi, 1.0); + EXPECT_TRUE(result); +} + +TEST(isInsideBboxTest, Outside) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(15.0, 15.0, roi, 1.0); + EXPECT_FALSE(result); +} + +TEST(isInsideBboxTest, Zero) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(0, 0, 0, 0); + bool result = image_projection_based_fusion::isInsideBbox(0.0, 0.0, roi, 1.0); + EXPECT_TRUE(result); +} diff --git a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt index 724e5a3354396..9754cea458577 100644 --- a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt +++ b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt @@ -8,7 +8,6 @@ find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) find_package(OpenMP) ament_auto_find_build_dependencies() @@ -22,26 +21,26 @@ include_directories( ${GRID_MAP_INCLUDE_DIR} ) -ament_auto_add_library(occupancy_grid_map_outlier_filter SHARED - src/occupancy_grid_map_outlier_filter_nodelet.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/occupancy_grid_map_outlier_filter_node.cpp ) -target_link_libraries(occupancy_grid_map_outlier_filter +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) if(OPENMP_FOUND) - set_target_properties(occupancy_grid_map_outlier_filter PROPERTIES + set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${OpenMP_CXX_FLAGS} LINK_FLAGS ${OpenMP_CXX_FLAGS} ) endif() # -- Occupancy Grid Map Outlier Filter -- -rclcpp_components_register_node(occupancy_grid_map_outlier_filter - PLUGIN "occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" EXECUTABLE occupancy_grid_map_outlier_filter_node) ament_auto_package(INSTALL_TO_SHARE) diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index 23d08db09553a..e92e5edb4b084 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -19,27 +19,18 @@ autoware_lanelet2_extension autoware_universe_utils - autoware_vehicle_msgs - diagnostic_updater - image_transport - libopencv-dev libpcl-all-dev message_filters nav_msgs pcl_conversions - pcl_msgs pcl_ros pointcloud_preprocessor rclcpp rclcpp_components sensor_msgs std_msgs - tf2 tf2_eigen - tf2_geometry_msgs tf2_ros - tier4_debug_msgs - tier4_pcl_extensions ament_lint_auto autoware_lint_common diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp similarity index 97% rename from perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp rename to perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index f62a3f7e689fc..1536db8610b76 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" +#include "occupancy_grid_map_outlier_filter_node.hpp" + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" -#include -#include -#include #include #include @@ -101,7 +102,7 @@ boost::optional getCost( } // namespace -namespace occupancy_grid_map_outlier_filter +namespace autoware::occupancy_grid_map_outlier_filter { RadiusSearch2dFilter::RadiusSearch2dFilter(rclcpp::Node & node) { @@ -489,8 +490,8 @@ void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink( transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output); } -} // namespace occupancy_grid_map_outlier_filter +} // namespace autoware::occupancy_grid_map_outlier_filter #include RCLCPP_COMPONENTS_REGISTER_NODE( - occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent) + autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent) diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp similarity index 91% rename from perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp rename to perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp index ae6d7a34fca2a..e9402d6416cbd 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ -#define OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#ifndef OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ +#define OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ +#include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include #include #include @@ -40,7 +40,7 @@ #include #include -namespace occupancy_grid_map_outlier_filter +namespace autoware::occupancy_grid_map_outlier_filter { using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; @@ -129,6 +129,6 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node std::string base_link_frame_; int cost_threshold_; }; -} // namespace occupancy_grid_map_outlier_filter +} // namespace autoware::occupancy_grid_map_outlier_filter -#endif // OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#endif // OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp index 6ac681eff4916..7e2c3f7ba304a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp @@ -171,7 +171,7 @@ struct dempsterShaferOccupancy } // initialize with probability - dempsterShaferOccupancy(double occupied_probability) + explicit dempsterShaferOccupancy(double occupied_probability) { // confine to [0, 1] double p = std::max(0.0, std::min(1.0, occupied_probability)); diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 3846b3a7d16e7..03d7a42ae7043 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -75,7 +75,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( // Create angle bins and sort points by range struct BinInfo3D { - BinInfo3D( + explicit BinInfo3D( const double _range = 0.0, const double _wx = 0.0, const double _wy = 0.0, const double _wz = 0.0, const double _projection_length = 0.0, const double _projected_wx = 0.0, const double _projected_wy = 0.0) diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 0c90a468f2ebd..0fb87c0f88ba0 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -95,6 +95,8 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( for (auto & split_object : split_objects) { // set radars within objects std::shared_ptr> radars_within_split_object; + + // cppcheck-suppress knownConditionTrueFalse if (split_objects.size() == 1) { // If object is not split, radar data within object is same radars_within_split_object = radars_within_object; diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index 34155c3477003..94c795cfc2403 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -508,29 +508,16 @@ void TrtYoloX::preprocess(const std::vector & images) const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; scales_.clear(); - bool letterbox = true; - if (letterbox) { - for (const auto & image : images) { - cv::Mat dst_image; - const float scale = std::min(input_width / image.cols, input_height / image.rows); - scales_.emplace_back(scale); - const auto scale_size = cv::Size(image.cols * scale, image.rows * scale); - cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - const auto bottom = input_height - dst_image.rows; - const auto right = input_width - dst_image.cols; - copyMakeBorder( - dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); - dst_images.emplace_back(dst_image); - } - } else { - for (const auto & image : images) { - cv::Mat dst_image; - const float scale = -1.0; - scales_.emplace_back(scale); - const auto scale_size = cv::Size(input_width, input_height); - cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - dst_images.emplace_back(dst_image); - } + for (const auto & image : images) { + cv::Mat dst_image; + const float scale = std::min(input_width / image.cols, input_height / image.rows); + scales_.emplace_back(scale); + const auto scale_size = cv::Size(image.cols * scale, image.rows * scale); + cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); + const auto bottom = input_height - dst_image.rows; + const auto right = input_width - dst_image.cols; + copyMakeBorder(dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); + dst_images.emplace_back(dst_image); } const auto chw_images = cv::dnn::blobFromImages( dst_images, norm_factor_, cv::Size(), cv::Scalar(), false, false, CV_32F); @@ -650,34 +637,21 @@ void TrtYoloX::preprocessWithRoi( const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; scales_.clear(); - bool letterbox = true; int b = 0; - if (letterbox) { - for (const auto & image : images) { - cv::Mat dst_image; - cv::Mat cropped = image(rois[b]); - const float scale = std::min( - input_width / static_cast(rois[b].width), - input_height / static_cast(rois[b].height)); - scales_.emplace_back(scale); - const auto scale_size = cv::Size(rois[b].width * scale, rois[b].height * scale); - cv::resize(cropped, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - const auto bottom = input_height - dst_image.rows; - const auto right = input_width - dst_image.cols; - copyMakeBorder( - dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); - dst_images.emplace_back(dst_image); - b++; - } - } else { - for (const auto & image : images) { - cv::Mat dst_image; - const float scale = -1.0; - scales_.emplace_back(scale); - const auto scale_size = cv::Size(input_width, input_height); - cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - dst_images.emplace_back(dst_image); - } + for (const auto & image : images) { + cv::Mat dst_image; + cv::Mat cropped = image(rois[b]); + const float scale = std::min( + input_width / static_cast(rois[b].width), + input_height / static_cast(rois[b].height)); + scales_.emplace_back(scale); + const auto scale_size = cv::Size(rois[b].width * scale, rois[b].height * scale); + cv::resize(cropped, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); + const auto bottom = input_height - dst_image.rows; + const auto right = input_width - dst_image.cols; + copyMakeBorder(dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); + dst_images.emplace_back(dst_image); + b++; } const auto chw_images = cv::dnn::blobFromImages( dst_images, norm_factor_, cv::Size(), cv::Scalar(), false, false, CV_32F); diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 0202a26e46ef6..bc668c6af6c3c 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -129,10 +129,10 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const uint8_t traffic_light_type) { std::vector occlusion_ratios; - size_t not_detected_roi = 0; if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { + size_t not_detected_roi = 0; tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 6515a515397da..40e50aabf3c17 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -211,7 +211,9 @@ uint32_t CloudOcclusionPredictor::predict( { const uint32_t horizontal_sample_num = 20; const uint32_t vertical_sample_num = 20; - static_assert(horizontal_sample_num > 1 && vertical_sample_num > 1); + static_assert(horizontal_sample_num > 1); + static_assert(vertical_sample_num > 1); + const float min_dist_from_occlusion_to_tl = 5.0f; pcl::PointCloud tl_sample_cloud; diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index 2f912e30b8246..452d0a64d5c9c 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner/README.md @@ -47,10 +47,11 @@ It distributes route requests and planning results according to current MRM oper ### Subscriptions -| Name | Type | Description | -| --------------------- | ----------------------------------- | ---------------------- | -| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | -| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| Name | Type | Description | +| ---------------------------- | ----------------------------------------- | ---------------------- | +| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | +| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state | ### Publications @@ -170,6 +171,7 @@ To calculate `route_lanelets`, ### Rerouting Reroute here means changing the route while driving. Unlike route setting, it is required to keep a certain distance from vehicle to the point where the route is changed. +If the ego vehicle is not on autonomous driving state, the safety checking process will be skipped. ![rerouting_safety](./media/rerouting_safety.svg) diff --git a/planning/autoware_mission_planner/launch/mission_planner.launch.xml b/planning/autoware_mission_planner/launch/mission_planner.launch.xml index 655662c392213..8d77e417a6379 100644 --- a/planning/autoware_mission_planner/launch/mission_planner.launch.xml +++ b/planning/autoware_mission_planner/launch/mission_planner.launch.xml @@ -10,6 +10,7 @@ + diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index 0df01257f049e..2b1b943e002a8 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -55,6 +55,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); + sub_operation_mode_state_ = create_subscription( + "~/input/operation_mode_state", rclcpp::QoS(1), + std::bind(&MissionPlanner::on_operation_mode_state, this, _1)); sub_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); pub_marker_ = create_publisher("~/debug/route_marker", durable_qos); @@ -130,6 +133,11 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } +void MissionPlanner::on_operation_mode_state(const OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = msg; +} + void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; @@ -222,10 +230,23 @@ void MissionPlanner::on_set_lanelet_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - const auto reroute_availability = sub_reroute_availability_.takeData(); - if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { + if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received."); + } + + const bool is_autonomous_driving = + operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS && + operation_mode_state_->is_autoware_control_enabled + : false; + + if (is_reroute && is_autonomous_driving) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (!reroute_availability || !reroute_availability->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, + "Cannot reroute as the planner is not in lane following."); + } } change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); @@ -238,7 +259,7 @@ void MissionPlanner::on_set_lanelet_route( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (is_reroute && !check_reroute_safety(*current_route_, route)) { + if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); throw service_utils::ServiceException( @@ -271,10 +292,23 @@ void MissionPlanner::on_set_waypoint_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - const auto reroute_availability = sub_reroute_availability_.takeData(); - if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { + if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received."); + } + + const bool is_autonomous_driving = + operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS && + operation_mode_state_->is_autoware_control_enabled + : false; + + if (is_reroute && is_autonomous_driving) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (!reroute_availability || !reroute_availability->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, + "Cannot reroute as the planner is not in lane following."); + } } change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); @@ -287,7 +321,7 @@ void MissionPlanner::on_set_waypoint_route( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (is_reroute && !check_reroute_safety(*current_route_, route)) { + if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); throw service_utils::ServiceException( diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index 1a04a91c14ba3..1181ef54273ae 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,7 @@ namespace autoware::mission_planner { +using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; @@ -85,18 +87,21 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_operation_mode_state_; autoware::universe_utils::InterProcessPollingSubscriber sub_reroute_availability_{this, "~/input/reroute_availability"}; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; + OperationModeState::ConstSharedPtr operation_mode_state_; LaneletMapBin::ConstSharedPtr map_ptr_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_operation_mode_state(const OperationModeState::ConstSharedPtr msg); void on_map(const LaneletMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 3679c253e2bbb..0b54542cbea85 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -23,6 +23,8 @@ #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include + #include #include #include @@ -33,6 +35,8 @@ #include #include +using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; +using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; class PlannerInterface { public: @@ -52,6 +56,7 @@ class PlannerInterface node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); + diagnostics_pub_ = node.create_publisher("/diagnostics", 10); moving_object_speed_threshold = node.declare_parameter("slow_down.moving_object_speed_threshold"); @@ -135,6 +140,7 @@ class PlannerInterface rclcpp::Publisher::SharedPtr stop_reasons_pub_; rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; // Vehicle Parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index c8f962c3ad711..ec0b70e4e1b2d 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -557,7 +557,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto objects_ptr = objects_sub_.takeData(); const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; const auto acc_ptr = acc_sub_.takeData(); - if (!ego_odom_ptr || (!objects_ptr && (!use_pointcloud_ || !pointcloud_ptr)) || !acc_ptr) { + const bool can_detect_obstacles = objects_ptr || pointcloud_ptr; + if (!ego_odom_ptr || !can_detect_obstacles || !acc_ptr) { return; } diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 0413d4bbf3529..c531c4fefac95 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -25,6 +25,8 @@ #include #include +#include + namespace { StopSpeedExceeded createStopSpeedExceededMsg( @@ -104,6 +106,78 @@ VelocityFactorArray makeVelocityFactorArray( return velocity_factor_array; } +DiagnosticArray makeEmptyStopReasonDiagnosticArray(const rclcpp::Time & current_time) +{ + // Create status + DiagnosticStatus status; + status.level = status.OK; + status.name = "obstacle_cruise_planner"; + diagnostic_msgs::msg::KeyValue key_value; + { + // Decision + key_value.key = "decision"; + key_value.value = "none"; + status.values.push_back(key_value); + } + // create array + DiagnosticArray diagnostics; + diagnostics.header.stamp = current_time; + diagnostics.header.frame_id = "map"; + diagnostics.status.push_back(status); + return diagnostics; +} + +DiagnosticArray makeStopReasonDiagnosticArray( + const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose, + const StopObstacle & stop_obstacle) +{ + // Create status + DiagnosticStatus status; + status.level = status.OK; + status.name = "obstacle_cruise_planner"; + diagnostic_msgs::msg::KeyValue key_value; + { + // Decision + key_value.key = "decision"; + key_value.value = "stop"; + status.values.push_back(key_value); + } + + { // Stop info + key_value.key = "stop_position"; + const auto & p = stop_pose.position; + key_value.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + status.values.push_back(key_value); + key_value.key = "stop_orientation"; + const auto & o = stop_pose.orientation; + key_value.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + + std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; + status.values.push_back(key_value); + const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( + planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); + key_value.key = "distance_to_stop_pose"; + key_value.value = std::to_string(dist_to_stop_pose); + status.values.push_back(key_value); + } + + { + // Obstacle info + const auto & p = stop_obstacle.collision_point; + key_value.key = "collision_point"; + key_value.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + status.values.push_back(key_value); + } + + // create array + DiagnosticArray diagnostics; + diagnostics.header.stamp = planner_data.current_time; + diagnostics.header.frame_id = "map"; + diagnostics.status.push_back(status); + return diagnostics; +} + double calcMinimumDistanceToStop( const double initial_vel, const double max_acc, const double min_acc) { @@ -245,7 +319,7 @@ std::vector PlannerInterface::generateStopTrajectory( if (stop_obstacles.empty()) { stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time)); - + diagnostics_pub_->publish(makeEmptyStopReasonDiagnosticArray(planner_data.current_time)); // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -400,9 +474,11 @@ std::vector PlannerInterface::generateStopTrajectory( const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); + const auto stop_reason_diagnostic_array = + makeStopReasonDiagnosticArray(planner_data, stop_pose, *determined_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); - + diagnostics_pub_->publish(stop_reason_diagnostic_array); // Publish if ego vehicle will over run against the stop point with a limit acceleration const bool will_over_run = determined_zero_vel_dist.value() > 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 b5fd266006a48..45943d7deec09 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; } @@ -1104,7 +1094,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); @@ -1160,8 +1150,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 @@ -1169,7 +1157,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(); @@ -1228,7 +1216,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}; } @@ -1236,7 +1223,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(); @@ -1286,7 +1273,6 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( obj_matrix.hessian = H; obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1297,7 +1283,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(); @@ -1451,7 +1437,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - time_keeper_ptr_->toc(__func__, " "); return ConstraintMatrix{A, lb, ub}; } @@ -1477,7 +1462,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(); @@ -1510,7 +1495,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); @@ -1530,13 +1515,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); @@ -1563,8 +1547,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); } @@ -1647,7 +1629,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(); @@ -1700,7 +1682,6 @@ std::optional> MPTOptimizer::calcMPTPoints( traj_points.push_back(traj_point); } - time_keeper_ptr_->toc(__func__, " "); return traj_points; } @@ -1708,7 +1689,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( @@ -1723,8 +1704,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}; } diff --git a/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp b/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp index 645d6c92e3698..4b5c495cc85b6 100644 --- a/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp +++ b/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp @@ -59,11 +59,6 @@ void InvalidTrajectoryPublisherNode::onTimer() traj_pub_->publish(output); RCLCPP_INFO(this->get_logger(), "invalid trajectory is published."); - - bool EXIT_AFTER_PUBLISH = false; - if (EXIT_AFTER_PUBLISH) { - exit(0); - } } void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 5c66f5b906bba..b881b68798cdc 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -422,7 +422,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr // receive data current_odometry_ptr_ = sub_current_odometry_.takeData(); current_acceleration_ptr_ = sub_current_acceleration_.takeData(); - external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeNewData(); const auto operation_mode_ptr = sub_operation_mode_.takeData(); if (operation_mode_ptr) { operation_mode_ = *operation_mode_ptr; @@ -455,9 +455,6 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr calcExternalVelocityLimit(); updateDataForExternalVelocityLimit(); - // ignore current external velocity limit next time - external_velocity_limit_ptr_ = nullptr; - // For negative velocity handling, multiple -1 to velocity if it is for reverse. // NOTE: this process must be in the beginning of the process is_reverse_ = isReverse(input_points); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 856681d538db0..303d7449e40dd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -151,8 +151,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( *planner_data_->dynamic_object, data.current_lanelets, - [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); // Assume that the maximum allocation for data.other object is the sum of diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 53e06631a81d5..e6f84e7eece0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -22,6 +22,7 @@ lateral_offset_interval: 0.25 ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 + high_curvature_threshold: 0.1 # occupancy grid map occupancy_grid: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index bdb267ce223d3..6a10c0fb183f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -60,6 +60,7 @@ struct GoalPlannerParameters double lateral_offset_interval{0.0}; double ignore_distance_from_lane_start{0.0}; double margin_from_boundary{0.0}; + double high_curvature_threshold{0.0}; // occupancy grid map bool use_occupancy_grid_for_goal_search{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index d2ba416c7fa90..05385600926f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -52,6 +52,7 @@ struct PullOverPath size_t goal_id{}; size_t id{}; bool decided_velocity{false}; + double max_curvature{0.0}; // max curvature of the parking path PathWithLaneId getFullPath() const { @@ -72,6 +73,7 @@ struct PullOverPath return path; } + // path from the pull over start pose to the end pose PathWithLaneId getParkingPath() const { const PathWithLaneId full_path = getFullPath(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 00510a8725e32..1a711adb133eb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -293,9 +293,18 @@ void GoalPlannerModule::onTimer() planner->setPlannerData(local_planner_data); planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path) { + if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); + + // calculate absolute maximum curvature of parking path(start pose to end pose) for path + // priority + const std::vector curvatures = + autoware::motion_utils::calcCurvature(pull_over_path->getParkingPath().points); + pull_over_path->max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -817,23 +826,37 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto debugPrintPathPriority = [this]( const std::vector & sorted_pull_over_path_candidates, - const std::map & goal_id_to_index, - const std::optional> & path_id_to_margin_map_opt = std::nullopt, - const std::optional> & isSoftMarginOpt = - std::nullopt) { + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature) { std::stringstream ss; - ss << "\n---------------------- path priority ----------------------\n"; - for (const auto & path : sorted_pull_over_path_candidates) { - // clang-format off - ss << "path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id - << ", goal_id: " << path.goal_id - << ", goal_priority:" << goal_id_to_index.at(path.goal_id); - // clang-format on - if (path_id_to_margin_map_opt && isSoftMarginOpt) { - ss << ", margin: " << path_id_to_margin_map_opt->at(path.id) - << ((*isSoftMarginOpt)(path) ? " (soft)" : " (hard)"); + + // unsafe goal and it's priority are not visible as debug marker in rviz, + // so exclude unsafe goal from goal_priority + std::map goal_id_and_priority; + { + int priority = 0; + for (const auto & goal_candidate : goal_candidates) { + goal_id_and_priority[goal_candidate.id] = goal_candidate.is_safe ? priority++ : -1; } + } + + ss << "\n---------------------- path priority ----------------------\n"; + for (size_t i = 0; i < sorted_pull_over_path_candidates.size(); ++i) { + const auto & path = sorted_pull_over_path_candidates[i]; + + // goal_index is same to goal priority including unsafe goal + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const bool is_safe_goal = goal_candidates[goal_index].is_safe; + const int goal_priority = goal_id_and_priority[path.goal_id]; + + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) + << ", path_id: " << path.id << ", goal_id: " << path.goal_id + << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") + << ", margin: " << path_id_to_margin_map.at(path.id) + << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.max_curvature + << (isHighCurvature(path) ? " (high)" : " (low)"); ss << "\n"; } ss << "-----------------------------------------------------------\n"; @@ -844,6 +867,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins; const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins; + // (1) Sort pull_over_path_candidates based on the order in goal_candidates // Create a map of goal_id to its index in goal_candidates std::map goal_id_to_index; for (size_t i = 0; i < goal_candidates.size(); ++i) { @@ -868,6 +892,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri // if object recognition is enabled, sort by collision check margin if (parameters_->use_object_recognition) { + // (2) Sort by collision check margins const std::vector margins = std::invoke([&]() { std::vector combined_margins = soft_margins; combined_margins.insert(combined_margins.end(), hard_margins.begin(), hard_margins.end()); @@ -914,23 +939,45 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; }); - // Sort pull_over_path_candidates based on the order in efficient_path_order - if (parameters_->path_priority == "efficient_path") { - const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_margin_map[path.id]; - return std::any_of( - soft_margins.begin(), soft_margins.end(), - [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); - }; - const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - return !isSoftMargin(a) && !isSoftMargin(b) && - std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; - }; + // (3) Sort by curvature + // If the curvature is less than the threshold, prioritize the path. + const auto isHighCurvature = [&](const PullOverPath & path) -> bool { + return path.max_curvature >= parameters_->high_curvature_threshold; + }; + const auto isSoftMargin = [&](const PullOverPath & path) -> bool { + const double margin = path_id_to_margin_map[path.id]; + return std::any_of( + soft_margins.begin(), soft_margins.end(), + [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); + }; + const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { + return !isSoftMargin(a) && !isSoftMargin(b) && + std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; + }; + + // NOTE: this is just partition sort based on curvature threshold within each sub partitions + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [&](const PullOverPath & a, const PullOverPath & b) { + // if both are soft margin or both are same hard margin, prioritize the path with lower + // curvature. + if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { + return !isHighCurvature(a) && isHighCurvature(b); + } + // otherwise, keep the order based on the margin. + return false; + }); + + // (4) Sort pull_over_path_candidates based on the order in efficient_path_order keeping the + // collision check margin and curvature priority. + if (parameters_->path_priority == "efficient_path") { std::stable_sort( sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&](const auto & a, const auto & b) { - // if both are soft margin or both are same hard margin, sort by planner priority + // if any of following conditions are met, sort by path type priority + // - both are soft margin + // - both are same hard margin if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { return comparePathTypePriority(a, b); } @@ -938,18 +985,24 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return false; }); - // debug print path priority: sorted by efficient_path_order and collision check margin + // debug print path priority sorted by + // - efficient_path_order + // - collision check margin + // - curvature debugPrintPathPriority( - sorted_pull_over_path_candidates, goal_id_to_index, path_id_to_margin_map, isSoftMargin); + sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map, + isSoftMargin, isHighCurvature); } } else { - // Sort pull_over_path_candidates based on the order in efficient_path_order + /** + * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the + * future. sort by curvature is not implemented yet. + * Sort pull_over_path_candidates based on the order in efficient_path_order + */ if (parameters_->path_priority == "efficient_path") { std::stable_sort( sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&](const auto & a, const auto & b) { return comparePathTypePriority(a, b); }); - // debug print path priority: sorted by efficient_path_order and collision check margin - debugPrintPathPriority(sorted_pull_over_path_candidates, goal_id_to_index); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index b079db9babf31..b86bbbca7d98b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -61,6 +61,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.ignore_distance_from_lane_start = node->declare_parameter(ns + "ignore_distance_from_lane_start"); p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); + p.high_curvature_threshold = node->declare_parameter(ns + "high_curvature_threshold"); const std::string parking_policy_name = node->declare_parameter(ns + "parking_policy"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index d6f03f79186d8..b74cdbc6a569b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -145,8 +145,8 @@ PredictedObjects extractObjectsInExpandedPullOverLanes( route_handler, left_side, backward_distance, forward_distance, bound_offset); const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( - objects, lanes, [](const auto & obj, const auto & lanelet) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet); + objects, lanes, [](const auto & obj, const auto & lanelet, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet, yaw_threshold); }); return objects_in_lanes; 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 e8e83a0703a76..9df19c4de6834 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 @@ -50,7 +50,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)}, + common_data_ptr_{std::make_shared()}, + direction_{direction}, + type_{type} { } @@ -151,7 +154,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; } @@ -219,6 +233,7 @@ class LaneChangeBase std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; + lane_change::CommonDataPtr common_data_ptr_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index ffd2754acc38f..2fdf7c6b550a3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -17,16 +17,28 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include +#include #include +#include + #include #include +#include #include #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using route_handler::Direction; +using route_handler::RouteHandler; +using utils::path_safety_checker::ExtendedPredictedObjects; + struct LateralAccelerationMap { std::vector base_vel; @@ -68,7 +80,7 @@ struct LateralAccelerationMap } }; -struct LaneChangeCancelParameters +struct CancelParameters { bool enable_on_prepare_phase{true}; bool enable_on_lane_changing_phase{false}; @@ -83,7 +95,7 @@ struct LaneChangeCancelParameters int unsafe_hysteresis_threshold{2}; }; -struct LaneChangeParameters +struct Parameters { // trajectory generation double backward_lane_length{200.0}; @@ -92,8 +104,8 @@ struct LaneChangeParameters int lateral_acc_sampling_num{10}; // lane change parameters - double backward_length_buffer_for_end_of_lane; - double backward_length_buffer_for_blocking_object; + double backward_length_buffer_for_end_of_lane{0.0}; + double backward_length_buffer_for_blocking_object{0.0}; double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; @@ -143,7 +155,7 @@ struct LaneChangeParameters utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort - LaneChangeCancelParameters cancel{}; + CancelParameters cancel{}; double finish_judge_lateral_threshold{0.2}; @@ -151,32 +163,32 @@ struct LaneChangeParameters bool publish_debug_marker{false}; }; -enum class LaneChangeStates { +enum class States { Normal = 0, Cancel, Abort, Stop, }; -struct LaneChangePhaseInfo +struct PhaseInfo { double prepare{0.0}; double lane_changing{0.0}; [[nodiscard]] double sum() const { return prepare + lane_changing; } - LaneChangePhaseInfo(const double _prepare, const double _lane_changing) + PhaseInfo(const double _prepare, const double _lane_changing) : prepare(_prepare), lane_changing(_lane_changing) { } }; -struct LaneChangeInfo +struct Info { - LaneChangePhaseInfo longitudinal_acceleration{0.0, 0.0}; - LaneChangePhaseInfo velocity{0.0, 0.0}; - LaneChangePhaseInfo duration{0.0, 0.0}; - LaneChangePhaseInfo length{0.0, 0.0}; + PhaseInfo longitudinal_acceleration{0.0, 0.0}; + PhaseInfo velocity{0.0, 0.0}; + PhaseInfo duration{0.0, 0.0}; + PhaseInfo length{0.0, 0.0}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; @@ -190,22 +202,19 @@ struct LaneChangeInfo double terminal_lane_changing_velocity{0.0}; }; -struct LaneChangeLanesFilteredObjects +struct LanesObjects { - utils::path_safety_checker::ExtendedPredictedObjects current_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects target_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects other_lane{}; + ExtendedPredictedObjects current_lane{}; + ExtendedPredictedObjects target_lane{}; + ExtendedPredictedObjects other_lane{}; }; -enum class LaneChangeModuleType { +enum class ModuleType { NORMAL = 0, EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; -} // namespace autoware::behavior_path_planner -namespace autoware::behavior_path_planner::lane_change -{ struct PathSafetyStatus { bool is_safe{true}; @@ -218,6 +227,55 @@ struct LanesPolygon std::optional target; std::vector target_backward; }; + +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + +struct CommonData +{ + std::shared_ptr route_handler_ptr; + Odometry::ConstSharedPtr self_odometry_ptr; + std::shared_ptr bpp_param_ptr; + std::shared_ptr lc_param_ptr; + Lanes lanes; + Direction direction; + + [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } + + [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } + + [[nodiscard]] double get_ego_speed(bool use_norm = false) const + { + if (!use_norm) { + return get_ego_twist().linear.x; + } + + const auto x = get_ego_twist().linear.x; + const auto y = get_ego_twist().linear.y; + return std::hypot(x, y); + } +}; + +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using CommonDataPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change +namespace autoware::behavior_path_planner +{ +using LaneChangeModuleType = lane_change::ModuleType; +using LaneChangeParameters = lane_change::Parameters; +using LaneChangeStates = lane_change::States; +using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangeInfo = lane_change::Info; +using LaneChangeLanesFilteredObjects = lane_change::LanesObjects; +using LateralAccelerationMap = lane_change::LateralAccelerationMap; +} // namespace autoware::behavior_path_planner + #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 77c603e3bd975..97b5c621deea5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -31,8 +31,8 @@ struct LaneChangePath { PathWithLaneId path{}; ShiftedPath shifted_path{}; - PathWithLaneId prev_path{}; - LaneChangeInfo info{}; + LaneChangeInfo info; + bool is_safe{false}; }; using LaneChangePaths = std::vector; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 4fd6fce325f49..f73f989174b54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -56,18 +56,24 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; * * @param objects The predicted objects to filter. * @param lanelet + * @param yaw_threshold * @return result. */ -bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); +bool isCentroidWithinLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, + const double yaw_threshold); /** * @brief Filters objects based on object polygon overlapping with lanelet. * * @param objects The predicted objects to filter. * @param lanelet + * @param yaw_threshold * @return result. */ -bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); +bool isPolygonOverlapLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, + const double yaw_threshold); bool isPolygonOverlapLanelet( const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon); @@ -168,7 +174,9 @@ void filterObjectsByClass( */ std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition); + const std::function & + condition, + const double yaw_threshold = M_PI); /** * @brief Separate the objects into two part based on whether the object is within lanelet. @@ -176,7 +184,9 @@ std::pair, std::vector> separateObjectIndicesByLanel */ std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition); + const std::function & + condition, + const double yaw_threshold = M_PI); /** * @brief Get the predicted path from an object. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 469be03eb905c..95a498b7b8475 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -57,15 +57,34 @@ bool is_within_circle( namespace autoware::behavior_path_planner::utils::path_safety_checker { -bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +bool isCentroidWithinLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + if ( + std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > + yaw_threshold) { + return false; + } + + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position)) + .basicPoint(), + lanelet.polygon2d().basicPolygon()); } -bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +bool isPolygonOverlapLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + if ( + std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > + yaw_threshold) { + return false; + } + const auto lanelet_polygon = utils::toPolygon2d(lanelet); return isPolygonOverlapLanelet(object, lanelet_polygon); } @@ -174,7 +193,9 @@ void filterObjectsByClass( std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition) + const std::function & + condition, + const double yaw_threshold) { if (target_lanelets.empty()) { return {}; @@ -184,7 +205,9 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - const auto filter = [&](const auto & llt) { return condition(objects.objects.at(i), llt); }; + const auto filter = [&](const auto & llt) { + return condition(objects.objects.at(i), llt, yaw_threshold); + }; const auto found = std::find_if(target_lanelets.begin(), target_lanelets.end(), filter); if (found != target_lanelets.end()) { target_indices.push_back(i); @@ -198,13 +221,15 @@ std::pair, std::vector> separateObjectIndicesByLanel std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition) + const std::function & + condition, + const double yaw_threshold) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets, condition); + separateObjectIndicesByLanelets(objects, target_lanelets, condition, yaw_threshold); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 7b30745057743..03b6e13cb2d3e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -74,8 +74,9 @@ class PullOutPlannerBase *dynamic_objects, parameters_.th_moving_object_velocity); auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + stop_objects, pull_out_lanes, + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); utils::path_safety_checker::filterObjectsByClass( pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 00b6649851f87..9b94d3505c405 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1217,8 +1217,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( // filter for objects located in pull out lanes and moving at a speed below the threshold auto [stop_objects_in_pull_out_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + stop_objects, pull_out_lanes, + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); const auto path = planner_data_->route_handler->getCenterLinePath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index 40d36898b2309..8e20cf2c9f660 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -659,6 +659,27 @@ if(isWithinCrosswalk()) then (yes) stop else (\n no) endif + +if(is object within intersection?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif + +if(is object on right side of the ego path?) then (yes) +if(are there adjacent lanes on right side of ego lane?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif +else (\n no) +if(are there adjacent lanes on left side of ego lane?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif +endif + #FF006C :return true; stop } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index d02b39047e71c..49599004e0952 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -352,6 +352,10 @@ struct ObjectData // avoidance target { } + Pose getPose() const { return object.kinematics.initial_pose_with_covariance.pose; } + + Point getPosition() const { return object.kinematics.initial_pose_with_covariance.pose.position; } + PredictedObject object; // object behavior. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 9c771224e89eb..eb0d903f2e398 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -64,7 +64,7 @@ MarkerArray createObjectsCubeMarkerArray( } marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); msg.markers.push_back(marker); } @@ -80,10 +80,8 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std: "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); + marker.points.push_back(createPoint(p.x(), p.y(), object.getPosition().z)); } marker.points.push_back(marker.points.front()); @@ -142,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray( for (const auto & object : objects) { if (verbose) { marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" @@ -162,7 +160,7 @@ MarkerArray createObjectInfoMarkerArray( { marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); marker.pose.position.z += 2.0; std::ostringstream string_stream; string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index a171ae9161041..c3a67eb074d73 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -165,9 +165,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // the avoidance path is already approved - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) || - (helper_->getShift(object_pos) < 0.0 && !is_object_on_right); + const auto is_approved = + (helper_->getShift(object.getPosition()) > 0.0 && is_object_on_right) || + (helper_->getShift(object.getPosition()) < 0.0 && !is_object_on_right); if (is_approved) { return std::make_pair(desire_shift_length, avoidance_distance); } @@ -363,9 +363,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( if (is_return_shift_to_goal) { return true; } - const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; const bool has_object_near_goal = - autoware::universe_utils::calcDistance2d(goal_pose.position, object_pos) < + autoware::universe_utils::calcDistance2d(goal_pose.position, o.getPosition()) < parameters_->object_check_goal_distance; return has_object_near_goal; }(); @@ -1027,8 +1026,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const auto has_object_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { return autoware::universe_utils::calcDistance2d( - data_->route_handler->getGoalPose().position, - o.object.kinematics.initial_pose_with_covariance.pose.position) < + data_->route_handler->getGoalPose().position, o.getPosition()) < parameters_->object_check_goal_distance; }); if (has_object_near_goal) { @@ -1097,9 +1095,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const bool has_last_shift_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return autoware::universe_utils::calcDistance2d( - last_sl.end.position, - o.object.kinematics.initial_pose_with_covariance.pose.position) < + return autoware::universe_utils::calcDistance2d(last_sl.end.position, o.getPosition()) < parameters_->object_check_goal_distance; }); if (has_last_shift_near_goal) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 03fe48c6ab441..30d1aad16374b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -293,8 +293,7 @@ bool isWithinCrosswalk( { using Point = boost::geometry::model::d2::point_xy; - const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position; - const Point p_object{p.x, p.y}; + const Point p_object{object.getPosition().x, object.getPosition().y}; // get conflicting crosswalk crosswalk constexpr int PEDESTRIAN_GRAPH_ID = 1; @@ -335,14 +334,16 @@ bool isWithinIntersection( route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); return boost::geometry::within( - object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lanelet::utils::to2D(polygon.basicPolygon())); } bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), object.overhang_lanelet.polygon2d().basicPolygon())) { return true; } @@ -351,7 +352,8 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr lanelet::ConstLanelets prev_lanelet; if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) { if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), prev_lanelet.front().polygon2d().basicPolygon())) { return true; } @@ -361,10 +363,20 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr lanelet::ConstLanelet next_lanelet; if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), next_lanelet.polygon2d().basicPolygon())) { return true; } + } else { + for (const auto & lane : route_handler->getNextLanelets(object.overhang_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lane.polygon2d().basicPolygon())) { + return true; + } + } } return false; @@ -372,20 +384,18 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr bool isParallelToEgoLane(const ObjectData & object, const double threshold) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto closest_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose)); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); + const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object.getPose())); return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } bool isMergingToEgoLane(const ObjectData & object) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto closest_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); + const auto yaw_deviation = calcYawDeviation(closest_pose, object.getPose()); if (isOnRight(object)) { if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { @@ -422,9 +432,8 @@ bool isParkedVehicle( return false; } - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; const auto centerline_pos = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position; + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -442,7 +451,8 @@ bool isParkedVehicle( return same_direction_lane; } - return static_cast(opposite_lanes.front().invert()); + return static_cast( + route_handler->getMostRightLanelet(opposite_lanes.front()).invert()); }(); const auto center_to_left_boundary = distance2d( @@ -457,7 +467,7 @@ bool isParkedVehicle( if (sub_type == "road_shoulder") { // assuming it's parked vehicle if its CoG is within road shoulder lanelet. if (boost::geometry::within( - to2D(toLaneletPoint(object_pos)).basicPoint(), + to2D(toLaneletPoint(object.getPosition())).basicPoint(), most_left_lanelet.polygon2d().basicPolygon())) { return true; } @@ -468,7 +478,7 @@ bool isParkedVehicle( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pos)).basicPoint()); + to2D(toLaneletPoint(object.getPosition())).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -490,7 +500,8 @@ bool isParkedVehicle( return same_direction_lane; } - return static_cast(opposite_lanes.front().invert()); + return static_cast( + route_handler->getMostLeftLanelet(opposite_lanes.front()).invert()); }(); const auto center_to_right_boundary = distance2d( @@ -505,7 +516,7 @@ bool isParkedVehicle( if (sub_type == "road_shoulder") { // assuming it's parked vehicle if its CoG is within road shoulder lanelet. if (boost::geometry::within( - to2D(toLaneletPoint(object_pos)).basicPoint(), + to2D(toLaneletPoint(object.getPosition())).basicPoint(), most_right_lanelet.polygon2d().basicPolygon())) { return true; } @@ -516,7 +527,7 @@ bool isParkedVehicle( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pos)).basicPoint()); + to2D(toLaneletPoint(object.getPosition())).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = @@ -527,9 +538,8 @@ bool isParkedVehicle( return false; } - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { return false; } @@ -544,13 +554,13 @@ bool isCloseToStopFactor( { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle bool is_close_to_stop_factor = false; // check traffic light - const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); + const auto to_traffic_light = + getDistanceToNextTrafficLight(object.getPose(), data.extend_lanelets); { is_close_to_stop_factor = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; @@ -792,14 +802,52 @@ bool isSatisfiedWithNonVehicleCondition( } // Object is on center line -> ignore. - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { object.info = ObjectInfo::TOO_NEAR_TO_CENTERLINE; return false; } + if (object.is_within_intersection) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "object is within intersection. don't have to avoid it."); + return false; + } + + const auto right_lane = + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true); + if (right_lane.has_value() && isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto left_lane = + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true); + if (left_lane.has_value() && !isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto right_opposite_lanes = + planner_data->route_handler->getRightOppositeLanelets(object.overhang_lanelet); + if (!right_opposite_lanes.empty() && isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto left_opposite_lanes = + planner_data->route_handler->getLeftOppositeLanelets(object.overhang_lanelet); + if (!left_opposite_lanes.empty() && !isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + return true; } @@ -844,9 +892,8 @@ bool isSatisfiedWithVehicleCondition( return false; } - const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto is_moving_distance_longer_than_threshold = - calcDistance2d(object.init_pose, current_pose) > + calcDistance2d(object.init_pose, object.getPose()) > parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; @@ -944,9 +991,8 @@ double getRoadShoulderDistance( using autoware::universe_utils::Point2d; using lanelet::utils::to2D; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - autoware::motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); + autoware::motion_utils::findNearestIndex(data.reference_path.points, object.getPosition()); const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; @@ -957,7 +1003,7 @@ double getRoadShoulderDistance( std::vector> intersects; for (const auto & p1 : object.overhang_points) { const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); const auto p_tmp = geometry_msgs::build().position(p1.second).orientation(centerline_pose.orientation); @@ -1326,8 +1372,7 @@ std::vector generateObstaclePolygonsForDrivableArea( object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - obstacles_for_drivable_area.push_back( - {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); + obstacles_for_drivable_area.push_back({object.getPose(), obj_poly, !isOnRight(object)}); } return obstacles_for_drivable_area; } @@ -1495,7 +1540,7 @@ void fillObjectMovingTime( object_data.last_stop = now; object_data.move_time = 0.0; if (is_new_object) { - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); object_data.stop_time = 0.0; object_data.last_move = now; stopped_objects.push_back(object_data); @@ -1510,7 +1555,7 @@ void fillObjectMovingTime( } if (is_new_object) { - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); object_data.move_time = std::numeric_limits::infinity(); object_data.stop_time = 0.0; object_data.last_move = now; @@ -1520,7 +1565,7 @@ void fillObjectMovingTime( object_data.last_stop = same_id_obj->last_stop; object_data.move_time = (now - same_id_obj->last_stop).seconds(); object_data.stop_time = 0.0; - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); if (object_data.move_time > object_parameter.moving_time_threshold) { stopped_objects.erase(same_id_obj); @@ -1617,9 +1662,9 @@ void updateRegisteredObject( } constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose; + const auto r_pos = registered_object.getPose(); const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR; + return calcDistance2d(r_pos, o.getPose()) < POS_THR; }); // same id object is not detected, but object is found around registered. update registered. @@ -1834,6 +1879,8 @@ void filterTargetObjects( continue; } } else { + o.is_within_intersection = + filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = false; o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); @@ -1987,6 +2034,17 @@ lanelet::ConstLanelets getAdjacentLane( lanes.push_back(opt_right_lane.value()); } + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lane); + if (!is_right_shift && !left_opposite_lanes.empty()) { + lanes.push_back(left_opposite_lanes.front()); + + for (const auto & prev_lane : rh->getPreviousLanelets(left_opposite_lanes.front())) { + if (!exist(prev_lane.id())) { + lanes.push_back(prev_lane); + } + } + } + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); if (is_right_shift && !right_opposite_lanes.empty()) { lanes.push_back(right_opposite_lanes.front()); @@ -2064,21 +2122,35 @@ std::vector getSafetyCheckTargetObjects( return {}; } + const auto is_moving = [¶meters](const auto & object) { + const auto & object_twist = object.kinematics.initial_twist_with_covariance.twist; + const auto object_vel_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); + const auto object_parameter = + parameters->object_parameters.at(utils::getHighestProbLabel(object.classification)); + return object_vel_norm > object_parameter.moving_speed_threshold; + }; + + const auto filter = + [&is_moving](const auto & object, const auto & lanelet, [[maybe_unused]] const auto unused) { + // filter by yaw deviation only when the object is moving because the head direction is not + // reliable while object is stopping. + const auto yaw_threshold = is_moving(object) ? M_PI_2 : M_PI; + return utils::path_safety_checker::isCentroidWithinLanelet(object, lanelet, yaw_threshold); + }; + // check right lanes if (check_right_lanes) { const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, true); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } @@ -2092,15 +2164,13 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } @@ -2114,15 +2184,13 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 9cbe962e88cae..57aeac11c9aed 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -83,7 +83,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Get stop point const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, planner_param_.stop_margin, + original_path, stop_line, {lane_id_}, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { return true; @@ -128,7 +128,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (planner_param_.use_dead_line) { // Use '-' for margin because it's the backward distance from stop line const auto dead_line_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, -planner_param_.dead_line_margin, + original_path, stop_line, {lane_id_}, -planner_param_.dead_line_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (dead_line_point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index 648fc611ac6b1..943eef03fc6ce 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -48,11 +48,10 @@ void IntersectionLanelets::update( } } if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { - const auto first_attention_lane = first_attention_lane_.value(); // remove first_attention_area_ and non-straight lanelets from attention_non_preceding lanelet::ConstLanelets attention_non_preceding_ex_first; lanelet::ConstLanelets sibling_first_attention_lanelets; - for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane_.value())) { for (const auto & following : routing_graph_ptr->following(previous)) { sibling_first_attention_lanelets.push_back(following); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index 6ead1df3eb2d1..b749207aa2393 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -137,11 +137,9 @@ bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const if (!dist_to_stopline_opt) { return false; } - const double observed_velocity = - predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; const double dist_to_stopline = dist_to_stopline_opt.value(); - const double braking_distance = - (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); return dist_to_stopline > braking_distance; } @@ -153,10 +151,8 @@ bool ObjectInfo::can_stop_before_ego_lane( return false; } const double dist_to_stopline = dist_to_stopline_opt.value(); - const double observed_velocity = - predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); if (dist_to_stopline > braking_distance) { return false; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 0478aea34a44f..4d800b2926eb7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -125,7 +125,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line.value(), planner_param_.stop_margin, + original_path, stop_line.value(), {lane_id_}, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { setSafe(true); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 769932259e3e6..178fca4ce046f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -21,8 +21,10 @@ #include #include +#include #include #include +#include #define EIGEN_MPL2_ONLY #include @@ -31,7 +33,7 @@ namespace autoware::behavior_velocity_planner { namespace { -geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) +inline geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -61,9 +63,27 @@ std::optional checkCollision( template std::optional findCollisionSegment( const T & path, const geometry_msgs::msg::Point & stop_line_p1, - const geometry_msgs::msg::Point & stop_line_p2) + const geometry_msgs::msg::Point & stop_line_p2, const std::vector & target_lane_ids) { for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & prev_lane_ids = path.points.at(i).lane_ids; + const auto & next_lane_ids = path.points.at(i + 1).lane_ids; + + const bool is_target_lane_in_prev_lane = std::any_of( + target_lane_ids.begin(), target_lane_ids.end(), [&prev_lane_ids](size_t target_lane_id) { + return std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) != + prev_lane_ids.end(); + }); + const bool is_target_lane_in_next_lane = std::any_of( + target_lane_ids.begin(), target_lane_ids.end(), [&next_lane_ids](size_t target_lane_id) { + return std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != + next_lane_ids.end(); + }); + + if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) { + continue; + } + const auto & p1 = autoware::universe_utils::getPoint(path.points.at(i)); // Point before collision point const auto & p2 = @@ -81,12 +101,12 @@ std::optional findCollisionSegment( template std::optional findCollisionSegment( - const T & path, const LineString2d & stop_line) + const T & path, const LineString2d & stop_line, const std::vector & target_lane_ids) { const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1)); - return findCollisionSegment(path, stop_line_p1, stop_line_p2); + return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_ids); } template @@ -183,7 +203,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse std::optional createTargetPoint( const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const double margin, const double vehicle_offset); + const std::vector & lane_ids, const double margin, const double vehicle_offset); } // namespace arc_lane_utils } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index da6ef7262de74..21104fc022876 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -26,8 +26,6 @@ #include #endif -#include -#include #include #include @@ -109,10 +107,10 @@ std::optional findOffsetSegment( std::optional createTargetPoint( const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const double margin, const double vehicle_offset) + const std::vector & lane_ids, const double margin, const double vehicle_offset) { // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line); + const auto collision_segment = findCollisionSegment(path, stop_line, lane_ids); if (!collision_segment) { // No collision return {}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index db9a63169e565..223d1f0d83e28 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -49,11 +49,14 @@ TEST(findCollisionSegment, nominal) * **/ auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + for (auto & point : path.points) { + point.lane_ids.push_back(100); + } LineString2d stop_line; stop_line.emplace_back(Point2d(3.5, 3.0)); stop_line.emplace_back(Point2d(3.5, -3.0)); - auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, {100}); EXPECT_EQ(segment->first, static_cast(3)); EXPECT_DOUBLE_EQ(segment->second.x, 3.5); EXPECT_DOUBLE_EQ(segment->second.y, 0.0); 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 04ea5ca872098..0ec2fe861941b 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 @@ -18,12 +18,10 @@ #include #include -#include -#include +#include namespace autoware::behavior_velocity_planner { -namespace bg = boost::geometry; StopLineModule::StopLineModule( const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line, @@ -51,8 +49,15 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); // Calculate stop pose and insert index + auto lane = this->planner_data_->route_handler_->getLaneletsFromId(lane_id_); + auto next_lanes = this->planner_data_->route_handler_->getNextLanelets(lane); + std::vector search_lane_ids; + search_lane_ids.push_back(lane_id_); + for (const auto & next_lane : next_lanes) { + search_lane_ids.push_back(next_lane.id()); + } const auto stop_point = arc_lane_utils::createTargetPoint( - *path, stop_line, planner_param_.stop_margin, + *path, stop_line, search_lane_ids, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); // If no collision found, do nothing diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 1bf26d9f95c6a..8213b862b89e6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -379,7 +379,7 @@ std::optional VirtualTrafficLightModule::getPathIndexOfFirstEndLine() end_line_p2.y = end_line.back().y(); const auto collision = - arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2); + arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, {lane_id_}); if (!collision) { continue; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 5e6f9619e75c2..f0100f58074ba 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -325,7 +325,7 @@ void MotionVelocityPlannerNode::insert_slowdown( autoware::motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); if (from_insert_idx && to_insert_idx) { for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) - trajectory.points[idx].longitudinal_velocity_mps = 0.0; + trajectory.points[idx].longitudinal_velocity_mps = slowdown_interval.velocity; } else { RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); } diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 278d2dfc28e59..6882fbf068fd1 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -51,11 +51,11 @@ GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) [this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); }); gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); - auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); + auto bound_timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); auto period_control = std::chrono::duration_cast( std::chrono::duration(timer_callback_interval_sec_)); - timer_ = std::make_shared>( - this->get_clock(), period_control, std::move(timer_callback), + timer_ = std::make_shared>( + this->get_clock(), period_control, std::move(bound_timer_callback), this->get_node_base_interface()->get_context()); this->get_node_timers_interface()->add_timer(timer_, nullptr); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h index 02fa183e5d017..edbc55c8158c0 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -126,10 +126,8 @@ static Counts & counts() // bitness #if SIZE_MAX == UINT32_MAX #define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 -#elif SIZE_MAX == UINT64_MAX -#define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 #else -#error Unsupported bitness +#define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 #endif // endianess @@ -165,12 +163,13 @@ static Counts & counts() #endif #include #pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) -#define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ - [](size_t mask) noexcept -> int { \ - unsigned long index; \ // NOLINT - return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ - : ROBIN_HOOD(BITNESS); \ - }(x) +#define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ + [](size_t mask) noexcept -> int { \ + unsigned long index; \ // NOLINT + return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) \ // NOLINT + ? static_cast(index) \ // cppcheck-suppress syntaxError + : ROBIN_HOOD(BITNESS); \ // cppcheck-suppress syntaxError + }(x) // cppcheck-suppress syntaxError #else #if ROBIN_HOOD(BITNESS) == 32 #define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl @@ -429,14 +428,6 @@ class BulkPoolAllocator return *this; } - BulkPoolAllocator & - // NOLINTNEXTLINE (bugprone-unhandled-self-assignment,cert-oop54-cpp) - operator=(const BulkPoolAllocator & ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept - { - // does not do anything - return *this; - } - ~BulkPoolAllocator() noexcept { reset(); } // Deallocates all allocated memory. @@ -475,22 +466,6 @@ class BulkPoolAllocator mHead = obj; } - // Adds an already allocated block of memory to the allocator. This allocator is from now on - // responsible for freeing the data (with free()). If the provided data is not large enough to - // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. - void addOrFree(void * ptr, const size_t numBytes) noexcept - { - // calculate number of available elements in ptr - if (numBytes < ALIGNMENT + ALIGNED_SIZE) { - // not enough data for at least one element. Free and return. - ROBIN_HOOD_LOG("std::free") - std::free(ptr); - } else { - ROBIN_HOOD_LOG("add to buffer") - add(ptr, numBytes); - } - } - void swap(BulkPoolAllocator & other) noexcept { using std::swap; @@ -1382,7 +1357,7 @@ class Table Iter & operator++() noexcept { mInfo++; - mKeyVals++; + ++mKeyVals; fastForward(); return *this; } @@ -2035,41 +2010,6 @@ class Table return emplace(std::move(keyval)).first; } - // Returns 1 if key is found, 0 otherwise. - size_t count(const key_type & key) const - { // NOLINT (modernize-use-nodiscard) - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - template - // NOLINTNEXTLINE (modernize-use-nodiscard) - typename std::enable_if::type count(const OtherKey & key) const - { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - bool contains(const key_type & key) const - { // NOLINT (modernize-use-nodiscard) - return 1U == count(key); - } - - template - // NOLINTNEXTLINE (modernize-use-nodiscard) - typename std::enable_if::type contains(const OtherKey & key) const - { - return 1U == count(key); - } - // Returns a reference to the value found for key. // Throws std::out_of_range if element cannot be found template diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index e811ffb030cd6..f20c763d643ce 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -60,6 +60,7 @@ bool L2pingService::initialize() address.sin_family = AF_INET; address.sin_port = htons(port_); address.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(socket_, (struct sockaddr *)&address, sizeof(address)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/default_ad_api/src/utils/topics.hpp b/system/default_ad_api/src/utils/topics.hpp new file mode 100644 index 0000000000000..97d91b33c9fa3 --- /dev/null +++ b/system/default_ad_api/src/utils/topics.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 UTILS__TOPICS_HPP_ +#define UTILS__TOPICS_HPP_ + +#include +#include + +#include + +namespace default_ad_api::utils +{ + +template +MsgT ignore_stamp(MsgT msg) +{ + msg.stamp = rclcpp::Time(0, 0); + return msg; +}; + +template +void notify(PubT & pub, std::optional & prev, const MsgT & curr, FuncT && ignore) +{ + if (!prev || ignore(*prev) != ignore(curr)) { + prev = curr; + pub->publish(curr); + } +} + +} // namespace default_ad_api::utils + +#endif // UTILS__TOPICS_HPP_ diff --git a/system/default_ad_api/src/vehicle_door.cpp b/system/default_ad_api/src/vehicle_door.cpp index b94921c8fd41b..e37e91bdda8e2 100644 --- a/system/default_ad_api/src/vehicle_door.cpp +++ b/system/default_ad_api/src/vehicle_door.cpp @@ -14,6 +14,8 @@ #include "vehicle_door.hpp" +#include "utils/topics.hpp" + namespace default_ad_api { @@ -24,7 +26,14 @@ VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options) group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.relay_service(cli_command_, srv_command_, group_cli_, 3.0); adaptor.relay_service(cli_layout_, srv_layout_, group_cli_, 3.0); - adaptor.relay_message(pub_status_, sub_status_); + adaptor.init_pub(pub_status_); + adaptor.init_sub(sub_status_, this, &VehicleDoorNode::on_status); +} + +void VehicleDoorNode::on_status(vehicle_interface::DoorStatus::Message::ConstSharedPtr msg) +{ + utils::notify( + pub_status_, status_, *msg, utils::ignore_stamp); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/vehicle_door.hpp b/system/default_ad_api/src/vehicle_door.hpp index 92e8918afcdf8..e83e0a164d3f6 100644 --- a/system/default_ad_api/src/vehicle_door.hpp +++ b/system/default_ad_api/src/vehicle_door.hpp @@ -19,6 +19,8 @@ #include #include +#include + // This file should be included after messages. #include "utils/types.hpp" @@ -31,6 +33,7 @@ class VehicleDoorNode : public rclcpp::Node explicit VehicleDoorNode(const rclcpp::NodeOptions & options); private: + void on_status(vehicle_interface::DoorStatus::Message::ConstSharedPtr msg); rclcpp::CallbackGroup::SharedPtr group_cli_; Srv srv_command_; Srv srv_layout_; @@ -38,6 +41,7 @@ class VehicleDoorNode : public rclcpp::Node Cli cli_command_; Cli cli_layout_; Sub sub_status_; + std::optional status_; }; } // namespace default_ad_api diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp index 553c21b5ff4d7..496c95722002a 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp @@ -68,10 +68,10 @@ FileLoader::FileLoader(const PathConfig * path) } TreeData tree = TreeData::Load(path->resolved); - const auto paths = tree.optional("files").children("files"); + const auto files = tree.optional("files").children("files"); const auto edits = tree.optional("edits").children("edits"); const auto units = tree.optional("units").children("units"); - for (const auto & data : paths) create_path_config(data); + for (const auto & data : files) create_path_config(data); for (const auto & data : edits) create_edit_config(data); for (const auto & data : units) create_unit_config(data); } diff --git a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp b/system/diagnostic_graph_aggregator/src/common/graph/data.cpp index ff186098c32e3..2a4aef7e39c1f 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/data.cpp @@ -35,25 +35,25 @@ TreeData::TreeData(const YAML::Node & yaml, const TreePath & path) : path_(path) TreeData::Item TreeData::required(const std::string & name) { // TODO(Takagi, Isamu): check map type. - const auto path = path_.field(name); + const auto tree_path = path_.field(name); if (!yaml_[name]) { - throw FieldNotFound(path); + throw FieldNotFound(tree_path); } const auto data = yaml_[name]; yaml_.remove(name); - return TreeData(data, path); + return TreeData(data, tree_path); } TreeData::Item TreeData::optional(const std::string & name) { // TODO(Takagi, Isamu): check map type. - const auto path = path_.field(name); + const auto tree_path = path_.field(name); if (!yaml_[name]) { - return TreeData(YAML::Node(YAML::NodeType::Undefined), path); + return TreeData(YAML::Node(YAML::NodeType::Undefined), tree_path); } const auto data = yaml_[name]; yaml_.remove(name); - return TreeData(data, path); + return TreeData(data, tree_path); } bool TreeData::is_valid() const diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp index da801ae078e6c..d9cbdbaa64400 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp @@ -84,12 +84,12 @@ void NodeUnit::initialize_status() LeafUnit::LeafUnit(const UnitLoader & unit) : BaseUnit(unit) { - const auto node = unit.data().required("node").text(); - const auto name = unit.data().required("name").text(); - const auto sep = node.empty() ? "" : ": "; + const auto diag_node = unit.data().required("node").text(); + const auto diag_name = unit.data().required("name").text(); + const auto sep = diag_node.empty() ? "" : ": "; struct_.path = unit.path(); - struct_.name = node + sep + name; + struct_.name = diag_node + sep + diag_name; status_.level = DiagnosticStatus::STALE; } diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index f12e34c8aca46..cbb37343be05e 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -198,7 +198,8 @@ int get_ata_identify(int fd, HddInfo * info) // Create a control structure hdr.interface_id = 'S'; // This must be set to 'S' hdr.dxfer_direction = SG_DXFER_FROM_DEV; // a SCSI READ command - hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + // cppcheck-suppress cstyleCast hdr.cmdp = (unsigned char *)&ata; // SCSI command to be executed hdr.dxfer_len = sizeof(data); // number of bytes to be moved in the data transfer hdr.dxferp = data; // a pointer to user memory @@ -260,7 +261,8 @@ int get_ata_smart_data(int fd, HddInfo * info, const HddDevice & device) // Create a control structure hdr.interface_id = 'S'; // This must be set to 'S' hdr.dxfer_direction = SG_DXFER_FROM_DEV; // a SCSI READ command - hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + // cppcheck-suppress cstyleCast hdr.cmdp = (unsigned char *)&ata; // SCSI command to be executed hdr.dxfer_len = sizeof(data); // number of bytes to be moved in the data transfer hdr.dxferp = &data; // a pointer to user memory @@ -545,6 +547,7 @@ void run(int port) addr.sin_family = AF_INET; addr.sin_port = htons(port); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index 16f8fe9b93477..6de88d9e23276 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -114,6 +114,7 @@ void run(int port, const std::vector & list) addr.sin_family = AF_INET; addr.sin_port = htons(port); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp index e78d49b55a3df..1e6325d9c452b 100644 --- a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp +++ b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp @@ -76,6 +76,7 @@ void CPUMonitor::checkThrottling(diagnostic_updater::DiagnosticStatusWrapper & s addr.sin_family = AF_INET; addr.sin_port = htons(msr_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { stat.summary(DiagStatus::ERROR, "connect error"); diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index c57aea81d410d..85a88d2430bce 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -615,6 +615,7 @@ void HddMonitor::updateHddInfoList() addr.sin_family = AF_INET; addr.sin_port = htons(hdd_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { connect_diag_.summary(DiagStatus::ERROR, "connect error"); @@ -854,6 +855,7 @@ int HddMonitor::unmountDevice(std::string & device) addr.sin_family = AF_INET; addr.sin_port = htons(hdd_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { RCLCPP_ERROR(get_logger(), "socket connect error. %s", strerror(errno)); diff --git a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp index 0f321efe5c311..45ce75cbe3fae 100644 --- a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp +++ b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp @@ -200,6 +200,7 @@ void * msr_reader(void * args) addr.sin_family = AF_INET; addr.sin_port = htons(7634); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { close(sock); diff --git a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp index 4a948029a2f3a..aa5359ba1ceb6 100644 --- a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp +++ b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp @@ -206,6 +206,7 @@ void * hdd_reader(void * args) addr.sin_family = AF_INET; addr.sin_port = htons(7635); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { close(sock);