diff --git a/autoware_utils/README.md b/autoware_utils/README.md index a5597e9..38db4e0 100644 --- a/autoware_utils/README.md +++ b/autoware_utils/README.md @@ -47,7 +47,6 @@ The ROS module provides utilities for working with ROS messages and nodes: - **`debug_publisher.hpp`**: A helper class for publishing debug messages with timestamps. - **`diagnostics_interface.hpp`**: An interface for publishing diagnostic messages. - **`logger_level_configure.hpp`**: Utility for configuring logger levels dynamically. -- **`marker_helper.hpp`**: Helper functions for creating and manipulating visualization markers. - **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages. - **`msg_operation.hpp`**: Overloaded operators for quaternion messages. - **`parameter.hpp`**: Simplifies parameter retrieval and declaration. diff --git a/autoware_utils/include/autoware_utils/ros/marker_helper.hpp b/autoware_utils/include/autoware_utils/ros/marker_helper.hpp index bc2c015..5224947 100644 --- a/autoware_utils/include/autoware_utils/ros/marker_helper.hpp +++ b/autoware_utils/include/autoware_utils/ros/marker_helper.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2025 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,67 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ #define AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_visualization; } -#include -#include - -namespace autoware_utils -{ -inline geometry_msgs::msg::Point create_marker_position(double x, double y, double z) -{ - geometry_msgs::msg::Point point; - point.x = x; - point.y = y; - point.z = z; - return point; -} - -inline geometry_msgs::msg::Quaternion create_marker_orientation( - double x, double y, double z, double w) -{ - geometry_msgs::msg::Quaternion quaternion; - quaternion.x = x; - quaternion.y = y; - quaternion.z = z; - quaternion.w = w; - return quaternion; -} - -inline geometry_msgs::msg::Vector3 create_marker_scale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - scale.x = x; - scale.y = y; - scale.z = z; - return scale; -} - -inline std_msgs::msg::ColorRGBA create_marker_color(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = a; - return color; -} - -visualization_msgs::msg::Marker create_default_marker( - const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, - const int32_t type, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color); - -visualization_msgs::msg::Marker create_deleted_default_marker( - const rclcpp::Time & now, const std::string & ns, const int32_t id); - -void append_marker_array( - const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array, - const std::optional & current_time = {}); - -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/autoware_utils/package.xml b/autoware_utils/package.xml index 7e9bc20..c10ce5d 100644 --- a/autoware_utils/package.xml +++ b/autoware_utils/package.xml @@ -21,6 +21,7 @@ autoware_planning_msgs autoware_utils_geometry autoware_utils_pcl + autoware_utils_visualization autoware_vehicle_msgs builtin_interfaces diagnostic_msgs @@ -32,7 +33,6 @@ tf2_eigen tf2_geometry_msgs unique_identifier_msgs - visualization_msgs ament_cmake_ros ament_lint_auto diff --git a/autoware_utils_visualization/CMakeLists.txt b/autoware_utils_visualization/CMakeLists.txt new file mode 100644 index 0000000..f76e9c8 --- /dev/null +++ b/autoware_utils_visualization/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_utils_visualization) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + "src/marker_helper.cpp" +) + +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_auto_add_gtest(test_${PROJECT_NAME} ${test_files}) +endif() + +ament_auto_package() diff --git a/autoware_utils_visualization/README.md b/autoware_utils_visualization/README.md new file mode 100644 index 0000000..7097932 --- /dev/null +++ b/autoware_utils_visualization/README.md @@ -0,0 +1,11 @@ +# autoware_utils_visualization + +## Overview + +The **autoware_utils** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. +This package provides essential utilities for visualization. +It is extensively used in the Autoware project to handle common tasks such as creating markers for RViz. + +## Design + +- **`marker_helper.hpp`**: Helper functions for creating and manipulating visualization markers. diff --git a/autoware_utils_visualization/include/autoware_utils_visualization/marker_helper.hpp b/autoware_utils_visualization/include/autoware_utils_visualization/marker_helper.hpp new file mode 100644 index 0000000..6448f49 --- /dev/null +++ b/autoware_utils_visualization/include/autoware_utils_visualization/marker_helper.hpp @@ -0,0 +1,81 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_VISUALIZATION__MARKER_HELPER_HPP_ +#define AUTOWARE_UTILS_VISUALIZATION__MARKER_HELPER_HPP_ + +#include + +#include + +#include +#include + +namespace autoware_utils_visualization +{ +inline geometry_msgs::msg::Point create_marker_position(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; +} + +inline geometry_msgs::msg::Quaternion create_marker_orientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + return quaternion; +} + +inline geometry_msgs::msg::Vector3 create_marker_scale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +inline std_msgs::msg::ColorRGBA create_marker_color(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +visualization_msgs::msg::Marker create_default_marker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color); + +visualization_msgs::msg::Marker create_deleted_default_marker( + const rclcpp::Time & now, const std::string & ns, const int32_t id); + +void append_marker_array( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array, + const std::optional & current_time = {}); + +} // namespace autoware_utils_visualization + +#endif // AUTOWARE_UTILS_VISUALIZATION__MARKER_HELPER_HPP_ diff --git a/autoware_utils_visualization/package.xml b/autoware_utils_visualization/package.xml new file mode 100644 index 0000000..1fb33fb --- /dev/null +++ b/autoware_utils_visualization/package.xml @@ -0,0 +1,26 @@ + + + + autoware_utils_visualization + 1.1.0 + The autoware_utils_visualization package + Jian Kang + Ryohsuke Mitsudome + Esteve Fernandez + Yutaka Kondo + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware_utils/src/ros/marker_helper.cpp b/autoware_utils_visualization/src/marker_helper.cpp similarity index 93% rename from autoware_utils/src/ros/marker_helper.cpp rename to autoware_utils_visualization/src/marker_helper.cpp index a6c2fa6..ddd9385 100644 --- a/autoware_utils/src/ros/marker_helper.cpp +++ b/autoware_utils_visualization/src/marker_helper.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/ros/marker_helper.hpp" +#include "autoware_utils_visualization/marker_helper.hpp" #include -namespace autoware_utils +namespace autoware_utils_visualization { visualization_msgs::msg::Marker create_default_marker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, @@ -69,4 +69,4 @@ void append_marker_array( } } -} // namespace autoware_utils +} // namespace autoware_utils_visualization diff --git a/autoware_utils_visualization/test/cases/marker_helper.cpp b/autoware_utils_visualization/test/cases/marker_helper.cpp new file mode 100644 index 0000000..67afdf9 --- /dev/null +++ b/autoware_utils_visualization/test/cases/marker_helper.cpp @@ -0,0 +1,118 @@ +// Copyright 2025 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils_visualization/marker_helper.hpp" + +#include + +TEST(TestMarkerHelper, CreatePosition) +{ + const auto r = autoware_utils_visualization::create_marker_position(0.1, 0.2, 0.3); + EXPECT_DOUBLE_EQ(r.x, 0.1); + EXPECT_DOUBLE_EQ(r.y, 0.2); + EXPECT_DOUBLE_EQ(r.z, 0.3); +} + +TEST(TestMarkerHelper, CreateOrientation) +{ + const auto r = autoware_utils_visualization::create_marker_orientation(0.1, 0.2, 0.3, 0.4); + EXPECT_DOUBLE_EQ(r.x, 0.1); + EXPECT_DOUBLE_EQ(r.y, 0.2); + EXPECT_DOUBLE_EQ(r.z, 0.3); + EXPECT_DOUBLE_EQ(r.w, 0.4); +} + +TEST(TestMarkerHelper, CreateScale) +{ + const auto r = autoware_utils_visualization::create_marker_scale(0.1, 0.2, 0.3); + EXPECT_DOUBLE_EQ(r.x, 0.1); + EXPECT_DOUBLE_EQ(r.y, 0.2); + EXPECT_DOUBLE_EQ(r.z, 0.3); +} + +TEST(TestMarkerHelper, CreateColor) +{ + const auto r = autoware_utils_visualization::create_marker_color(0.1, 0.2, 0.3, 0.4); + EXPECT_FLOAT_EQ(r.r, 0.1); + EXPECT_FLOAT_EQ(r.g, 0.2); + EXPECT_FLOAT_EQ(r.b, 0.3); + EXPECT_FLOAT_EQ(r.a, 0.4); +} + +TEST(TestMarkerHelper, CreateDefaultMarker) +{ + using visualization_msgs::msg::Marker; + const auto stamp = rclcpp::Time(12345, 67890); + const auto scale = autoware_utils_visualization::create_marker_scale(0.1, 0.2, 0.3); + const auto color = autoware_utils_visualization::create_marker_color(0.1, 0.2, 0.3, 0.4); + + const auto m = autoware_utils_visualization::create_default_marker( + "frame", stamp, "ns", 99, Marker::CUBE, scale, color); + + EXPECT_EQ(m.header.stamp.sec, 12345); + EXPECT_EQ(m.header.stamp.nanosec, 67890); + EXPECT_EQ(m.header.frame_id, "frame"); + EXPECT_EQ(m.ns, "ns"); + EXPECT_EQ(m.id, 99); + EXPECT_EQ(m.action, Marker::ADD); + EXPECT_EQ(m.type, Marker::CUBE); + EXPECT_DOUBLE_EQ(m.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(m.pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(m.pose.position.z, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.w, 1.0); + EXPECT_DOUBLE_EQ(m.scale.x, 0.1); + EXPECT_DOUBLE_EQ(m.scale.y, 0.2); + EXPECT_DOUBLE_EQ(m.scale.z, 0.3); + EXPECT_FLOAT_EQ(m.color.r, 0.1); + EXPECT_FLOAT_EQ(m.color.g, 0.2); + EXPECT_FLOAT_EQ(m.color.b, 0.3); + EXPECT_FLOAT_EQ(m.color.a, 0.4); +} + +TEST(TestMarkerHelper, CreateDeleteMarker) +{ + using visualization_msgs::msg::Marker; + const auto stamp = rclcpp::Time(12345, 67890); + + const auto m = autoware_utils_visualization::create_deleted_default_marker(stamp, "ns", 99); + EXPECT_EQ(m.header.stamp.sec, 12345); + EXPECT_EQ(m.header.stamp.nanosec, 67890); + EXPECT_EQ(m.ns, "ns"); + EXPECT_EQ(m.id, 99); + EXPECT_EQ(m.action, Marker::DELETE); +} + +TEST(TestMarkerHelper, CreateAppendMarkerArray) +{ + visualization_msgs::msg::MarkerArray array1; + visualization_msgs::msg::MarkerArray array2; + array1.markers.resize(2); + array1.markers[0].id = 10; + array1.markers[1].id = 11; + array2.markers.resize(3); + array2.markers[0].id = 20; + array2.markers[1].id = 21; + array2.markers[2].id = 22; + + autoware_utils_visualization::append_marker_array(array2, &array1, std::nullopt); + EXPECT_EQ(array1.markers.size(), 5); + EXPECT_EQ(array1.markers[0].id, 10); + EXPECT_EQ(array1.markers[1].id, 11); + EXPECT_EQ(array1.markers[2].id, 20); + EXPECT_EQ(array1.markers[3].id, 21); + EXPECT_EQ(array1.markers[4].id, 22); +} diff --git a/autoware_utils_visualization/test/main.cpp b/autoware_utils_visualization/test/main.cpp new file mode 100644 index 0000000..7c283b2 --- /dev/null +++ b/autoware_utils_visualization/test/main.cpp @@ -0,0 +1,21 @@ +// Copyright 2025 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}