From 07e226c5c48fa5223f1b4b75316dfbbeece6a733 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 8 Jul 2024 08:10:59 +0900 Subject: [PATCH 01/17] fix(pointcloud_preprocessor): temporarily remove distortion corrector unit test (#7879) fix: temporarily remove distortion corretor unit test Signed-off-by: vividf --- .../pointcloud_preprocessor/CMakeLists.txt | 4 - .../test/test_distortion_corrector_node.cpp | 600 ------------------ 2 files changed, 604 deletions(-) delete mode 100644 sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 2d0649fe43954..53b6aae55d47b 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -231,12 +231,8 @@ if(BUILD_TESTING) ament_add_gtest(test_utilities test/test_utilities.cpp ) - ament_add_gtest(distortion_corrector_node_tests - test/test_distortion_corrector_node.cpp - ) target_link_libraries(test_utilities pointcloud_preprocessor_filter) - target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter) endif() diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp deleted file mode 100644 index 9e5d068be52f5..0000000000000 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ /dev/null @@ -1,600 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" - -#include - -#include -#include -#include -#include - -#include -#include - -class DistortionCorrectorTest : public ::testing::Test -{ -protected: - void SetUp() override - { - node_ = std::make_shared("test_node"); - distortion_corrector_2d_ = - std::make_shared(node_.get()); - distortion_corrector_3d_ = - std::make_shared(node_.get()); - - // Setup TF - tf_broadcaster_ = std::make_shared(node_); - tf_broadcaster_->sendTransform(generateStaticTransformMsg()); - - // Spin the node for a while to ensure transforms are published - auto start = std::chrono::steady_clock::now(); - auto timeout = std::chrono::seconds(1); - while (std::chrono::steady_clock::now() - start < timeout) { - rclcpp::spin_some(node_); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } - - void TearDown() override {} - - rclcpp::Time addMilliseconds(rclcpp::Time stamp, int ms) - { - auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); - return stamp + ms_in_ns; - } - - rclcpp::Time subtractMilliseconds(rclcpp::Time stamp, int ms) - { - auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); - return stamp - ms_in_ns; - } - - geometry_msgs::msg::TransformStamped generateTransformMsg( - const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, - double qx, double qy, double qz, double qw) - { - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = node_->get_clock()->now(); - tf_msg.header.frame_id = parent_frame; - tf_msg.child_frame_id = child_frame; - tf_msg.transform.translation.x = x; - tf_msg.transform.translation.y = y; - tf_msg.transform.translation.z = z; - tf_msg.transform.rotation.x = qx; - tf_msg.transform.rotation.y = qy; - tf_msg.transform.rotation.z = qz; - tf_msg.transform.rotation.w = qw; - return tf_msg; - } - - std::vector generateStaticTransformMsg() - { - return { - generateTransformMsg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), - generateTransformMsg("base_link", "imu_link", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; - } - - std::shared_ptr generateTwistMsg( - double linear_x, double angular_z, rclcpp::Time stamp) - { - auto twist_msg = std::make_shared(); - twist_msg->header.stamp = stamp; - twist_msg->header.frame_id = "base_link"; - twist_msg->twist.twist.linear.x = linear_x; - twist_msg->twist.twist.angular.z = angular_z; - return twist_msg; - } - - std::shared_ptr generateImuMsg( - double angular_vel_x, double angular_vel_y, double angular_vel_z, rclcpp::Time stamp) - { - auto imu_msg = std::make_shared(); - imu_msg->header.stamp = stamp; - imu_msg->header.frame_id = "imu_link"; - imu_msg->angular_velocity.x = angular_vel_x; - imu_msg->angular_velocity.y = angular_vel_y; - imu_msg->angular_velocity.z = angular_vel_z; - return imu_msg; - } - - std::vector> generateTwistMsgs( - rclcpp::Time pointcloud_timestamp) - { - std::vector> twist_msgs; - rclcpp::Time twist_stamp = subtractMilliseconds(pointcloud_timestamp, 5); - - for (int i = 0; i < 6; ++i) { - auto twist_msg = generateTwistMsg(10.0 + i * 2, 0.02 + i * 0.01, twist_stamp); - twist_msgs.push_back(twist_msg); - // Make sure the twist stamp is not identical to any point stamp, and imu stamp - twist_stamp = addMilliseconds(twist_stamp, 24); - } - - return twist_msgs; - } - - std::vector> generateImuMsgs( - rclcpp::Time pointcloud_timestamp) - { - std::vector> imu_msgs; - rclcpp::Time imu_stamp = subtractMilliseconds(pointcloud_timestamp, 10); - - for (int i = 0; i < 6; ++i) { - auto imu_msg = - generateImuMsg(0.01 + i * 0.005, -0.02 + i * 0.005, 0.05 + i * 0.005, imu_stamp); - imu_msgs.push_back(imu_msg); - // Make sure the imu stamp is not identical to any point stamp, and twist stamp - imu_stamp = addMilliseconds(imu_stamp, 27); - } - - return imu_msgs; - } - - sensor_msgs::msg::PointCloud2 generatePointCloudMsg( - bool is_generate_points, bool is_lidar_frame, rclcpp::Time stamp) - { - sensor_msgs::msg::PointCloud2 pointcloud_msg; - pointcloud_msg.header.stamp = stamp; - pointcloud_msg.header.frame_id = is_lidar_frame ? "lidar_top" : "base_link"; - pointcloud_msg.height = 1; - pointcloud_msg.is_dense = true; - pointcloud_msg.is_bigendian = false; - pointcloud_msg.point_step = - 20; // 3 float32 fields * 4 bytes/field + 1 float64 field * 8 bytes/field - - if (is_generate_points) { - std::vector points = { - 10.0f, 0.0f, 0.0f, // point 1 - 0.0f, 10.0f, 0.0f, // point 2 - 0.0f, 0.0f, 10.0f, // point 3 - 20.0f, 0.0f, 0.0f, // point 4 - 0.0f, 20.0f, 0.0f, // point 5 - 0.0f, 0.0f, 20.0f, // point 6 - 30.0f, 0.0f, 0.0f, // point 7 - 0.0f, 30.0f, 0.0f, // point 8 - 0.0f, 0.0f, 30.0f, // point 9 - 10.0f, 10.0f, 10.0f // point 10 - }; - - size_t number_of_points = points.size() / 3; - std::vector timestamps = - generatePointTimestamps(is_generate_points, stamp, number_of_points); - - std::vector data(number_of_points * pointcloud_msg.point_step); - - for (size_t i = 0; i < number_of_points; ++i) { - std::memcpy(data.data() + i * pointcloud_msg.point_step, &points[i * 3], 3 * sizeof(float)); - std::memcpy( - data.data() + i * pointcloud_msg.point_step + 12, ×tamps[i], sizeof(double)); - } - - pointcloud_msg.width = number_of_points; - pointcloud_msg.row_step = pointcloud_msg.point_step * pointcloud_msg.width; - pointcloud_msg.data = std::move(data); - } else { - pointcloud_msg.width = 0; - pointcloud_msg.row_step = 0; - } - - sensor_msgs::msg::PointField x_field; - x_field.name = "x"; - x_field.offset = 0; - x_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - x_field.count = 1; - - sensor_msgs::msg::PointField y_field; - y_field.name = "y"; - y_field.offset = 4; - y_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - y_field.count = 1; - - sensor_msgs::msg::PointField z_field; - z_field.name = "z"; - z_field.offset = 8; - z_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - z_field.count = 1; - - sensor_msgs::msg::PointField timestamp_field; - timestamp_field.name = "time_stamp"; - timestamp_field.offset = 12; - timestamp_field.datatype = sensor_msgs::msg::PointField::FLOAT64; - timestamp_field.count = 1; - - pointcloud_msg.fields = {x_field, y_field, z_field, timestamp_field}; - - return pointcloud_msg; - } - - std::vector generatePointTimestamps( - bool is_generate_points, rclcpp::Time pointcloud_timestamp, size_t number_of_points) - { - std::vector timestamps; - if (is_generate_points) { - rclcpp::Time point_stamp = pointcloud_timestamp; - for (size_t i = 0; i < number_of_points; ++i) { - double timestamp = point_stamp.seconds(); - timestamps.push_back(timestamp); - if (i > 0) { - point_stamp = point_stamp + rclcpp::Duration::from_seconds( - 0.001); // Assuming 1ms offset for demonstration - } - } - } - return timestamps; - } - - std::shared_ptr node_; - std::shared_ptr distortion_corrector_2d_; - std::shared_ptr distortion_corrector_3d_; - std::shared_ptr tf_broadcaster_; -}; - -TEST_F(DistortionCorrectorTest, TestProcessTwistMessage) -{ - auto twist_msg = generateTwistMsg(1.0, 0.5, node_->get_clock()->now()); - distortion_corrector_2d_->processTwistMessage(twist_msg); - - ASSERT_FALSE(distortion_corrector_2d_->twist_queue_.empty()); - EXPECT_EQ(distortion_corrector_2d_->twist_queue_.front().twist.linear.x, 1.0); - EXPECT_EQ(distortion_corrector_2d_->twist_queue_.front().twist.angular.z, 0.5); -} - -TEST_F(DistortionCorrectorTest, TestProcessIMUMessage) -{ - auto imu_msg = generateImuMsg(0.5, 0.3, 0.1, node_->get_clock()->now()); - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - - ASSERT_FALSE(distortion_corrector_2d_->angular_velocity_queue_.empty()); - EXPECT_NEAR(distortion_corrector_2d_->angular_velocity_queue_.front().vector.z, 0.0443032, 1e-5); -} - -TEST_F(DistortionCorrectorTest, TestIsInputValid) -{ - // input normal pointcloud without twist - sensor_msgs::msg::PointCloud2 pointcloud = - generatePointCloudMsg(true, false, node_->get_clock()->now()); - bool result = distortion_corrector_2d_->isInputValid(pointcloud); - EXPECT_FALSE(result); - - // input normal pointcloud with valid twist - auto twist_msg = generateTwistMsg(1.0, 0.5, node_->get_clock()->now()); - distortion_corrector_2d_->processTwistMessage(twist_msg); - - pointcloud = generatePointCloudMsg(true, false, node_->get_clock()->now()); - result = distortion_corrector_2d_->isInputValid(pointcloud); - EXPECT_TRUE(result); - - // input empty pointcloud - pointcloud = generatePointCloudMsg(false, false, node_->get_clock()->now()); - result = distortion_corrector_2d_->isInputValid(pointcloud); - EXPECT_FALSE(result); -} - -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink) -{ - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed_); -} - -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame) -{ - distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed_); -} - -TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame) -{ - distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame"); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed_); -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) -{ - rclcpp::Time timestamp = node_->get_clock()->now(); - // Generate the point cloud message - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Process empty twist queue - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->undistortPointCloud(false, pointcloud); - - // Verify the point cloud is not changed - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0f, 0.0f, 10.0f}, {20.0f, 0.0f, 0.0f}, - {0.0f, 20.0f, 0.0f}, {0.0f, 0.0f, 20.0f}, {30.0f, 0.0f, 0.0f}, {0.0f, 30.0f, 0.0f}, - {0.0f, 0.0f, 30.0f}, {10.0f, 10.0f, 10.0f}, - }; - - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 1e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 1e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 1e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud) -{ - rclcpp::Time timestamp = node_->get_clock()->now(); - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - // Generate an empty point cloud message - sensor_msgs::msg::PointCloud2 empty_pointcloud = generatePointCloudMsg(false, false, timestamp); - - // Process empty point cloud - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->undistortPointCloud(true, empty_pointcloud); - - // Verify the point cloud is still empty - EXPECT_EQ(empty_pointcloud.width, 0); - EXPECT_EQ(empty_pointcloud.row_step, 0); -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - - // Test using only twist - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(false, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.012002f, 5.75338e-07f, 10.0f}, - {20.024f, 0.00191863f, 0.0f}, {0.0340828f, 20.0f, 0.0f}, {0.0479994f, 4.02654e-06f, 20.0f}, - {30.06f, 0.00575818f, 0.0f}, {0.0662481f, 30.0f, 0.0f}, {0.0839996f, 1.03542e-05f, 30.0f}, - {10.0931f, 10.0029f, 10.0f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_2d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0119991f, 5.75201e-07f, 10.0f}, - {20.024f, 0.0019192f, 0.0f}, {0.0321653f, 20.0f, 0.0f}, {0.0479994f, 6.32762e-06f, 20.0f}, - {30.06f, 0.00863842f, 0.0f}, {0.063369f, 30.0f, 0.0f}, {0.0839996f, 1.84079e-05f, 30.0f}, - {10.0912f, 10.0048f, 10.0f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_2d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_2d_->initialize(); - distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); - distortion_corrector_2d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, -1.77636e-15f, -4.44089e-16f}, {-2.66454e-15f, 10.0f, -8.88178e-16f}, - {0.00622853f, 0.00600991f, 10.0084f}, {20.0106f, 0.010948f, 0.0157355f}, - {0.0176543f, 20.0176f, 0.0248379f}, {0.024499f, 0.0245179f, 20.0348f}, - {30.0266f, 0.0255826f, 0.0357166f}, {0.0355204f, 30.0353f, 0.0500275f}, - {0.047132f, 0.0439795f, 30.0606f}, {10.0488f, 10.046f, 10.0636f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } - - // Test using only twist - distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(false, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0119991f, 0.0f, 10.0f}, - {20.024f, 0.00120042f, 0.0f}, {0.0342002f, 20.0f, 0.0f}, {0.0479994f, 2.15994e-06f, 20.0f}, - {30.06f, 0.00450349f, 0.0f}, {0.0666005f, 30.0f, 0.0f}, {0.0839996f, 7.55993e-06f, 30.0f}, - {10.0936f, 10.0024f, 10.0f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); - distortion_corrector_3d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, - {0.0f, 10.0f, 0.0f}, - {0.0118491f, -0.000149993f, 10.0f}, - {20.024f, 0.00220075f, 0.000600241f}, - {0.0327002f, 20.0f, 0.000900472f}, - {0.0468023f, -0.00119623f, 20.0f}, - {30.06f, 0.0082567f, 0.00225216f}, - {0.0621003f, 30.0f, 0.00270227f}, - {0.0808503f, -0.00313673f, 30.0f}, - {10.0904f, 10.0032f, 10.0024f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) -{ - // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); - sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); - - // Generate and process multiple twist messages - auto twist_msgs = generateTwistMsgs(timestamp); - for (const auto & twist_msg : twist_msgs) { - distortion_corrector_3d_->processTwistMessage(twist_msg); - } - - // Generate and process multiple IMU messages - auto imu_msgs = generateImuMsgs(timestamp); - for (const auto & imu_msg : imu_msgs) { - distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); - } - - distortion_corrector_3d_->initialize(); - distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top"); - distortion_corrector_3d_->undistortPointCloud(true, pointcloud); - - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, - {-4.76837e-07f, 10.0f, 4.76837e-07f}, - {0.00572586f, 0.00616837f, 10.0086f}, - {20.0103f, 0.0117249f, 0.0149349f}, - {0.0158343f, 20.0179f, 0.024497f}, - {0.0251098f, 0.0254798f, 20.0343f}, - {30.0259f, 0.0290527f, 0.034577f}, - {0.0319824f, 30.0358f, 0.0477753f}, - {0.0478067f, 0.0460052f, 30.06f}, - {10.0462f, 10.0489f, 10.0625f}, - }; - - // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); - } -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} From e1cd1da4dee812d508a3ec6ed615855721563e23 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 8 Jul 2024 09:10:41 +0900 Subject: [PATCH 02/17] refactor(gnss_poser): apply static analysis (#7874) * refactor based on linter Signed-off-by: a-maumau * style(pre-commit): autofix --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/gnss_poser/gnss_poser_core.hpp | 26 +++---- sensing/gnss_poser/src/gnss_poser_core.cpp | 71 ++++++++++--------- 2 files changed, 50 insertions(+), 47 deletions(-) diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 97ca1d8bffe77..033abf04f1255 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -49,29 +49,29 @@ class GNSSPoser : public rclcpp::Node private: using MapProjectorInfo = map_interface::MapProjectorInfo; - void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); - void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); - void callbackGnssInsOrientationStamped( + void callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); + void callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); - bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); - bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - geometry_msgs::msg::Point getMedianPosition( + static bool is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); + static bool can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); + static geometry_msgs::msg::Point get_median_position( const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Point getAveragePosition( + static geometry_msgs::msg::Point get_average_position( const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); - geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( + static geometry_msgs::msg::Quaternion get_quaternion_by_heading(const int heading); + static geometry_msgs::msg::Quaternion get_quaternion_by_position_difference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); - bool getTransform( + bool get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); - bool getStaticTransform( + bool get_static_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, const builtin_interfaces::msg::Time & stamp); - void publishTF( + void publish_tf( const std::string & frame_id, const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg); @@ -99,7 +99,7 @@ class GNSSPoser : public rclcpp::Node autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; - int gnss_pose_pub_method; + int gnss_pose_pub_method_; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 7a3b40336ff3d..40a60d17ea7db 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -36,25 +36,27 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), msg_gnss_ins_orientation_stamped_( std::make_shared()), - gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method")) + gnss_pose_pub_method_(static_cast(declare_parameter("gnss_pose_pub_method"))) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); adaptor.init_sub( - sub_map_projector_info_, - [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); + sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { + callback_map_projector_info(msg); + }); // Set up position buffer - int buff_epoch = declare_parameter("buff_epoch"); + int buff_epoch = static_cast(declare_parameter("buff_epoch")); position_buffer_.set_capacity(buff_epoch); // Set subscribers and publishers nav_sat_fix_sub_ = create_subscription( - "fix", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavSatFix, this, std::placeholders::_1)); + "fix", rclcpp::QoS{1}, + std::bind(&GNSSPoser::callback_nav_sat_fix, this, std::placeholders::_1)); autoware_orientation_sub_ = create_subscription( "autoware_orientation", rclcpp::QoS{1}, - std::bind(&GNSSPoser::callbackGnssInsOrientationStamped, this, std::placeholders::_1)); + std::bind(&GNSSPoser::callback_gnss_ins_orientation_stamped, this, std::placeholders::_1)); pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); pose_cov_pub_ = create_publisher( @@ -68,13 +70,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0; } -void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +void GNSSPoser::callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg) { projector_info_ = *msg; received_map_projector_info_ = true; } -void GNSSPoser::callbackNavSatFix( +void GNSSPoser::callback_nav_sat_fix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { // Return immediately if map_projector_info has not been received yet. @@ -94,15 +96,15 @@ void GNSSPoser::callbackNavSatFix( } // check fixed topic - const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); + const bool is_status_fixed = is_fixed(nav_sat_fix_msg_ptr->status); // publish is_fixed topic tier4_debug_msgs::msg::BoolStamped is_fixed_msg; is_fixed_msg.stamp = this->now(); - is_fixed_msg.data = is_fixed; + is_fixed_msg.data = is_status_fixed; fixed_pub_->publish(is_fixed_msg); - if (!is_fixed) { + if (!is_status_fixed) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), "Not Fixed Topic. Skipping Calculate."); @@ -122,7 +124,7 @@ void GNSSPoser::callbackNavSatFix( geometry_msgs::msg::Pose gnss_antenna_pose{}; // publish pose immediately - if (!gnss_pose_pub_method) { + if (!gnss_pose_pub_method_) { gnss_antenna_pose.position = position; } else { // fill position buffer @@ -134,8 +136,9 @@ void GNSSPoser::callbackNavSatFix( return; } // publish average pose or median pose of position buffer - gnss_antenna_pose.position = (gnss_pose_pub_method == 1) ? getAveragePosition(position_buffer_) - : getMedianPosition(position_buffer_); + gnss_antenna_pose.position = (gnss_pose_pub_method_ == 1) + ? get_average_position(position_buffer_) + : get_median_position(position_buffer_); } // calc gnss antenna orientation @@ -144,7 +147,7 @@ void GNSSPoser::callbackNavSatFix( orientation = msg_gnss_ins_orientation_stamped_->orientation.orientation; } else { static auto prev_position = gnss_antenna_pose.position; - orientation = getQuaternionByPositionDifference(gnss_antenna_pose.position, prev_position); + orientation = get_quaternion_by_position_difference(gnss_antenna_pose.position, prev_position); prev_position = gnss_antenna_pose.position; } @@ -157,7 +160,7 @@ void GNSSPoser::callbackNavSatFix( auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; - getStaticTransform( + get_static_transform( gnss_frame, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); @@ -179,11 +182,11 @@ void GNSSPoser::callbackNavSatFix( gnss_base_pose_cov_msg.header = gnss_base_pose_msg.header; gnss_base_pose_cov_msg.pose.pose = gnss_base_pose_msg.pose; gnss_base_pose_cov_msg.pose.covariance[7 * 0] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; gnss_base_pose_cov_msg.pose.covariance[7 * 1] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; gnss_base_pose_cov_msg.pose.covariance[7 * 2] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; if (use_gnss_ins_orientation_) { gnss_base_pose_cov_msg.pose.covariance[7 * 3] = @@ -201,30 +204,30 @@ void GNSSPoser::callbackNavSatFix( pose_cov_pub_->publish(gnss_base_pose_cov_msg); // broadcast map to gnss_base_link - publishTF(map_frame_, gnss_base_frame_, gnss_base_pose_msg); + publish_tf(map_frame_, gnss_base_frame_, gnss_base_pose_msg); } -void GNSSPoser::callbackGnssInsOrientationStamped( +void GNSSPoser::callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg) { *msg_gnss_ins_orientation_stamped_ = *msg; } -bool GNSSPoser::isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) +bool GNSSPoser::is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) { return nav_sat_status_msg.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; } -bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) +bool GNSSPoser::can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) { return nav_sat_fix_msg.position_covariance_type > sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -geometry_msgs::msg::Point GNSSPoser::getMedianPosition( +geometry_msgs::msg::Point GNSSPoser::get_median_position( const boost::circular_buffer & position_buffer) { - auto getMedian = [](std::vector array) { + auto get_median = [](std::vector array) { std::sort(std::begin(array), std::end(array)); const size_t median_index = array.size() / 2; double median = (array.size() % 2) @@ -243,13 +246,13 @@ geometry_msgs::msg::Point GNSSPoser::getMedianPosition( } geometry_msgs::msg::Point median_point; - median_point.x = getMedian(array_x); - median_point.y = getMedian(array_y); - median_point.z = getMedian(array_z); + median_point.x = get_median(array_x); + median_point.y = get_median(array_y); + median_point.z = get_median(array_z); return median_point; } -geometry_msgs::msg::Point GNSSPoser::getAveragePosition( +geometry_msgs::msg::Point GNSSPoser::get_average_position( const boost::circular_buffer & position_buffer) { std::vector array_x; @@ -271,7 +274,7 @@ geometry_msgs::msg::Point GNSSPoser::getAveragePosition( return average_point; } -geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int heading) +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_heading(const int heading) { int heading_conv = 0; // convert heading[0(North)~360] to yaw[-M_PI(West)~M_PI] @@ -288,7 +291,7 @@ geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int headi return tf2::toMsg(quaternion); } -geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByPositionDifference( +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_position_difference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point) { const double yaw = std::atan2(point.y - prev_point.y, point.x - prev_point.x); @@ -297,7 +300,7 @@ geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByPositionDifference( return tf2::toMsg(quaternion); } -bool GNSSPoser::getTransform( +bool GNSSPoser::get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr) { @@ -340,7 +343,7 @@ bool GNSSPoser::getTransform( return true; } -bool GNSSPoser::getStaticTransform( +bool GNSSPoser::get_static_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, const builtin_interfaces::msg::Time & stamp) @@ -385,7 +388,7 @@ bool GNSSPoser::getStaticTransform( return true; } -void GNSSPoser::publishTF( +void GNSSPoser::publish_tf( const std::string & frame_id, const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg) { From 4dc308000b44ece5ca0df03454a97ef86bc4b9e1 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 8 Jul 2024 11:18:31 +0900 Subject: [PATCH 03/17] fix(pointcloud_preprocessor): fix documentation (#7878) fix: fix wrong name for json file Signed-off-by: vividf Co-authored-by: Kenzo Lobos Tsunekawa --- sensing/pointcloud_preprocessor/docs/distortion-corrector.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 96124f473f825..033e82607057a 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -36,7 +36,7 @@ Please note that the processing time difference between the two distortion metho ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector.schema.json") }} +{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json") }} ## Launch From fa6d8c583539cfd30d3eb537e4bcdd5fc5ef03c4 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 8 Jul 2024 13:47:05 +0900 Subject: [PATCH 04/17] chore: update CODEOWNERS (#7819) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions Co-authored-by: Kenzo Lobos Tsunekawa --- .github/CODEOWNERS | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 8f947ce2d2a13..0fe324f84f81a 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -6,7 +6,7 @@ common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.j common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp -common/autoware_point_types/** taichi.higashide@tier4.jp +common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -115,9 +115,9 @@ perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4 perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp -perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp +perception/lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@tier4.jp +perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com yoshi.ri@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -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 daniel.sanchez@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@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 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/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/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 @@ -204,7 +204,7 @@ sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minod sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp +sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp From 598d9375e915758711c7b63e0e9446a81efc0430 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 8 Jul 2024 14:49:05 +0900 Subject: [PATCH 05/17] fix(motion_planning): fix processing time topic names (#7885) Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_planner_node/src/node.cpp | 4 ++-- .../autoware_motion_velocity_planner_node/src/node.hpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) 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 f0100f58074ba..f181b0c76b51e 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 @@ -83,8 +83,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); - processing_time_publisher_ = this->create_publisher( - "~/debug/total_time/processing_time_ms", 1); + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 8debb9cbedf00..4bf24cadb36f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -97,7 +97,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; - autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; From 45a0c3abd0a7436e650b937c5b8da8833f0ba535 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 15:53:24 +0900 Subject: [PATCH 06/17] refactor(probabilistic_occupancy_grid_map)!: fix namespace and directory structure (#7872) * refactor: update include paths for probabilistic occupancy grid map Signed-off-by: Taekjin LEE * refactor: update include paths for probabilistic occupancy grid map Signed-off-by: Taekjin LEE * refactor: update namespace for nodes Signed-off-by: Taekjin LEE * refactor: update namespace for lib Signed-off-by: Taekjin LEE * refactor: remove unused dependency Signed-off-by: Taekjin LEE * refactor: use const pointer for occupancy grid map data Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../CMakeLists.txt | 35 ++++++++++--------- .../cost_value}/cost_value.hpp | 14 +++++--- .../costmap_2d}/occupancy_grid_map.hpp | 9 +++-- .../costmap_2d}/occupancy_grid_map_base.hpp | 9 +++-- .../costmap_2d}/occupancy_grid_map_fixed.hpp | 11 +++--- .../occupancy_grid_map_projective.hpp | 11 +++--- .../fusion_policy/fusion_policy.hpp} | 12 ++++--- .../updater/binary_bayes_filter_updater.hpp} | 11 +++--- .../log_odds_bayes_filter_updater.hpp} | 15 ++++---- .../updater/ogm_updater_interface.hpp} | 14 ++++---- .../utils/utils.hpp | 12 ++++--- ...serscan_based_occupancy_grid_map.launch.py | 2 +- ...ntcloud_based_occupancy_grid_map.launch.py | 2 +- ...ntcloud_based_occupancy_grid_map.launch.py | 2 +- .../costmap_2d}/occupancy_grid_map.cpp | 17 +++++---- .../costmap_2d}/occupancy_grid_map_base.cpp | 12 ++++--- .../costmap_2d}/occupancy_grid_map_fixed.cpp | 29 ++++++++------- .../occupancy_grid_map_projective.cpp | 26 ++++++++------ .../fusion_policy/fusion_policy.cpp} | 9 +++-- .../updater/binary_bayes_filter_updater.cpp} | 13 ++++--- .../log_odds_bayes_filter_updater.cpp} | 10 ++++-- .../{src => lib}/utils/utils.cpp | 15 ++++---- .../package.xml | 5 +-- .../synchronized_grid_map_fusion_node.cpp | 20 +++++------ .../synchronized_grid_map_fusion_node.hpp | 18 +++++----- ...aserscan_based_occupancy_grid_map_node.cpp | 14 ++++---- ...aserscan_based_occupancy_grid_map_node.hpp | 16 ++++----- ...intcloud_based_occupancy_grid_map_node.cpp | 18 +++++----- ...intcloud_based_occupancy_grid_map_node.hpp | 16 ++++----- .../test/cost_value_test.cpp | 6 ++-- .../test/fusion_policy_test.cpp | 9 ++--- .../test/test_utils.cpp | 12 +++---- 32 files changed, 241 insertions(+), 183 deletions(-) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/cost_value}/cost_value.hpp (90%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map.hpp (91%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_base.hpp (91%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_fixed.hpp (74%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map => autoware/probabilistic_occupancy_grid_map/costmap_2d}/occupancy_grid_map_projective.hpp (77%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp => autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp} (84%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp => autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp} (72%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp => autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp} (69%) rename perception/probabilistic_occupancy_grid_map/include/{probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp => autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp} (68%) rename perception/probabilistic_occupancy_grid_map/include/{ => autoware}/probabilistic_occupancy_grid_map/utils/utils.hpp (90%) rename perception/probabilistic_occupancy_grid_map/{src/laserscan_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map.cpp (93%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_base.cpp (95%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_fixed.cpp (90%) rename perception/probabilistic_occupancy_grid_map/{src/pointcloud_based_occupancy_grid_map => lib/costmap_2d}/occupancy_grid_map_projective.cpp (93%) rename perception/probabilistic_occupancy_grid_map/{src/fusion/single_frame_fusion_policy.cpp => lib/fusion_policy/fusion_policy.cpp} (97%) rename perception/probabilistic_occupancy_grid_map/{src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp => lib/updater/binary_bayes_filter_updater.cpp} (88%) rename perception/probabilistic_occupancy_grid_map/{src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp => lib/updater/log_odds_bayes_filter_updater.cpp} (88%) rename perception/probabilistic_occupancy_grid_map/{src => lib}/utils/utils.cpp (95%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/fusion/synchronized_grid_map_fusion_node.hpp (85%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp (82%) rename perception/probabilistic_occupancy_grid_map/{include/probabilistic_occupancy_grid_map => src}/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp (81%) diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 928532f928e38..d01ef157e21b4 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -14,8 +14,8 @@ include_directories( ) ament_auto_add_library(${PROJECT_NAME}_common SHARED - src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/updater/binary_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(${PROJECT_NAME}_common ${PCL_LIBRARIES} @@ -24,9 +24,9 @@ target_link_libraries(${PROJECT_NAME}_common # PointcloudBasedOccupancyGridMap ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp + lib/costmap_2d/occupancy_grid_map_base.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/costmap_2d/occupancy_grid_map_projective.cpp ) target_link_libraries(pointcloud_based_occupancy_grid_map @@ -35,14 +35,14 @@ target_link_libraries(pointcloud_based_occupancy_grid_map ) rclcpp_components_register_node(pointcloud_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" EXECUTABLE pointcloud_based_occupancy_grid_map_node ) # LaserscanBasedOccupancyGridMap ament_auto_add_library(laserscan_based_occupancy_grid_map SHARED src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp - src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp + lib/costmap_2d/occupancy_grid_map.cpp ) target_link_libraries(laserscan_based_occupancy_grid_map @@ -51,17 +51,17 @@ target_link_libraries(laserscan_based_occupancy_grid_map ) rclcpp_components_register_node(laserscan_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" EXECUTABLE laserscan_based_occupancy_grid_map_node ) # GridMapFusionNode ament_auto_add_library(synchronized_grid_map_fusion SHARED src/fusion/synchronized_grid_map_fusion_node.cpp - src/fusion/single_frame_fusion_policy.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/fusion_policy/fusion_policy.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/updater/log_odds_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(synchronized_grid_map_fusion @@ -69,7 +69,7 @@ target_link_libraries(synchronized_grid_map_fusion ) rclcpp_components_register_node(synchronized_grid_map_fusion - PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + PLUGIN "autoware::occupancy_grid_map::GridMapFusionNode" EXECUTABLE synchronized_grid_map_fusion_node ) @@ -88,13 +88,14 @@ if(BUILD_TESTING) # gtest ament_add_gtest(test_utils - test/test_utils.cpp + test/test_utils.cpp ) ament_add_gtest(costmap_unit_tests - test/cost_value_test.cpp) + test/cost_value_test.cpp + ) ament_add_gtest(fusion_policy_unit_tests - test/fusion_policy_test.cpp - src/fusion/single_frame_fusion_policy.cpp + test/fusion_policy_test.cpp + lib/fusion_policy/fusion_policy.cpp ) target_link_libraries(test_utils ${PCL_LIBRARIES} diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp index 880297a1210ed..d470d206c41c6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp @@ -48,12 +48,14 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ #include -namespace occupancy_cost_value +namespace autoware::occupancy_grid_map +{ +namespace cost_value { static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; @@ -101,6 +103,8 @@ struct InverseCostTranslationTable static const CostTranslationTable cost_translation_table; static const InverseCostTranslationTable inverse_cost_translation_table; -} // namespace occupancy_cost_value -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +} // namespace cost_value +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp index d2210cf9c348a..93e768c0f6b4e 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -90,5 +92,6 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index f01b2d74e160b..2b2057af9cc12 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -91,5 +93,6 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp similarity index 74% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 298f327d632d7..9dfd8b8689123 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -43,5 +45,6 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp similarity index 77% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 67b51d6184c8c..1f0f92d933ff3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -51,5 +53,6 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp index 1f0553878ef5a..a2d7c90a023a5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -25,6 +25,8 @@ /*min and max prob threshold to prevent log-odds to diverge*/ #define EPSILON_PROB 0.03 +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; @@ -61,6 +63,8 @@ unsigned char singleFrameOccupancyFusion( unsigned char singleFrameOccupancyFusion( const std::vector & occupancy, FusionMethod method, const std::vector & reliability); + } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp similarity index 72% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp index 3a921035ef219..af28b7962b3bc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface @@ -38,5 +40,6 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp similarity index 69% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp index 245484e442609..d7bb1184c06d6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -25,6 +25,8 @@ // LOBF means: Log Odds Bayes Filter // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface @@ -45,5 +47,6 @@ class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp similarity index 68% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp index e85edf2245ef3..75231089ac33c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D @@ -27,8 +29,7 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D public: OccupancyGridMapUpdaterInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) - : Costmap2D( - cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) + : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } virtual ~OccupancyGridMapUpdaterInterface() = default; @@ -37,5 +38,6 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 0950272dac470..50b470d904ef6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -45,6 +45,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -117,6 +119,8 @@ bool extractCommonPointCloud( sensor_msgs::msg::PointCloud2 & output_obstacle_pc); unsigned char getApproximateOccupancyState(const unsigned char & value); + } // namespace utils +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 62cfa4bcb5228..1edc538b3de3f 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -85,7 +85,7 @@ def launch_setup(context, *args, **kwargs): ), ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index b112dd0a84b83..ef40839d2a5aa 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -196,7 +196,7 @@ def launch_setup(context, *args, **kwargs): # generate composable node node = ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", namespace=frame_name, remappings=[ diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 0e84e4860fdf3..bb5ef025a7e47 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): composable_nodes.append( ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ( diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp index 3d02be9ca7156..25eafcd564e2d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp @@ -49,21 +49,23 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMap::OccupancyGridMap( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -140,7 +142,7 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r raytraceFreespace(pointcloud, robot_pose); // occupied - MarkCell marker(costmap_, occupancy_cost_value::LETHAL_OBSTACLE); + MarkCell marker(costmap_, cost_value::LETHAL_OBSTACLE); for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y) { unsigned int mx, my; @@ -155,12 +157,12 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r void OccupancyGridMap::updateFreespaceCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE); + updateCellsByPointCloud(pointcloud, cost_value::FREE_SPACE); } void OccupancyGridMap::updateOccupiedCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE); + updateCellsByPointCloud(pointcloud, cost_value::LETHAL_OBSTACLE); } void OccupancyGridMap::updateCellsByPointCloud( @@ -244,9 +246,10 @@ void OccupancyGridMap::raytraceFreespace(const PointCloud2 & pointcloud, const P } constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold - MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE); + MarkCell marker(costmap_, cost_value::FREE_SPACE); raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); } } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 3d176e41583a0..5e9b5598c8f63 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -69,13 +69,16 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapInterface::OccupancyGridMapInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -230,3 +233,4 @@ void OccupancyGridMapInterface::raytrace( } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index d4e177209f99d..f25009d47bdcc 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -33,6 +33,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -136,7 +139,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } // Second step: Add unknown cell @@ -162,9 +165,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); if (!no_freespace_point) { const auto & target = *raw_distance_iter; - raytrace( - source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); } continue; } @@ -177,7 +179,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } else if (no_freespace_point) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } @@ -186,13 +188,13 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -203,7 +205,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -215,7 +217,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= distance_margin_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -229,3 +231,4 @@ void OccupancyGridMapFixedBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map 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/lib/costmap_2d/occupancy_grid_map_projective.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 03d7a42ae7043..a2400fb3c81b2 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -34,6 +34,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -192,7 +195,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, ray_end.wx, ray_end.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } if (pub_debug_grid_) @@ -219,7 +222,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); break; } @@ -227,7 +230,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); continue; } @@ -243,13 +246,13 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -262,7 +265,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -274,7 +277,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= obstacle_separation_threshold_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -299,3 +302,4 @@ void OccupancyGridMapProjectiveBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp similarity index 97% rename from perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp rename to perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp index 7e2c3f7ba304a..1e88b7a39fd14 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { @@ -61,9 +63,9 @@ namespace overwrite_fusion */ State getApproximateState(const unsigned char & occupancy) { - if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + if (occupancy >= cost_value::OCCUPIED_THRESHOLD) { return State::OCCUPIED; - } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + } else if (occupancy <= cost_value::FREE_THRESHOLD) { return State::FREE; } else { return State::UNKNOWN; @@ -320,3 +322,4 @@ unsigned char singleFrameOccupancyFusion( } } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp index 40f538a64f6e9..74506d095b36d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -48,15 +50,15 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( float pz{}; float not_pz{}; float po_hat{}; - if (z == occupancy_cost_value::LETHAL_OBSTACLE) { + if (z == cost_value::LETHAL_OBSTACLE) { pz = probability_matrix_(Index::OCCUPIED, Index::OCCUPIED); not_pz = probability_matrix_(Index::FREE, Index::OCCUPIED); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::FREE_SPACE) { + } else if (z == cost_value::FREE_SPACE) { pz = 1.f - probability_matrix_(Index::FREE, Index::FREE); not_pz = 1.f - probability_matrix_(Index::OCCUPIED, Index::FREE); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::NO_INFORMATION) { + } else if (z == cost_value::NO_INFORMATION) { const float inv_v_ratio = 1.f / v_ratio_; po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); } @@ -79,3 +81,4 @@ bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp index 607694d6da571..9f3c3c7e40eaf 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -35,7 +37,7 @@ inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( using fusion_policy::convertProbabilityToChar; using fusion_policy::log_odds_fusion::logOddsFusion; - constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown = cost_value::NO_INFORMATION; constexpr unsigned char unknown_margin = 1; /* Tau and ST decides how fast the observation decay to the unknown status*/ constexpr double tau = 0.75; @@ -68,4 +70,6 @@ bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupanc } return true; } + } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp rename to perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 209207fe10f34..e8c2cdb2617df 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -189,13 +191,14 @@ bool extractCommonPointCloud( */ unsigned char getApproximateOccupancyState(const unsigned char & value) { - if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { - return occupancy_cost_value::LETHAL_OBSTACLE; - } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { - return occupancy_cost_value::FREE_SPACE; + if (value >= cost_value::OCCUPIED_THRESHOLD) { + return cost_value::LETHAL_OBSTACLE; + } else if (value <= cost_value::FREE_THRESHOLD) { + return cost_value::FREE_SPACE; } else { - return occupancy_cost_value::NO_INFORMATION; + return cost_value::NO_INFORMATION; } } } // namespace utils +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index d8ae702ef60ca..d8647ae2ba744 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -13,9 +13,9 @@ ament_cmake_auto autoware_cmake + eigen3_cmake_module autoware_universe_utils - eigen3_cmake_module grid_map_costmap_2d grid_map_msgs grid_map_ros @@ -27,12 +27,9 @@ rclcpp rclcpp_components sensor_msgs - tf2 tf2_eigen - tf2_geometry_msgs tf2_ros tf2_sensor_msgs - visualization_msgs pointcloud_preprocessor pointcloud_to_laserscan diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index fa93db802b0ce..a271e17a1e566 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" +#include "synchronized_grid_map_fusion_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; using costmap_2d::OccupancyGridMapLOBFUpdater; @@ -404,7 +404,7 @@ nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; costmap2d.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } @@ -424,7 +424,7 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; gridmap.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } return gridmap; @@ -453,14 +453,14 @@ nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsg msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); - unsigned char * data = occupancy_grid_map.getCharMap(); + const unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp similarity index 85% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 84ca13c8b1881..c839f160aab9b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -45,7 +45,7 @@ // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -124,6 +124,6 @@ class GridMapFusionNode : public rclcpp::Node fusion_policy::FusionMethod fusion_method_; }; -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 777c180fe1a3a..6c4ad8135b37d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "laserscan_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -35,7 +35,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMap; using costmap_2d::OccupancyGridMapBBFUpdater; @@ -229,12 +229,12 @@ OccupancyGrid::UniquePtr LaserscanBasedOccupancyGridMapNode::OccupancyGridMapToM unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 82% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index a8adea6e700e5..a599f7b564c8b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -36,7 +36,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapUpdaterInterface; @@ -100,6 +100,6 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node bool enable_single_frame_mode_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index d019280aefda0..3fe75263b3f28 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "pointcloud_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -40,7 +40,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapBBFUpdater; using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -250,12 +250,12 @@ OccupancyGrid::UniquePtr PointcloudBasedOccupancyGridMapNode::OccupancyGridMapTo unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 81% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 1d119102dc28d..e0c7ef74988f4 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -38,7 +38,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapInterface; @@ -93,6 +93,6 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node bool filter_obstacle_pointcloud_by_raw_pointcloud_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp index c41c809a92b0c..f03852562f7ca 100644 --- a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the CostTranslationTable and InverseCostTranslationTable functions -using occupancy_cost_value::cost_translation_table; -using occupancy_cost_value::inverse_cost_translation_table; +using autoware::occupancy_grid_map::cost_value::cost_translation_table; +using autoware::occupancy_grid_map::cost_value::inverse_cost_translation_table; TEST(CostTranslationTableTest, TestRange) { diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp index 6b3dc8a2ebcef..78137371c720e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" + +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the log-odds update rule TEST(FusionPolicyTest, TestLogOddsUpdateRule) { - using fusion_policy::log_odds_fusion::logOddsFusion; + using autoware::occupancy_grid_map::fusion_policy::log_odds_fusion::logOddsFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; @@ -49,7 +50,7 @@ TEST(FusionPolicyTest, TestLogOddsUpdateRule) // Test the dempster-shafer update rule TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) { - using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + using autoware::occupancy_grid_map::fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 4bc3dca0a67bd..17df981d849d4 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. // autoware -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -77,14 +77,14 @@ TEST(TestUtils, TestCropPointcloudByHeight) sensor_msgs::msg::PointCloud2 test1_output, test2_output, test3_output; // case1: normal input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); // case2: normal input, empty output - EXPECT_NO_THROW(utils::cropPointcloudByHeight( + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( ros_cloud_10, mock_buffer, "base_link", -2.0, -0.1, test2_output)); // case3: empty input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); } From 0c2045904535f837cc5b2d01170adc68b6bb820c Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 8 Jul 2024 15:59:06 +0900 Subject: [PATCH 07/17] refactor(lanelet2_map_preprocessor): apply static analysis (#7870) * refactor based on linter Signed-off-by: a-maumau * style(pre-commit): autofix * apply suggestion Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/fix_lane_change_tags.cpp | 33 +++---- .../src/fix_z_value_by_pcd.cpp | 43 ++++---- .../src/merge_close_lines.cpp | 98 ++++++++----------- .../src/merge_close_points.cpp | 23 ++--- .../src/remove_unreferenced_geometry.cpp | 27 ++--- .../src/transform_maps.cpp | 27 ++--- 6 files changed, 116 insertions(+), 135 deletions(-) diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index cbd279af2b46f..571c2e91c53a5 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -27,10 +27,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -48,32 +45,32 @@ bool loadLaneletMap( return true; } -lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Lanelets convert_to_vector(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Lanelets lanelets; - for (lanelet::Lanelet lanelet : lanelet_map_ptr->laneletLayer) { - lanelets.push_back(lanelet); - } + std::copy( + lanelet_map_ptr->laneletLayer.begin(), lanelet_map_ptr->laneletLayer.end(), + std::back_inserter(lanelets)); return lanelets; } -void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr) +void fix_tags(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lanelets = convertToVector(lanelet_map_ptr); - lanelet::traffic_rules::TrafficRulesPtr trafficRules = + auto lanelets = convert_to_vector(lanelet_map_ptr); + lanelet::traffic_rules::TrafficRulesPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); - lanelet::routing::RoutingGraphUPtr routingGraph = - lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *trafficRules); + lanelet::routing::RoutingGraphUPtr routing_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *traffic_rules); for (auto & llt : lanelets) { - if (!routingGraph->conflicting(llt).empty()) { + if (!routing_graph->conflicting(llt).empty()) { continue; } llt.attributes().erase("turn_direction"); - if (!!routingGraph->adjacentRight(llt)) { + if (!!routing_graph->adjacentRight(llt)) { llt.rightBound().attributes()["lane_change"] = "yes"; } - if (!!routingGraph->adjacentLeft(llt)) { + if (!!routing_graph->adjacentLeft(llt)) { llt.leftBound().attributes()["lane_change"] = "yes"; } } @@ -91,11 +88,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - fixTags(llt_map_ptr); + fix_tags(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp index 14a33b01beee0..af565208c5f71 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,16 +57,16 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -double getMinHeightAroundPoint( +double get_min_height_around_point( const pcl::PointCloud::Ptr & pcd_map_ptr, const pcl::KdTreeFLANN & kdtree, const pcl::PointXYZ & search_pt, const double search_radius3d, const double search_radius2d) { - std::vector pointIdxRadiusSearch; - std::vector pointRadiusSquaredDistance; + std::vector point_idx_radius_search; + std::vector point_radius_squared_distance; if ( kdtree.radiusSearch( - search_pt, search_radius3d, pointIdxRadiusSearch, pointRadiusSquaredDistance) <= 0) { + search_pt, search_radius3d, point_idx_radius_search, point_radius_squared_distance) <= 0) { std::cout << "no points found within 3d radius " << search_radius3d << std::endl; return search_pt.z; } @@ -73,8 +74,7 @@ double getMinHeightAroundPoint( double min_height = std::numeric_limits::max(); bool found = false; - for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); i++) { - std::size_t pt_idx = pointIdxRadiusSearch.at(i); + for (auto pt_idx : point_idx_radius_search) { const auto pt = pcd_map_ptr->points.at(pt_idx); if (pt.z > min_height) { continue; @@ -91,8 +91,9 @@ double getMinHeightAroundPoint( return min_height; } -void adjustHeight( - const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) +void adjust_height( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr) { std::unordered_set done; double search_radius2d = 0.5; @@ -107,11 +108,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -121,11 +122,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -148,14 +149,14 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - adjustHeight(pcd_map_ptr, llt_map_ptr); + adjust_height(pcd_map_ptr, llt_map_ptr); lanelet::write(llt_output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp index e0d071ea11dc1..0e40d04a2cec3 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -26,10 +26,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -47,21 +44,17 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -lanelet::ConstPoint3d get3DPointFrom2DArcLength( +lanelet::ConstPoint3d get3d_point_from2d_arc_length( const lanelet::ConstLineString3d & line, const double s) { double accumulated_distance2d = 0; @@ -71,27 +64,23 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength( auto prev_pt = line.front(); for (size_t i = 1; i < line.size(); i++) { const auto & pt = line[i]; - double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt)); + double distance2d = + lanelet::geometry::distance2d(lanelet::utils::to2D(prev_pt), lanelet::utils::to2D(pt)); if (accumulated_distance2d + distance2d >= s) { double ratio = (s - accumulated_distance2d) / distance2d; auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; std::cout << interpolated_pt << std::endl; - return lanelet::ConstPoint3d( - lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + return lanelet::ConstPoint3d{ + lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()}; } accumulated_distance2d += distance2d; prev_pt = pt; } RCLCPP_ERROR(rclcpp::get_logger("merge_close_lines"), "interpolation failed"); - return lanelet::ConstPoint3d(); -} - -double getLineLength(const lanelet::ConstLineString3d & line) -{ - return boost::geometry::length(line.basicLineString()); + return {}; } -bool areLinesSame( +bool are_lines_same( const lanelet::ConstLineString3d & line1, const lanelet::ConstLineString3d & line2) { bool same_ends = false; @@ -105,66 +94,63 @@ bool areLinesSame( return false; } - double sum_distance = 0; - for (const auto & pt : line1) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line2); - } - for (const auto & pt : line2) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line1); - } + double sum_distance = + std::accumulate(line1.begin(), line1.end(), 0.0, [&line2](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line2); + }); + sum_distance += + std::accumulate(line2.begin(), line2.end(), 0.0, [&line1](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line1); + }); - double avg_distance = sum_distance / (line1.size() + line2.size()); + double avg_distance = sum_distance / static_cast(line1.size() + line2.size()); std::cout << line1 << " " << line2 << " " << avg_distance << std::endl; - if (avg_distance < 1.0) { - return true; - } else { - return false; - } + return avg_distance < 1.0; } -lanelet::BasicPoint3d getClosestPointOnLine( +lanelet::BasicPoint3d get_closest_point_on_line( const lanelet::BasicPoint3d & search_point, const lanelet::ConstLineString3d & line) { - auto arc_coordinate = lanelet::geometry::toArcCoordinates(to2D(line), to2D(search_point)); + auto arc_coordinate = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(line), lanelet::utils::to2D(search_point)); std::cout << arc_coordinate.length << " " << arc_coordinate.distance << std::endl; - return get3DPointFrom2DArcLength(line, arc_coordinate.length).basicPoint(); + return get3d_point_from2d_arc_length(line, arc_coordinate.length).basicPoint(); } -lanelet::LineString3d mergeTwoLines( +lanelet::LineString3d merge_two_lines( const lanelet::LineString3d & line1, const lanelet::ConstLineString3d & line2) { lanelet::Points3d new_points; for (const auto & p1 : line1) { - lanelet::BasicPoint3d p1_basic_point = p1.basicPoint(); - lanelet::BasicPoint3d p2_basic_point = getClosestPointOnLine(p1, line2); + const lanelet::BasicPoint3d & p1_basic_point = p1.basicPoint(); + lanelet::BasicPoint3d p2_basic_point = get_closest_point_on_line(p1, line2); lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point) / 2; lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); new_points.push_back(new_point); } - return lanelet::LineString3d(lanelet::utils::getId(), new_points); + return lanelet::LineString3d{lanelet::utils::getId(), new_points}; } -void copyData(lanelet::LineString3d & dst, lanelet::LineString3d & src) +void copy_data(lanelet::LineString3d & dst, const lanelet::LineString3d & src) { - lanelet::Points3d points; dst.clear(); - for (lanelet::Point3d & pt : src) { - dst.push_back(pt); + for (const lanelet::ConstPoint3d & pt : src) { + dst.push_back(static_cast(pt)); } } -void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_lines(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lines = convertLineLayerToLineStrings(lanelet_map_ptr); + auto lines = convert_line_layer_to_line_strings(lanelet_map_ptr); for (size_t i = 0; i < lines.size(); i++) { auto line_i = lines.at(i); for (size_t j = 0; j < i; j++) { auto line_j = lines.at(j); - if (areLinesSame(line_i, line_j)) { - auto merged_line = mergeTwoLines(line_i, line_j); - copyData(line_i, merged_line); - copyData(line_j, merged_line); + if (are_lines_same(line_i, line_j)) { + auto merged_line = merge_two_lines(line_i, line_j); + copy_data(line_i, merged_line); + copy_data(line_j, merged_line); line_i.setId(line_j.id()); std::cout << line_j << " " << line_i << std::endl; // lanelet_map_ptr->add(merged_line); @@ -189,11 +175,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergeLines(llt_map_ptr); + merge_lines(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp index e18a366003e10..3cbbffc702019 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,17 +43,12 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } @@ -72,9 +67,9 @@ lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_ma // return lanelet::LineString3d(lanelet::utils::getId(), new_points); // } -void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { - const auto & points = convertPointsLayerToPoints(lanelet_map_ptr); + const auto & points = convert_points_layer_to_points(lanelet_map_ptr); for (size_t i = 0; i < points.size(); i++) { auto point_i = points.at(i); @@ -109,11 +104,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergePoints(llt_map_ptr); + merge_points(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp index 633dae4e20c1d..3ab10551e36f9 100644 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,28 +43,29 @@ bool loadLaneletMap( return true; } -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -void removeUnreferencedGeometry(lanelet::LaneletMapPtr & lanelet_map_ptr) +void remove_unreferenced_geometry(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); - for (auto llt : lanelet_map_ptr->laneletLayer) { + for (const auto & llt : lanelet_map_ptr->laneletLayer) { new_map->add(llt); } lanelet_map_ptr = new_map; @@ -82,10 +83,10 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - removeUnreferencedGeometry(llt_map_ptr); + remove_unreferenced_geometry(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp index d29df716b11e7..d55e03c7faee5 100644 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,9 +57,9 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -void transformMaps( - pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr, - const Eigen::Affine3d affine) +void transform_maps( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const Eigen::Affine3d & affine) { { for (lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { @@ -74,14 +75,14 @@ void transformMaps( for (auto & pt : pcd_map_ptr->points) { Eigen::Vector3d eigen_pt(pt.x, pt.y, pt.z); auto transformed_pt = affine * eigen_pt; - pt.x = transformed_pt.x(); - pt.y = transformed_pt.y(); - pt.z = transformed_pt.z(); + pt.x = static_cast(transformed_pt.x()); + pt.y = static_cast(transformed_pt.y()); + pt.z = static_cast(transformed_pt.z()); } } } -Eigen::Affine3d createAffineMatrixFromXYZRPY( +Eigen::Affine3d create_affine_matrix_from_xyzrpy( const double x, const double y, const double z, const double roll, const double pitch, const double yaw) { @@ -127,19 +128,19 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - Eigen::Affine3d affine = createAffineMatrixFromXYZRPY(x, y, z, roll, pitch, yaw); + Eigen::Affine3d affine = create_affine_matrix_from_xyzrpy(x, y, z, roll, pitch, yaw); const auto mgrs_grid = node->declare_parameter("mgrs_grid", projector.getProjectedMGRSGrid()); std::cout << "using mgrs grid: " << mgrs_grid << std::endl; - transformMaps(pcd_map_ptr, llt_map_ptr, affine); + transform_maps(pcd_map_ptr, llt_map_ptr, affine); lanelet::write(llt_output_path, *llt_map_ptr, projector); pcl::io::savePCDFileBinary(pcd_output_path, *pcd_map_ptr); From deef7c4a484e62828e9e8ee2a3092db95fed9640 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 8 Jul 2024 16:22:41 +0900 Subject: [PATCH 08/17] fix(pointcloud_preprocessor): fix the bug where the geometry message was not saved correctly. (#7886) * fix: fix bug that geometry message didn't save correctly Signed-off-by: vividf * chore: change some functions from public to protected Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: Kenzo Lobos Tsunekawa --- .../distortion_corrector.hpp | 36 +++++++++---------- .../distortion_corrector.cpp | 19 +++++----- 2 files changed, 25 insertions(+), 30 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 6f372f0e74646..8a7b7c90113da 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -61,7 +61,8 @@ class DistortionCorrectorBase template class DistortionCorrector : public DistortionCorrectorBase { -public: +protected: + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_; bool pointcloud_transform_needed_{false}; bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; @@ -72,28 +73,12 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque twist_queue_; std::deque angular_velocity_queue_; - explicit DistortionCorrector(rclcpp::Node * node) - : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) - { - } - void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; - - void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; - void getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - void enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - - bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); + void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame); + void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); void getTwistAndIMUIterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu); - void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); void undistortPoint( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, @@ -105,6 +90,19 @@ class DistortionCorrector : public DistortionCorrectorBase static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; + +public: + explicit DistortionCorrector(rclcpp::Node * node) + : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) + { + } + void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; + + void processIMUMessage( + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; + bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); }; class DistortionCorrector2D : public DistortionCorrector diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 81c29a9f6202a..8c8752c7e278e 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -47,21 +47,20 @@ template void DistortionCorrector::processIMUMessage( const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = - std::make_shared(); - getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); - enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr); + getIMUTransformation(base_frame, imu_msg->header.frame_id); + enqueueIMU(imu_msg); } template void DistortionCorrector::getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) + const std::string & base_frame, const std::string & imu_frame) { if (imu_transform_exists_) { return; } + geometry_imu_to_base_link_ptr_ = std::make_shared(); + tf2::Transform tf2_imu_to_base_link; if (base_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); @@ -83,20 +82,18 @@ void DistortionCorrector::getIMUTransformation( } } - geometry_imu_to_base_link_ptr->transform.rotation = + geometry_imu_to_base_link_ptr_->transform.rotation = tf2::toMsg(tf2_imu_to_base_link.getRotation()); } template -void DistortionCorrector::enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) +void DistortionCorrector::enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { geometry_msgs::msg::Vector3Stamped angular_velocity; angular_velocity.vector = imu_msg->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr); + tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr_); transformed_angular_velocity.header = imu_msg->header; angular_velocity_queue_.push_back(transformed_angular_velocity); From 094f2e2a7ba4db09b591f526ef6d60df4937ff95 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 16:30:07 +0900 Subject: [PATCH 09/17] refactor(multi_object_tracker)!: fix namespace and directory structure (#7863) * refactor: update include paths for debugger and processor modules Signed-off-by: Taekjin LEE * refactor: update include paths for debugger and processor modules Signed-off-by: Taekjin LEE * refactor: move include files into 'autoware' Signed-off-by: Taekjin LEE * refactor: set namespace Signed-off-by: Taekjin LEE * refactor: refine logics to pass cppcheck Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 44 ++++++++++--------- .../association/association.hpp} | 14 +++--- .../association}/solver/gnn_solver.hpp | 12 ++--- .../solver/gnn_solver_interface.hpp | 10 +++-- .../association/solver/mu_ssp.hpp} | 12 +++-- .../association/solver/ssp.hpp} | 12 +++-- .../tracker/model/bicycle_tracker.hpp | 17 ++++--- .../tracker/model/big_vehicle_tracker.hpp | 17 ++++--- .../model/multiple_vehicle_tracker.hpp | 17 ++++--- .../tracker/model/normal_vehicle_tracker.hpp | 17 ++++--- .../tracker/model/pass_through_tracker.hpp | 11 +++-- .../model/pedestrian_and_bicycle_tracker.hpp | 17 ++++--- .../tracker/model/pedestrian_tracker.hpp | 19 ++++---- .../tracker/model/tracker_base.hpp | 13 ++++-- .../tracker/model/unknown_tracker.hpp | 15 ++++--- .../motion_model/bicycle_motion_model.hpp | 13 ++++-- .../motion_model/ctrv_motion_model.hpp | 14 +++--- .../tracker/motion_model/cv_motion_model.hpp | 13 ++++-- .../motion_model/motion_model_base.hpp | 11 +++-- .../tracker/object_model/object_model.hpp | 10 +++-- .../multi_object_tracker/tracker/tracker.hpp | 6 +-- .../multi_object_tracker/utils/utils.hpp | 6 +-- .../association/association.cpp} | 11 +++-- .../mu_successive_shortest_path/mu_ssp.cpp} | 6 ++- .../successive_shortest_path/ssp.cpp} | 6 ++- .../tracker/model/bicycle_tracker.cpp | 9 +++- .../tracker/model/big_vehicle_tracker.cpp | 8 +++- .../model/multiple_vehicle_tracker.cpp | 7 ++- .../tracker/model/normal_vehicle_tracker.cpp | 9 +++- .../tracker/model/pass_through_tracker.cpp | 9 +++- .../model/pedestrian_and_bicycle_tracker.cpp | 7 ++- .../tracker/model/pedestrian_tracker.cpp | 9 +++- .../tracker/model/tracker_base.cpp | 9 +++- .../tracker/model/unknown_tracker.cpp | 9 +++- .../motion_model/bicycle_motion_model.cpp | 11 +++-- .../motion_model/ctrv_motion_model.cpp | 10 +++-- .../tracker/motion_model/cv_motion_model.cpp | 10 +++-- .../motion_model/motion_model_base.cpp | 7 ++- .../src/debugger/debug_object.cpp | 9 +++- .../debugger/debug_object.hpp | 13 ++++-- .../src/debugger/debugger.cpp | 6 ++- .../debugger/debugger.hpp | 13 ++++-- ...core.cpp => multi_object_tracker_node.cpp} | 19 ++++---- .../multi_object_tracker_node.hpp} | 22 +++++----- .../src/processor/input_manager.cpp | 32 ++++++++------ .../processor/input_manager.hpp | 10 ++--- .../src/processor/processor.cpp | 9 +++- .../processor/processor.hpp | 12 +++-- 48 files changed, 398 insertions(+), 204 deletions(-) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/data_association.hpp => autoware/multi_object_tracker/association/association.hpp} (81%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association => autoware/multi_object_tracker/association}/solver/gnn_solver.hpp (55%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association => autoware/multi_object_tracker/association}/solver/gnn_solver_interface.hpp (73%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp => autoware/multi_object_tracker/association/solver/mu_ssp.hpp} (71%) rename perception/multi_object_tracker/include/{multi_object_tracker/data_association/solver/successive_shortest_path.hpp => autoware/multi_object_tracker/association/solver/ssp.hpp} (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/bicycle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pass_through_tracker.hpp (81%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp (71%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/pedestrian_tracker.hpp (78%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/tracker_base.hpp (91%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/model/unknown_tracker.hpp (80%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp (87%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp (86%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp (84%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/motion_model/motion_model_base.hpp (85%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/object_model/object_model.hpp (96%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/tracker/tracker.hpp (84%) rename perception/multi_object_tracker/include/{ => autoware}/multi_object_tracker/utils/utils.hpp (98%) rename perception/multi_object_tracker/{src/data_association/data_association.cpp => lib/association/association.cpp} (96%) rename perception/multi_object_tracker/{src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp => lib/association/mu_successive_shortest_path/mu_ssp.cpp} (88%) rename perception/multi_object_tracker/{src/data_association/successive_shortest_path/successive_shortest_path.cpp => lib/association/successive_shortest_path/ssp.cpp} (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/bicycle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/big_vehicle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/multiple_vehicle_tracker.cpp (93%) rename perception/multi_object_tracker/{src => lib}/tracker/model/normal_vehicle_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pass_through_tracker.cpp (95%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pedestrian_and_bicycle_tracker.cpp (93%) rename perception/multi_object_tracker/{src => lib}/tracker/model/pedestrian_tracker.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/model/tracker_base.cpp (96%) rename perception/multi_object_tracker/{src => lib}/tracker/model/unknown_tracker.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/bicycle_motion_model.cpp (98%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/ctrv_motion_model.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/cv_motion_model.cpp (97%) rename perception/multi_object_tracker/{src => lib}/tracker/motion_model/motion_model_base.cpp (93%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/debugger/debug_object.hpp (91%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/debugger/debugger.hpp (94%) rename perception/multi_object_tracker/src/{multi_object_tracker_core.cpp => multi_object_tracker_node.cpp} (96%) rename perception/multi_object_tracker/{include/multi_object_tracker/multi_object_tracker_core.hpp => src/multi_object_tracker_node.hpp} (82%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/processor/input_manager.hpp (95%) rename perception/multi_object_tracker/{include/multi_object_tracker => src}/processor/processor.hpp (91%) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 4f9268583dd34..370e5bb0b9161 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -20,32 +20,34 @@ include_directories( ) # Generate exe file -set(MULTI_OBJECT_TRACKER_SRC - src/multi_object_tracker_core.cpp +set(${PROJECT_NAME}_src + src/multi_object_tracker_node.cpp src/debugger/debugger.cpp src/debugger/debug_object.cpp src/processor/processor.cpp src/processor/input_manager.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp - src/tracker/motion_model/motion_model_base.cpp - src/tracker/motion_model/bicycle_motion_model.cpp +) +set(${PROJECT_NAME}_lib + lib/association/association.cpp + lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/tracker/motion_model/motion_model_base.cpp + lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv - src/tracker/motion_model/ctrv_motion_model.cpp - src/tracker/motion_model/cv_motion_model.cpp - src/tracker/model/tracker_base.cpp - src/tracker/model/big_vehicle_tracker.cpp - src/tracker/model/normal_vehicle_tracker.cpp - src/tracker/model/multiple_vehicle_tracker.cpp - src/tracker/model/bicycle_tracker.cpp - src/tracker/model/pedestrian_tracker.cpp - src/tracker/model/pedestrian_and_bicycle_tracker.cpp - src/tracker/model/unknown_tracker.cpp - src/tracker/model/pass_through_tracker.cpp + lib/tracker/motion_model/ctrv_motion_model.cpp + lib/tracker/motion_model/cv_motion_model.cpp + lib/tracker/model/tracker_base.cpp + lib/tracker/model/big_vehicle_tracker.cpp + lib/tracker/model/normal_vehicle_tracker.cpp + lib/tracker/model/multiple_vehicle_tracker.cpp + lib/tracker/model/bicycle_tracker.cpp + lib/tracker/model/pedestrian_tracker.cpp + lib/tracker/model/pedestrian_and_bicycle_tracker.cpp + lib/tracker/model/unknown_tracker.cpp + lib/tracker/model/pass_through_tracker.cpp ) - ament_auto_add_library(${PROJECT_NAME} SHARED - ${MULTI_OBJECT_TRACKER_SRC} + ${${PROJECT_NAME}_src} + ${${PROJECT_NAME}_lib} ) target_link_libraries(${PROJECT_NAME} @@ -54,8 +56,8 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "multi_object_tracker::MultiObjectTracker" - EXECUTABLE ${PROJECT_NAME}_node + PLUGIN "autoware::multi_object_tracker::MultiObjectTracker" + EXECUTABLE multi_object_tracker_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index c30423db02c26..2c12341d0aa67 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -16,13 +16,13 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include #include @@ -34,6 +34,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class DataAssociation { private: @@ -61,4 +63,6 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp similarity index 55% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp index 631fd73f8583d..3b72b800e86bc 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 73% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp index 1fb508bcf12b2..88f9181d96d9b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class GnnSolverInterface @@ -30,6 +32,8 @@ class GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) = 0; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp index a4d9d727e6cc5..024670c3389c6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class MuSSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class MuSSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp index 1c4b15cd68b59..0f987fc49ee7c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class SSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class SSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 63ad496b70ed9..5374e28d5f9cf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BicycleTracker : public Tracker { @@ -67,4 +70,6 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 7112e6a08ade1..c3f824aff35b4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BigVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class BigVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 75aec0b06d6b8..a0a6bd7781761 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -16,16 +16,19 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include +namespace autoware::multi_object_tracker +{ + class MultipleVehicleTracker : public Tracker { private: @@ -48,4 +51,6 @@ class MultipleVehicleTracker : public Tracker virtual ~MultipleVehicleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 1165cecab258b..f7edebfc31378 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class NormalVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index cd661dab52c6e..e7f0a5fd699e1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yutaka Shimizu // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #include "kalman_filter/kalman_filter.hpp" #include "tracker_base.hpp" +namespace autoware::multi_object_tracker +{ + class PassThroughTracker : public Tracker { private: @@ -44,4 +47,6 @@ class PassThroughTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 3c3ac038b085e..f593280c2e183 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +namespace autoware::multi_object_tracker +{ class PedestrianAndBicycleTracker : public Tracker { @@ -46,4 +49,6 @@ class PedestrianAndBicycleTracker : public Tracker virtual ~PedestrianAndBicycleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c5be57f656eb5..1e0f778a69137 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -16,15 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ class PedestrianTracker : public Tracker { @@ -49,7 +50,7 @@ class PedestrianTracker : public Tracker }; BoundingBox bounding_box_; Cylinder cylinder_; - + // cspell: ignore CTRV CTRVMotionModel motion_model_; using IDX = CTRVMotionModel::IDX; @@ -75,4 +76,6 @@ class PedestrianTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 44b3884c392e6..bc52e49f763fd 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -33,6 +33,9 @@ #include +namespace autoware::multi_object_tracker +{ + class Tracker { private: @@ -114,4 +117,6 @@ class Tracker virtual bool predict(const rclcpp::Time & time) = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp similarity index 80% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index ca8ecba160bd8..4bc03f439ffc2 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" + +namespace autoware::multi_object_tracker +{ class UnknownTracker : public Tracker { @@ -62,4 +65,6 @@ class UnknownTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp similarity index 87% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index f6ce2842388c6..6e20d31aad168 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class BicycleMotionModel : public MotionModel { private: @@ -107,4 +110,6 @@ class BicycleMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp similarity index 86% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 657048079c46c..3cca786d9f65b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,8 +32,10 @@ #endif #include -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ +// cspell: ignore CTRV class CTRVMotionModel : public MotionModel { private: @@ -92,4 +94,6 @@ class CTRVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 421e413d7aab7..ad3061285a80c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class CVMotionModel : public MotionModel { private: @@ -85,4 +88,6 @@ class CVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp similarity index 85% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index e740612e96d4f..130053fafd2ed 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #include "kalman_filter/kalman_filter.hpp" @@ -31,6 +31,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class MotionModel { private: @@ -65,4 +68,6 @@ class MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp similarity index 96% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp index 6a771344bb36b..b49464109eec8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ #include @@ -46,6 +46,9 @@ constexpr T kmph2mps(const T kmph) } // namespace +namespace autoware::multi_object_tracker +{ + namespace object_model { @@ -301,5 +304,6 @@ static const ObjectModel bicycle(ObjectModelType::Bicycle); static const ObjectModel pedestrian(ObjectModelType::Pedestrian); } // namespace object_model +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp index a8bc58e254fc2..ea1e60d4c7e17 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #include "model/bicycle_tracker.hpp" #include "model/big_vehicle_tracker.hpp" @@ -29,4 +29,4 @@ #include "model/tracker_base.hpp" #include "model/unknown_tracker.hpp" -#endif // MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp similarity index 98% rename from perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index 0bda7870ae2b9..833f768e171f4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #include #include @@ -276,4 +276,4 @@ inline bool getMeasurementYaw( } // namespace utils -#endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/lib/association/association.cpp similarity index 96% rename from perception/multi_object_tracker/src/data_association/data_association.cpp rename to perception/multi_object_tracker/lib/association/association.cpp index f7b40cd178528..ac31cad4cc13c 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/lib/association/association.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/data_association.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -68,6 +68,9 @@ double getFormedYawAngle( } } // namespace +namespace autoware::multi_object_tracker +{ + DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, @@ -224,3 +227,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp similarity index 88% rename from perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp rename to perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp index 18779a7fffbdf..9ae6e300b2b9b 100644 --- a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" #include @@ -24,6 +24,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { void MuSSP::maximizeLinearAssignment( @@ -39,3 +41,5 @@ void MuSSP::maximizeLinearAssignment( solve_muSSP(cost, direct_assignment, reverse_assignment); } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp similarity index 98% rename from perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp rename to perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp index 0b52c09cb54b8..d20992c7d183b 100644 --- a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" #include #include @@ -22,6 +22,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { struct ResidualEdge @@ -368,3 +370,5 @@ void SSP::maximizeLinearAssignment( #endif } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index a438c830596d7..ba53ccec1ad43 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( @@ -334,3 +337,5 @@ bool BicycleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index 90d33e65b46bd..0593b7fc9dc12 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,8 @@ #include #endif +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( @@ -398,3 +400,5 @@ bool BigVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 67445e5b0daa2..a6e5021e4fe7d 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool MultipleVehicleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index 67282893083c1..ef345692b32ca 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( @@ -399,3 +402,5 @@ bool NormalVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp similarity index 95% rename from perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 17be415480360..c872944fab3d7 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -16,10 +16,10 @@ // Author: v1.0 Yutaka Shimizu // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -36,6 +36,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -117,3 +120,5 @@ bool PassThroughTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 00609934990a3..8c665e6078a2b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool PedestrianAndBicycleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index d9412c3b5739c..ee50c2e5449ed 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( @@ -338,3 +341,5 @@ bool PedestrianTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp similarity index 96% rename from perception/multi_object_tracker/src/tracker/model/tracker_base.cpp rename to perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp index f27bad172a4b9..daa8db5db91e6 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -14,9 +14,9 @@ // // -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -40,6 +40,9 @@ float decayProbability(const float & prior, const float & delta_time) } } // namespace +namespace autoware::multi_object_tracker +{ + Tracker::Tracker( const rclcpp::Time & time, const std::vector & classification, @@ -202,3 +205,5 @@ geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 6905d7c3b8ced..0648007e0b807 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -13,13 +13,13 @@ // limitations under the License. #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -36,6 +36,9 @@ #endif #include "object_recognition_utils/object_recognition_utils.hpp" +namespace autoware::multi_object_tracker +{ + UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -234,3 +237,5 @@ bool UnknownTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 466108a2bef0e..2e325d18579a6 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -17,17 +17,20 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ + // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity @@ -487,3 +490,5 @@ bool BicycleMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index b19af1d26d162..b1c5a36f9ad5b 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -17,17 +17,19 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -386,3 +388,5 @@ bool CTRVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 231ba73796ed9..2e2ba660a6e0d 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -17,19 +17,21 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CV // Constant Velocity (CV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -306,3 +308,5 @@ bool CVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp index 4d51021de634b..ffbb452815fa7 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +namespace autoware::multi_object_tracker +{ MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) { @@ -90,3 +93,5 @@ bool MotionModel::getPredictedState( tmp_ekf_for_no_update.getP(P); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index fe6d3d5c75564..58b7dfc5cef48 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include "autoware_perception_msgs/msg/tracked_object.hpp" @@ -48,6 +48,9 @@ int32_t uuidToInt(const boost::uuids::uuid & uuid) } } // namespace +namespace autoware::multi_object_tracker +{ + TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) { // set frame id @@ -79,7 +82,7 @@ void TrackerObjectDebugger::collect( const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { - if (!is_initialized_) is_initialized_ = true; + is_initialized_ = true; message_time_ = message_time; @@ -391,3 +394,5 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma return; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/src/debugger/debug_object.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp rename to perception/multi_object_tracker/src/debugger/debug_object.hpp index 08c9eb975d8e6..ed4c556959861 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#ifndef DEBUGGER__DEBUG_OBJECT_HPP_ +#define DEBUGGER__DEBUG_OBJECT_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -37,6 +37,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + struct ObjectData { rclcpp::Time time; @@ -99,4 +102,6 @@ class TrackerObjectDebugger void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index e692ae76468e4..3e47bbe9bed8d 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debugger.hpp" +#include "debugger.hpp" #include +namespace autoware::multi_object_tracker +{ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) : node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) { @@ -207,3 +209,5 @@ void TrackerDebugger::publishObjectsMarkers() // reset object data object_debugger_.reset(); } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/src/debugger/debugger.hpp similarity index 94% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp rename to perception/multi_object_tracker/src/debugger/debugger.hpp index a1c516147b220..77618e1882be7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/src/debugger/debugger.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#ifndef DEBUGGER__DEBUGGER_HPP_ +#define DEBUGGER__DEBUGGER_HPP_ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include #include @@ -33,6 +33,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + /** * @brief Debugger class for multi object tracker * @details This class is used to publish debug information of multi object tracker @@ -104,4 +107,6 @@ class TrackerDebugger void publishObjectsMarkers(); }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp similarity index 96% rename from perception/multi_object_tracker/src/multi_object_tracker_core.cpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.cpp index 97c3d93d191b2..d3e1c9686b69b 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -15,13 +15,12 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/multi_object_tracker_core.hpp" +#include "multi_object_tracker_node.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include -#include #include @@ -66,7 +65,7 @@ boost::optional getTransformAnonymous( } // namespace -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -192,7 +191,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - data_association_ = std::make_unique( + association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); } @@ -295,9 +294,9 @@ void MultiObjectTracker::runProcess( const auto & list_tracker = processor_->getListTracker(); const auto & detected_objects = transformed_objects; // global nearest neighbor - Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( + Eigen::MatrixXd score_matrix = association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement - data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + association_->assign(score_matrix, direct_assignment, reverse_assignment); // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( @@ -358,6 +357,8 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->publishObjectsMarkers(); } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker + +#include -RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::multi_object_tracker::MultiObjectTracker) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp similarity index 82% rename from perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.hpp index aff3cbd00eabe..db5eaaa88ca8c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp @@ -16,14 +16,14 @@ // Author: v1.0 Yukihiro Saito /// -#ifndef MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ -#define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#ifndef MULTI_OBJECT_TRACKER_NODE_HPP_ +#define MULTI_OBJECT_TRACKER_NODE_HPP_ -#include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger/debugger.hpp" -#include "multi_object_tracker/processor/input_manager.hpp" -#include "multi_object_tracker/processor/processor.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "debugger/debugger.hpp" +#include "processor/input_manager.hpp" +#include "processor/processor.hpp" #include @@ -52,7 +52,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObject = autoware_perception_msgs::msg::DetectedObject; @@ -82,7 +82,7 @@ class MultiObjectTracker : public rclcpp::Node // internal states std::string world_frame_id_; // tracking frame - std::unique_ptr data_association_; + std::unique_ptr association_; std::unique_ptr processor_; // input manager @@ -103,6 +103,6 @@ class MultiObjectTracker : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#endif // MULTI_OBJECT_TRACKER_NODE_HPP_ diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index bee76e6c05940..5172cc5062450 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/input_manager.hpp" +#include "input_manager.hpp" #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { /////////////////////////// /////// InputStream /////// @@ -62,7 +62,7 @@ void InputStream::onMessage( { const DetectedObjects objects = *msg; objects_que_.push_back(objects); - if (objects_que_.size() > que_size_) { + while (objects_que_.size() > que_size_) { objects_que_.pop_front(); } @@ -160,21 +160,27 @@ void InputStream::getObjectsOlderThan( return; } - for (const auto & object : objects_que_) { - const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); - - // remove objects older than the specified duration + for (const auto & objects : objects_que_) { + const rclcpp::Time object_time = rclcpp::Time(objects.header.stamp); + // ignore objects older than the specified duration if (object_time < object_oldest_time) { - objects_que_.pop_front(); continue; } // Add the object if the object is older than the specified latest time - if (object_latest_time >= object_time) { - std::pair object_pair(index_, object); - objects_list.push_back(object_pair); - // remove the object from the queue + if (object_time <= object_latest_time) { + std::pair objects_pair(index_, objects); + objects_list.push_back(objects_pair); + } + } + + // remove objects older than 'object_latest_time' + while (!objects_que_.empty()) { + const rclcpp::Time object_time = rclcpp::Time(objects_que_.front().header.stamp); + if (object_time < object_latest_time) { objects_que_.pop_front(); + } else { + break; } } } @@ -368,4 +374,4 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li return is_any_object; } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/src/processor/input_manager.hpp similarity index 95% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp rename to perception/multi_object_tracker/src/processor/input_manager.hpp index 29b0c18ae766f..432a4105d815f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/src/processor/input_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#ifndef PROCESSOR__INPUT_MANAGER_HPP_ +#define PROCESSOR__INPUT_MANAGER_HPP_ #include "rclcpp/rclcpp.hpp" @@ -26,7 +26,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; using ObjectsList = std::vector>; @@ -155,6 +155,6 @@ class InputManager void optimizeTimings(); }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#endif // PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 50d1a021c5838..17871c89b5258 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/processor.hpp" +#include "processor.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware_perception_msgs/msg/tracked_objects.hpp" #include +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; TrackerProcessor::TrackerProcessor( @@ -248,3 +251,5 @@ void TrackerProcessor::getTentativeObjects( } } } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/src/processor/processor.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp rename to perception/multi_object_tracker/src/processor/processor.hpp index 6d0dbcd036e53..5eac113b3974c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/src/processor/processor.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +#ifndef PROCESSOR__PROCESSOR_HPP_ +#define PROCESSOR__PROCESSOR_HPP_ -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -29,6 +29,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class TrackerProcessor { public: @@ -78,4 +80,6 @@ class TrackerProcessor const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // PROCESSOR__PROCESSOR_HPP_ From f28ccd834427d2c7a07f3beef0fc3bc6edb2eb3b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 8 Jul 2024 16:34:56 +0900 Subject: [PATCH 10/17] refactor(detected_object_validation)!: fix namespace and directory structure (#7866) * refactor: update include paths for detected object filters Signed-off-by: Taekjin LEE * refactor: set proper namespace Signed-off-by: Taekjin LEE * refactor: move utils to be shared Signed-off-by: Taekjin LEE * refactor: remove unnecessary dependencies in detected_object_validation/package.xml Signed-off-by: Taekjin LEE * chore: Override functions in pcl_validator.hpp Signed-off-by: Taekjin LEE * style(pre-commit): autofix * refactor: pcl_validator to obstacle_pointcloud_validator, ogm_validator to occupancy_grid_map_validator Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../detected_object_validation/CMakeLists.txt | 28 +++++------- .../utils/utils.hpp | 10 +++-- .../{src => lib/utils}/utils.cpp | 6 ++- .../detected_object_validation/package.xml | 3 -- .../lanelet_filter.cpp} | 20 +++++---- .../lanelet_filter/lanelet_filter.hpp} | 27 ++++++----- .../obstacle_pointcloud}/debugger.hpp | 16 ++++--- .../obstacle_pointcloud_validator.cpp} | 14 +++--- .../obstacle_pointcloud_validator.hpp} | 45 ++++++++++++------- .../occupancy_grid_map_validator.cpp} | 21 +++++---- .../occupancy_grid_map_validator.hpp} | 19 +++++--- .../position_filter.cpp} | 12 +++-- .../position_filter/position_filter.hpp} | 23 +++++----- .../test_object_position_filter.cpp | 4 +- .../test/test_utils.cpp | 13 +++--- .../src/low_intensity_cluster_filter_node.hpp | 4 +- 16 files changed, 153 insertions(+), 112 deletions(-) rename perception/detected_object_validation/include/{ => autoware}/detected_object_validation/utils/utils.hpp (82%) rename perception/detected_object_validation/{src => lib/utils}/utils.cpp (87%) rename perception/detected_object_validation/src/{object_lanelet_filter.cpp => lanelet_filter/lanelet_filter.cpp} (95%) rename perception/detected_object_validation/{include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp => src/lanelet_filter/lanelet_filter.hpp} (81%) rename perception/detected_object_validation/{include/detected_object_validation/obstacle_pointcloud_based_validator => src/obstacle_pointcloud}/debugger.hpp (89%) rename perception/detected_object_validation/src/{obstacle_pointcloud_based_validator.cpp => obstacle_pointcloud/obstacle_pointcloud_validator.cpp} (97%) rename perception/detected_object_validation/{include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp => src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp} (79%) rename perception/detected_object_validation/src/{occupancy_grid_based_validator.cpp => occupancy_grid_map/occupancy_grid_map_validator.cpp} (92%) rename perception/detected_object_validation/{include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp => src/occupancy_grid_map/occupancy_grid_map_validator.hpp} (83%) rename perception/detected_object_validation/src/{object_position_filter.cpp => position_filter/position_filter.cpp} (91%) rename perception/detected_object_validation/{include/detected_object_validation/detected_object_filter/object_position_filter.hpp => src/position_filter/position_filter.hpp} (71%) diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index c416209d7d55d..6d93e7b426ad2 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -28,12 +28,8 @@ include_directories( ) # Generate occupancy grid based validator exe file -set(OCCUPANCY_GRID_BASED_VALIDATOR_SRC - src/occupancy_grid_based_validator.cpp -) - ament_auto_add_library(occupancy_grid_based_validator SHARED - ${OCCUPANCY_GRID_BASED_VALIDATOR_SRC} + src/occupancy_grid_map/occupancy_grid_map_validator.cpp ) target_link_libraries(occupancy_grid_based_validator @@ -42,12 +38,8 @@ target_link_libraries(occupancy_grid_based_validator ) # Generate obstacle pointcloud based validator exe file -set(OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC - src/obstacle_pointcloud_based_validator.cpp -) - ament_auto_add_library(obstacle_pointcloud_based_validator SHARED - ${OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC} + src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp ) target_link_libraries(obstacle_pointcloud_based_validator @@ -56,32 +48,32 @@ target_link_libraries(obstacle_pointcloud_based_validator ) ament_auto_add_library(object_lanelet_filter SHARED - src/object_lanelet_filter.cpp - src/utils.cpp + src/lanelet_filter/lanelet_filter.cpp + lib/utils/utils.cpp ) ament_auto_add_library(object_position_filter SHARED - src/object_position_filter.cpp - src/utils.cpp + src/position_filter/position_filter.cpp + lib/utils/utils.cpp ) rclcpp_components_register_node(obstacle_pointcloud_based_validator - PLUGIN "obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator" + PLUGIN "autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator" EXECUTABLE obstacle_pointcloud_based_validator_node ) rclcpp_components_register_node(object_lanelet_filter - PLUGIN "object_lanelet_filter::ObjectLaneletFilterNode" + PLUGIN "autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode" EXECUTABLE object_lanelet_filter_node ) rclcpp_components_register_node(object_position_filter - PLUGIN "object_position_filter::ObjectPositionFilterNode" + PLUGIN "autoware::detected_object_validation::position_filter::ObjectPositionFilterNode" EXECUTABLE object_position_filter_node ) rclcpp_components_register_node(occupancy_grid_based_validator - PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator" + PLUGIN "autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator" EXECUTABLE occupancy_grid_based_validator_node ) diff --git a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp similarity index 82% rename from perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp rename to perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp index c55e992213eb5..3ec119d2a48c5 100644 --- a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp +++ b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ -#define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#define AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #include #include + +namespace autoware::detected_object_validation +{ namespace utils { struct FilterTargetLabel @@ -48,5 +51,6 @@ inline bool hasBoundingBox(const autoware_perception_msgs::msg::DetectedObject & } } // namespace utils +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#endif // AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/utils.cpp b/perception/detected_object_validation/lib/utils/utils.cpp similarity index 87% rename from perception/detected_object_validation/src/utils.cpp rename to perception/detected_object_validation/lib/utils/utils.cpp index f6e2157cb885f..327f65a0c833e 100644 --- a/perception/detected_object_validation/src/utils.cpp +++ b/perception/detected_object_validation/lib/utils/utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" #include +namespace autoware::detected_object_validation +{ namespace utils { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -27,4 +29,6 @@ bool FilterTargetLabel::isTarget(const uint8_t label) const (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); } + } // namespace utils +} // namespace autoware::detected_object_validation diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index d8eb1257ba451..09440c9f6358d 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -22,17 +22,14 @@ autoware_perception_msgs autoware_test_utils autoware_universe_utils - geometry_msgs message_filters nav_msgs object_recognition_utils pcl_conversions rclcpp rclcpp_components - tf2 tf2_geometry_msgs tf2_ros - tier4_perception_msgs ament_lint_auto autoware_lint_common diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp similarity index 95% rename from perception/detected_object_validation/src/object_lanelet_filter.cpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index d1ba34f4a61b2..09440fedc1764 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" +#include "lanelet_filter.hpp" -#include -#include -#include -#include +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_lanelet2_extension/utility/message_conversion.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -25,7 +25,9 @@ #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_lanelet_filter_node", node_options), @@ -309,7 +311,9 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( return false; } -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_lanelet_filter::ObjectLaneletFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp similarity index 81% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 3ffd8d18c9a67..25d78a160c246 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#ifndef LANELET_FILTER__LANELET_FILTER_HPP_ +#define LANELET_FILTER__LANELET_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_lanelet2_extension/utility/utilities.hpp" -#include -#include -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -33,7 +33,9 @@ #include #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::MultiPoint2d; @@ -92,6 +94,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#endif // LANELET_FILTER__LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp similarity index 89% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp index d338580d95418..5886474d51b34 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp @@ -12,19 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#ifndef OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ +#define OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { class Debugger { @@ -106,6 +108,8 @@ class Debugger return pointcloud_xyz; } }; -} // namespace obstacle_pointcloud_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation + +#endif // OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp similarity index 97% rename from perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index fbb3f863be7c4..3fe527935c485 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "obstacle_pointcloud_validator.hpp" #include #include @@ -25,11 +27,12 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; @@ -369,8 +372,9 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( "debug/pipeline_latency_ms", pipeline_latency); } -} // namespace obstacle_pointcloud_based_validator +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation #include RCLCPP_COMPONENTS_REGISTER_NODE( - obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator) + autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp similarity index 79% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index a16bb63fd0c87..19b42ecf9055b 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -13,17 +13,17 @@ // limitations under the License. // NOLINTNEXTLINE(whitespace/line_length) -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#ifndef OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ // NOLINTNEXTLINE(whitespace/line_length) -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#define OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ -#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "debugger.hpp" -#include -#include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -42,7 +42,10 @@ #include #include #include -namespace obstacle_pointcloud_based_validator + +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { struct PointsNumThresholdParam @@ -91,14 +94,17 @@ class Validator2D : public Validator pcl::PointCloud::Ptr convertToXYZ( const pcl::PointCloud::Ptr & pointcloud_xy); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return convertToXYZ(neighbor_pointcloud_); } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr neighbor_pointcloud); @@ -112,13 +118,16 @@ class Validator3D : public Validator public: explicit Validator3D(PointsNumThresholdParam & points_num_threshold_param); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return neighbor_pointcloud_; } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr neighbor_pointcloud); @@ -154,7 +163,9 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); }; -} // namespace obstacle_pointcloud_based_validator + +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation // NOLINTNEXTLINE(whitespace/line_length) -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#endif // OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp similarity index 92% rename from perception/detected_object_validation/src/occupancy_grid_based_validator.cpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index b8682a6e56b3b..ce900f30f4255 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "occupancy_grid_map_validator.hpp" -#include -#include -#include +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "object_recognition_utils/object_classification.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -26,11 +28,12 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { using Shape = autoware_perception_msgs::msg::Shape; using Polygon2d = autoware::universe_utils::Polygon2d; @@ -173,7 +176,9 @@ void OccupancyGridBasedValidator::showDebugImage( cv::waitKey(2); } -} // namespace occupancy_grid_based_validator +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_based_validator::OccupancyGridBasedValidator) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp similarity index 83% rename from perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp index beb8faf5db1a3..52b6f79e5e20a 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ -#define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#ifndef OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ +#define OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ + +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include #include #include #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +31,9 @@ #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { class OccupancyGridBasedValidator : public rclcpp::Node { @@ -68,6 +71,8 @@ class OccupancyGridBasedValidator : public rclcpp::Node const nav_msgs::msg::OccupancyGrid & ros_occ_grid, const autoware_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid); }; -} // namespace occupancy_grid_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation + +#endif // OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/position_filter/position_filter.cpp similarity index 91% rename from perception/detected_object_validation/src/object_position_filter.cpp rename to perception/detected_object_validation/src/position_filter/position_filter.cpp index dccff8a6ccc67..ac0bab404c476 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "position_filter.hpp" -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_position_filter_node", node_options), @@ -78,7 +80,9 @@ bool ObjectPositionFilterNode::isObjectInBounds( position.y > lower_bound_y_ && position.y < upper_bound_y_; } -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_position_filter::ObjectPositionFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::position_filter::ObjectPositionFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/src/position_filter/position_filter.hpp similarity index 71% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp rename to perception/detected_object_validation/src/position_filter/position_filter.hpp index ea70d62fd952d..30a5d7fc9dfb1 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#ifndef POSITION_FILTER__POSITION_FILTER_HPP_ +#define POSITION_FILTER__POSITION_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +30,9 @@ #include #include -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { class ObjectPositionFilterNode : public rclcpp::Node @@ -57,6 +59,7 @@ class ObjectPositionFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#endif // POSITION_FILTER__POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp index 03cebf91418ce..5bd174d5a9e17 100644 --- a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp +++ b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "../../src/position_filter/position_filter.hpp" #include #include @@ -21,10 +21,10 @@ #include +using autoware::detected_object_validation::position_filter::ObjectPositionFilterNode; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; -using object_position_filter::ObjectPositionFilterNode; std::shared_ptr generateTestManager() { diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp index f7077dadb1ef2..7865f928f23a8 100644 --- a/perception/detected_object_validation/test/test_utils.cpp +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" #include +using autoware::detected_object_validation::utils::FilterTargetLabel; using AutowareLabel = autoware_perception_msgs::msg::ObjectClassification; -utils::FilterTargetLabel createFilterTargetAll() +FilterTargetLabel createFilterTargetAll() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = true; filter.CAR = true; filter.TRUCK = true; @@ -34,9 +35,9 @@ utils::FilterTargetLabel createFilterTargetAll() return filter; } -utils::FilterTargetLabel createFilterTargetVehicle() +FilterTargetLabel createFilterTargetVehicle() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = false; filter.CAR = true; filter.TRUCK = true; diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp index 8e21306218ab2..61e23860cd195 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp @@ -15,9 +15,9 @@ #ifndef LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ #define LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ +#include "autoware/detected_object_validation/utils/utils.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "detected_object_validation/utils/utils.hpp" #include #include @@ -64,7 +64,7 @@ class LowIntensityClusterFilter : public rclcpp::Node // Eigen::Vector4f max_boundary_transformed_; bool is_validation_range_transformed_ = false; const std::string base_link_frame_id_ = "base_link"; - utils::FilterTargetLabel filter_target_; + autoware::detected_object_validation::utils::FilterTargetLabel filter_target_; // debugger std::unique_ptr> stop_watch_ptr_{ From 1cbd11d789d12ba0bd771b2222d22476747d86a5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 8 Jul 2024 22:55:42 +0900 Subject: [PATCH 11/17] feat(autoware_universe_utils): add TimeKeeper to track function's processing time (#7754) Signed-off-by: Takayuki Murooka --- common/autoware_universe_utils/CMakeLists.txt | 25 ++ common/autoware_universe_utils/README.md | 249 ++++++++++++++++++ .../examples/example_scoped_time_track.cpp | 61 +++++ .../examples/example_time_keeper.cpp | 64 +++++ .../universe_utils/system/time_keeper.hpp | 202 ++++++++++++++ .../src/system/time_keeper.cpp | 175 ++++++++++++ .../test/src/system/test_time_keeper.cpp | 53 ++++ .../path_optimizer/common_structs.hpp | 52 ---- .../autoware/path_optimizer/mpt_optimizer.hpp | 5 +- .../include/autoware/path_optimizer/node.hpp | 7 +- .../state_equation_generator.hpp | 7 +- .../src/mpt_optimizer.cpp | 63 ++--- planning/autoware_path_optimizer/src/node.cpp | 71 +++-- .../src/state_equation_generator.cpp | 3 +- 14 files changed, 893 insertions(+), 144 deletions(-) create mode 100644 common/autoware_universe_utils/examples/example_scoped_time_track.cpp create mode 100644 common/autoware_universe_utils/examples/example_time_keeper.cpp create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp create mode 100644 common/autoware_universe_utils/src/system/time_keeper.cpp create mode 100644 common/autoware_universe_utils/test/src/system/test_time_keeper.cpp diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 9e86ddeb60692..2fdeef2119ab8 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,11 +1,15 @@ cmake_minimum_required(VERSION 3.14) project(autoware_universe_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) +find_package(fmt REQUIRED) + ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp @@ -16,6 +20,11 @@ ament_auto_add_library(autoware_universe_utils SHARED src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp src/system/backtrace.cpp + src/system/time_keeper.cpp +) + +target_link_libraries(autoware_universe_utils + fmt::fmt ) if(BUILD_TESTING) @@ -30,4 +39,20 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_universe_utils + ) + install(TARGETS ${example_name} + DESTINATION lib/${PROJECT_NAME} + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 22b9355b09635..2d24f84423299 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -7,3 +7,252 @@ This package contains many common functions used by other packages, so please re ## For developers `autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. + +## `autoware::universe_utils` + +### `systems` + +#### `autoware::universe_utils::TimeKeeper` + +##### Constructor + +```cpp +template +explicit TimeKeeper(Reporters... reporters); +``` + +- Initializes the `TimeKeeper` with a list of reporters. + +##### Methods + +- `void add_reporter(std::ostream * os);` + + - Adds a reporter to output processing times to an `ostream`. + - `os`: Pointer to the `ostream` object. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher with `std_msgs::msg::String`. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void start_track(const std::string & func_name);` + + - Starts tracking the processing time of a function. + - `func_name`: Name of the function to be tracked. + +- `void end_track(const std::string & func_name);` + - Ends tracking the processing time of a function. + - `func_name`: Name of the function to end tracking. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` + +#### `autoware::universe_utils::ScopedTimeTrack` + +##### Description + +Class for automatically tracking the processing time of a function within a scope. + +##### Constructor + +```cpp +ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); +``` + +- `func_name`: Name of the function to be tracked. +- `time_keeper`: Reference to the `TimeKeeper` object. + +##### Destructor + +```cpp +~ScopedTimeTrack(); +``` + +- Destroys the `ScopedTimeTrack` object, ending the tracking of the function. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp new file mode 100644 index 0000000000000..010cc58aba8ae --- /dev/null +++ b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp @@ -0,0 +1,61 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp new file mode 100644 index 0000000000000..3f6037e68daac --- /dev/null +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp new file mode 100644 index 0000000000000..96070f0f30b77 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -0,0 +1,202 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ + +#include "autoware/universe_utils/system/stop_watch.hpp" + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware::universe_utils +{ +/** + * @brief Class representing a node in the time tracking tree + */ +class ProcessingTimeNode : public std::enable_shared_from_this +{ +public: + /** + * @brief Construct a new ProcessingTimeNode object + * + * @param name Name of the node + */ + explicit ProcessingTimeNode(const std::string & name); + + /** + * @brief Add a child node + * + * @param name Name of the child node + * @return std::shared_ptr Shared pointer to the newly added child node + */ + std::shared_ptr add_child(const std::string & name); + + /** + * @brief Get the result string representing the node and its children in a tree structure + * + * @return std::string Result string representing the node and its children + */ + std::string to_string() const; + + /** + * @brief Construct a ProcessingTimeTree message from the node and its children + * + * @return tier4_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree message + */ + tier4_debug_msgs::msg::ProcessingTimeTree to_msg() const; + + /** + * @brief Get the parent node + * + * @return std::shared_ptr Shared pointer to the parent node + */ + std::shared_ptr get_parent_node() const; + + /** + * @brief Get the child nodes + * + * @return std::vector> Vector of shared pointers to the child + * nodes + */ + std::vector> get_child_nodes() const; + + /** + * @brief Set the processing time for the node + * + * @param processing_time Processing time to be set + */ + void set_time(const double processing_time); + + /** + * @brief Get the name of the node + * + * @return std::string Name of the node + */ + std::string get_name() const; + +private: + const std::string name_; //!< Name of the node + double processing_time_{0.0}; //!< Processing time of the node + std::shared_ptr parent_node_{nullptr}; //!< Shared pointer to the parent node + std::vector> + child_nodes_; //!< Vector of shared pointers to the child nodes +}; + +using ProcessingTimeDetail = + tier4_debug_msgs::msg::ProcessingTimeTree; //!< Alias for the ProcessingTimeTree message + +/** + * @brief Class for tracking and reporting the processing time of various functions + */ +class TimeKeeper +{ +public: + template + explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) + { + reporters_.reserve(sizeof...(Reporters)); + (add_reporter(reporters), ...); + } + + /** + * @brief Add a reporter to output processing times to an ostream + * + * @param os Pointer to the ostream object + */ + void add_reporter(std::ostream * os); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher with + * std_msgs::msg::String + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Start tracking the processing time of a function + * + * @param func_name Name of the function to be tracked + */ + void start_track(const std::string & func_name); + + /** + * @brief End tracking the processing time of a function + * + * @param func_name Name of the function to end tracking + */ + void end_track(const std::string & func_name); + +private: + /** + * @brief Report the processing times to all registered reporters + */ + void report(); + + std::shared_ptr + current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr root_node_; //!< Shared pointer to the root time node + autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; //!< StopWatch object for tracking the processing time + + std::vector &)>> + reporters_; //!< Vector of functions for reporting the processing times +}; + +/** + * @brief Class for automatically tracking the processing time of a function within a scope + */ +class ScopedTimeTrack +{ +public: + /** + * @brief Construct a new ScopedTimeTrack object + * + * @param func_name Name of the function to be tracked + * @param time_keeper Reference to the TimeKeeper object + */ + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + + /** + * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function + */ + ~ScopedTimeTrack(); + +private: + const std::string func_name_; //!< Name of the function being tracked + TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp new file mode 100644 index 0000000000000..58bed6677227c --- /dev/null +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -0,0 +1,175 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include +#include + +namespace autoware::universe_utils +{ + +ProcessingTimeNode::ProcessingTimeNode(const std::string & name) : name_(name) +{ +} + +std::shared_ptr ProcessingTimeNode::add_child(const std::string & name) +{ + auto new_child_node = std::make_shared(name); + new_child_node->parent_node_ = shared_from_this(); + child_nodes_.push_back(new_child_node); + return new_child_node; +} + +std::string ProcessingTimeNode::to_string() const +{ + std::function + construct_string = [&]( + const ProcessingTimeNode & node, std::ostringstream & oss, + const std::string & prefix, bool is_last, bool is_root) { + if (!is_root) { + oss << prefix << (is_last ? "└── " : "├── "); + } + oss << node.name_ << " (" << node.processing_time_ << "ms)\n"; + for (size_t i = 0; i < node.child_nodes_.size(); ++i) { + const auto & child = node.child_nodes_[i]; + construct_string( + *child, oss, prefix + (is_last ? " " : "│ "), i == node.child_nodes_.size() - 1, + false); + } + }; + + std::ostringstream oss; + construct_string(*this, oss, "", true, true); + return oss.str(); +} + +tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const +{ + tier4_debug_msgs::msg::ProcessingTimeTree time_tree_msg; + + std::function + construct_msg = [&]( + const ProcessingTimeNode & node, + tier4_debug_msgs::msg::ProcessingTimeTree & tree_msg, int parent_id) { + tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; + time_node_msg.name = node.name_; + time_node_msg.processing_time = node.processing_time_; + time_node_msg.id = tree_msg.nodes.size() + 1; + time_node_msg.parent_id = parent_id; + tree_msg.nodes.emplace_back(time_node_msg); + + for (const auto & child : node.child_nodes_) { + construct_msg(*child, tree_msg, time_node_msg.id); + } + }; + construct_msg(*this, time_tree_msg, 0); + + return time_tree_msg; +} + +std::shared_ptr ProcessingTimeNode::get_parent_node() const +{ + return parent_node_; +} +std::vector> ProcessingTimeNode::get_child_nodes() const +{ + return child_nodes_; +} +void ProcessingTimeNode::set_time(const double processing_time) +{ + processing_time_ = processing_time; +} +std::string ProcessingTimeNode::get_name() const +{ + return name_; +} + +void TimeKeeper::add_reporter(std::ostream * os) +{ + reporters_.emplace_back([os](const std::shared_ptr & node) { + *os << "==========================" << std::endl; + *os << node->to_string() << std::endl; + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + publisher->publish(node->to_msg()); + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + std_msgs::msg::String msg; + msg.data = node->to_string(); + publisher->publish(msg); + }); +} + +void TimeKeeper::start_track(const std::string & func_name) +{ + if (current_time_node_ == nullptr) { + current_time_node_ = std::make_shared(func_name); + root_node_ = current_time_node_; + } else { + current_time_node_ = current_time_node_->add_child(func_name); + } + stop_watch_.tic(func_name); +} + +void TimeKeeper::end_track(const std::string & func_name) +{ + if (current_time_node_->get_name() != func_name) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but end_track({}) is called", + current_time_node_->get_name(), func_name)); + } + const double processing_time = stop_watch_.toc(func_name); + current_time_node_->set_time(processing_time); + current_time_node_ = current_time_node_->get_parent_node(); + + if (current_time_node_ == nullptr) { + report(); + } +} + +void TimeKeeper::report() +{ + if (current_time_node_ != nullptr) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but report() is called", current_time_node_->get_name())); + } + for (const auto & reporter : reporters_) { + reporter(root_node_); + } + current_time_node_.reset(); + root_node_.reset(); +} + +ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper) +: func_name_(func_name), time_keeper_(time_keeper) +{ + time_keeper_.start_track(func_name_); +} + +ScopedTimeTrack::~ScopedTimeTrack() +{ + time_keeper_.end_track(func_name_); +} + +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp new file mode 100644 index 0000000000000..71ca7cc74bec5 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -0,0 +1,53 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include +#include + +#include + +#include +#include +#include + +TEST(system, TimeKeeper) +{ + using autoware::universe_utils::ScopedTimeTrack; + using autoware::universe_utils::TimeKeeper; + + rclcpp::Node node{"sample_node"}; + + auto publisher = node.create_publisher( + "~/debug/processing_time_tree", 1); + + TimeKeeper time_keeper(&std::cerr, publisher); + + ScopedTimeTrack st{"main_func", time_keeper}; + + { // funcA + ScopedTimeTrack st{"funcA", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + { // funcB + ScopedTimeTrack st{"funcB", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + { // funcC + ScopedTimeTrack st{"funcC", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } +} diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 7f2688ffaad95..399262f93e19d 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -17,7 +17,6 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -43,57 +42,6 @@ struct PlannerData double ego_vel{}; }; -struct TimeKeeper -{ - void init() - { - accumulated_msg = "\n"; - accumulated_time = 0.0; - } - - template - TimeKeeper & operator<<(const T & msg) - { - latest_stream << msg; - return *this; - } - - void endLine() - { - const auto msg = latest_stream.str(); - accumulated_msg += msg + "\n"; - latest_stream.str(""); - - if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); - } - } - - void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - - void toc(const std::string & func_name, const std::string & white_spaces) - { - const double elapsed_time = stop_watch_.toc(func_name); - *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; - accumulated_time = elapsed_time; - endLine(); - } - - std::string getLog() const { return accumulated_msg; } - - bool enable_calculation_time_info; - std::string accumulated_msg = "\n"; - std::stringstream latest_stream; - - double getAccumulatedTime() const { return accumulated_time; } - - double accumulated_time{0.0}; - - autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; -}; - struct DebugData { // settting diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 4303e5c7e05b4..3443ab663b642 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -19,6 +19,7 @@ #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" @@ -107,7 +108,7 @@ class MPTOptimizer rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr); + const std::shared_ptr time_keeper_); std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; @@ -222,7 +223,7 @@ class MPTOptimizer autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; rclcpp::Logger logger_; MPTParam mpt_param_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 359c20f2a4d29..6f75c649e02b2 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -22,10 +22,11 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "rclcpp/rclcpp.hpp" #include +#include #include #include @@ -65,7 +66,7 @@ class PathOptimizer : public rclcpp::Node // argument variables autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; - mutable std::shared_ptr time_keeper_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; @@ -98,6 +99,8 @@ class PathOptimizer : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index b8162f5ef2d32..1d23d7788dca1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -18,6 +18,7 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -39,9 +40,9 @@ class StateEquationGenerator StateEquationGenerator() = default; StateEquationGenerator( const double wheel_base, const double max_steer_rad, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), - time_keeper_ptr_(time_keeper_ptr) + time_keeper_(time_keeper) { } @@ -56,7 +57,7 @@ class StateEquationGenerator private: std::unique_ptr vehicle_model_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 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}; } From a70aa0e55d8debc49b4d39f9118ef72c6697fe3b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 09:29:58 +0900 Subject: [PATCH 12/17] fix(multi_object_tracker): fix publish interval adjust timing (#7904) refactor: optimize publish time check in multi_object_tracker_node.cpp Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/multi_object_tracker_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp index d3e1c9686b69b..198d9e2238566 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -221,8 +221,9 @@ void MultiObjectTracker::onTrigger() } else { // Publish if the next publish time is close const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - if ((current_time - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(current_time); + const rclcpp::Time publish_time = this->now(); + if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(publish_time); } } } From 7dcdd676f55f4c24893a589da0e543a2d7a72a97 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 09:55:53 +0900 Subject: [PATCH 13/17] refactor(tier4_perception_launch): add maintainer to tier4_perception_launch (#7893) refactor: add maintainer to tier4_perception_launch Signed-off-by: Taekjin LEE --- launch/tier4_perception_launch/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 57d4b209efeef..5c15a937304ad 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -7,6 +7,7 @@ Yukihiro Saito Shunsuke Miura Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto From 6d0247e078d2e944c482b78a95eac9025f5863c2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 11:24:09 +0900 Subject: [PATCH 14/17] refactor(euclidean_cluster)!: fix namespace and directory structure (#7887) * refactor: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE refactor: fix namespace Signed-off-by: Taekjin LEE chore: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/debugger/debugger.hpp | 3 -- .../src/detection_by_tracker_node.cpp | 4 +-- .../src/detection_by_tracker_node.hpp | 8 +++--- perception/euclidean_cluster/CMakeLists.txt | 28 +++++++++---------- .../euclidean_cluster/euclidean_cluster.hpp | 8 +++--- .../euclidean_cluster_interface.hpp | 4 +-- .../euclidean_cluster/utils.hpp | 4 +-- .../voxel_grid_based_euclidean_cluster.hpp | 8 +++--- .../launch/euclidean_cluster.launch.py | 4 +-- ...xel_grid_based_euclidean_cluster.launch.py | 4 +-- .../lib/euclidean_cluster.cpp | 6 ++-- perception/euclidean_cluster/lib/utils.cpp | 6 ++-- .../voxel_grid_based_euclidean_cluster.cpp | 6 ++-- .../src/euclidean_cluster_node.cpp | 8 +++--- .../src/euclidean_cluster_node.hpp | 7 +++-- ...oxel_grid_based_euclidean_cluster_node.cpp | 8 +++--- ...oxel_grid_based_euclidean_cluster_node.hpp | 6 ++-- .../src/roi_pointcloud_fusion/node.cpp | 4 +-- 18 files changed, 62 insertions(+), 64 deletions(-) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster.hpp (87%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster_interface.hpp (95%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/utils.hpp (94%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp (90%) diff --git a/perception/detection_by_tracker/src/debugger/debugger.hpp b/perception/detection_by_tracker/src/debugger/debugger.hpp index 56f37fb10043d..bda42aa5f0b55 100644 --- a/perception/detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/detection_by_tracker/src/debugger/debugger.hpp @@ -17,9 +17,6 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp index c22d015aa11a4..4c65500d96aaa 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp @@ -115,7 +115,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) setMaxSearchRange(); shape_estimator_ = std::make_shared(true, true); - cluster_ = std::make_shared( + cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); published_time_publisher_ = @@ -277,7 +277,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( const auto & label = target_object.classification.front().label; // initialize clustering parameters - euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( + autoware::euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( false, 4, 10000, initial_cluster_range, initial_voxel_size, 0); // convert to pcl diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp index 095708fb4b51f..9c974d72cfdca 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp @@ -15,12 +15,12 @@ #ifndef DETECTION_BY_TRACKER_NODE_HPP_ #define DETECTION_BY_TRACKER_NODE_HPP_ +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "autoware/shape_estimation/shape_estimator.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" @@ -69,7 +69,7 @@ class DetectionByTracker : public rclcpp::Node TrackerHandler tracker_handler_; std::shared_ptr shape_estimator_; - std::shared_ptr cluster_; + std::shared_ptr cluster_; std::shared_ptr debugger_; std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index fe1fa9fda5fca..dacbcf460a14a 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -13,45 +13,45 @@ include_directories( ${PCL_INCLUDE_DIRS} ) -ament_auto_add_library(cluster_lib SHARED - lib/utils.cpp +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/euclidean_cluster.cpp lib/voxel_grid_based_euclidean_cluster.cpp + lib/utils.cpp ) -target_link_libraries(cluster_lib +target_link_libraries(${PROJECT_NAME}_lib ${PCL_LIBRARIES} ) -target_include_directories(cluster_lib +target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ ) -ament_auto_add_library(euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_node_core SHARED src/euclidean_cluster_node.cpp ) -target_link_libraries(euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(euclidean_cluster_node_core - PLUGIN "euclidean_cluster::EuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_node_core + PLUGIN "autoware::euclidean_cluster::EuclideanClusterNode" EXECUTABLE euclidean_cluster_node ) -ament_auto_add_library(voxel_grid_based_euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_voxel_grid_node_core SHARED src/voxel_grid_based_euclidean_cluster_node.cpp ) -target_link_libraries(voxel_grid_based_euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_voxel_grid_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_node_core - PLUGIN "euclidean_cluster::VoxelGridBasedEuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_voxel_grid_node_core + PLUGIN "autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode" EXECUTABLE voxel_grid_based_euclidean_cluster_node ) diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp similarity index 87% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp index aec3f56936828..8c4aad537cc43 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp @@ -14,14 +14,14 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanCluster : public EuclideanClusterInterface { @@ -42,4 +42,4 @@ class EuclideanCluster : public EuclideanClusterInterface float tolerance_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp similarity index 95% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp index 65b907f747666..461d75d4931db 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanClusterInterface { @@ -54,4 +54,4 @@ class EuclideanClusterInterface int max_cluster_size_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp similarity index 94% rename from perception/euclidean_cluster/include/euclidean_cluster/utils.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp index 50d2306d48f72..a52c0840c4d41 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); void convertPointCloudClusters2Msg( @@ -37,4 +37,4 @@ void convertPointCloudClusters2Msg( void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp similarity index 90% rename from perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index 375cc2d19a01f..5c51bb85ce177 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -14,15 +14,15 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface { @@ -52,4 +52,4 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface int min_points_number_per_voxel_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index b8d4f61a9cf28..053438a4352b0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", "low_height/pointcloud"), @@ -60,7 +60,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 607e1bf30ccaa..0b502b1c43d67 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", "low_height/pointcloud"), @@ -61,7 +61,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index 5ba1b99403280..0481b7e1b742d 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanCluster::EuclideanCluster() { @@ -93,4 +93,4 @@ bool EuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6d119ef3d01aa..624c838ef0647 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -11,7 +11,7 @@ // 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 "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include @@ -19,7 +19,7 @@ #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { @@ -128,4 +128,4 @@ void convertObjectMsg2SensorMsg( output.height = 1; output.is_dense = false; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 126f877afddb0..096de3dc1b296 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() { @@ -166,4 +166,4 @@ bool VoxelGridBasedEuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index f986b0334935f..b0b9448dfc0c4 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) : Node("euclidean_cluster_node", options) @@ -86,8 +86,8 @@ void EuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::EuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::EuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 92f10696d17dc..5ab48abb4f042 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include @@ -26,7 +26,8 @@ #include #include -namespace euclidean_cluster + +namespace autoware::euclidean_cluster { class EuclideanClusterNode : public rclcpp::Node { @@ -45,4 +46,4 @@ class EuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index e9425095a3b0d..e54c55e4873ee 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "voxel_grid_based_euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( const rclcpp::NodeOptions & options) @@ -89,8 +89,8 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::VoxelGridBasedEuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index b0c5d0e5a7fbf..b48e30b37de04 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include @@ -26,7 +26,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node { @@ -45,4 +45,4 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 7f19402d9e565..904e66cb53298 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -25,7 +25,7 @@ #include #endif -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" namespace image_projection_based_fusion { @@ -68,7 +68,7 @@ void RoiPointCloudFusionNode::postprocess( // publish debug cluster if (cluster_debug_pub_->get_subscription_count() > 0) { sensor_msgs::msg::PointCloud2 debug_cluster_msg; - euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); cluster_debug_pub_->publish(debug_cluster_msg); } } From 03877b072f7d58f5c42fc06a33d2b330941acd3b Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 9 Jul 2024 13:42:29 +0900 Subject: [PATCH 15/17] chore(radar_object_tracker): delete maintainer (#7898) Signed-off-by: scepter914 --- perception/radar_object_tracker/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 79ae0385d4b5e..f104e2d7ea456 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -6,7 +6,6 @@ Do tracking radar object Yoshi Ri Yukihiro Saito - Satoshi Tanaka Taekjin Lee Apache License 2.0 From 458dbe5c5445fe95a62d9a4804509acbc024d703 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 9 Jul 2024 07:45:46 +0200 Subject: [PATCH 16/17] chore(autoware_behavior_velocity_planner): remove no_prefix function from tests (#7589) Signed-off-by: Esteve Fernandez --- .../test/src/test_node_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 3482d7be8ec48..cc2f4bf3b96b4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -54,7 +54,6 @@ std::shared_ptr generateNode() const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); From f716b5b0ac4678d5d75ae06936d66e340292930d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 15:02:25 +0900 Subject: [PATCH 17/17] fix(euclidean_cluster): include autoware_universe_utils headers (#7908) fix: include autoware_universe_utils headers instead of compare_map_segmentation package Signed-off-by: Taekjin LEE --- perception/euclidean_cluster/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index c5cdd9c54b988..2644786ead061 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_perception_msgs - compare_map_segmentation + autoware_universe_utils geometry_msgs libpcl-all-dev pcl_conversions