Skip to content

Commit

Permalink
refactor(autoware_point_types): prefix namespace with autoware::point…
Browse files Browse the repository at this point in the history
…_types (#9169)

Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp>
  • Loading branch information
esteve authored Oct 30, 2024
1 parent 68d3bcf commit 94b9ad0
Show file tree
Hide file tree
Showing 25 changed files with 102 additions and 102 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE_POINT_TYPES__TYPES_HPP_
#define AUTOWARE_POINT_TYPES__TYPES_HPP_
#ifndef AUTOWARE__POINT_TYPES__TYPES_HPP_
#define AUTOWARE__POINT_TYPES__TYPES_HPP_

#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

Expand All @@ -22,7 +22,7 @@
#include <cmath>
#include <tuple>

namespace autoware_point_types
namespace autoware::point_types
{
template <class T>
bool float_eq(const T a, const T b, const T eps = 10e-6)
Expand Down Expand Up @@ -166,23 +166,23 @@ using PointXYZIRCAEDTGenerator = std::tuple<
field_return_type_generator, field_channel_generator, field_azimuth_generator,
field_elevation_generator, field_distance_generator, field_time_stamp_generator>;

} // namespace autoware_point_types
} // namespace autoware::point_types

POINT_CLOUD_REGISTER_POINT_STRUCT(
autoware_point_types::PointXYZIRC,
autoware::point_types::PointXYZIRC,
(float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel))

POINT_CLOUD_REGISTER_POINT_STRUCT(
autoware_point_types::PointXYZIRADRT,
autoware::point_types::PointXYZIRADRT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(
double, time_stamp, time_stamp))

POINT_CLOUD_REGISTER_POINT_STRUCT(
autoware_point_types::PointXYZIRCAEDT,
autoware::point_types::PointXYZIRCAEDT,
(float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
std::uint8_t, return_type,
return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)(
float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp))
#endif // AUTOWARE_POINT_TYPES__TYPES_HPP_
#endif // AUTOWARE__POINT_TYPES__TYPES_HPP_
22 changes: 11 additions & 11 deletions common/autoware_point_types/test/test_point_types.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware_point_types/types.hpp"
#include "autoware/point_types/types.hpp"

#include <gtest/gtest.h>

#include <limits>

TEST(PointEquality, PointXYZI)
{
using autoware_point_types::PointXYZI;
using autoware::point_types::PointXYZI;

PointXYZI pt0{0, 1, 2, 3};
PointXYZI pt1{0, 1, 2, 3};
Expand All @@ -29,7 +29,7 @@ TEST(PointEquality, PointXYZI)

TEST(PointEquality, PointXYZIRADRT)
{
using autoware_point_types::PointXYZIRADRT;
using autoware::point_types::PointXYZIRADRT;

PointXYZIRADRT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8};
PointXYZIRADRT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8};
Expand All @@ -38,7 +38,7 @@ TEST(PointEquality, PointXYZIRADRT)

TEST(PointEquality, PointXYZIRCAEDT)
{
using autoware_point_types::PointXYZIRCAEDT;
using autoware::point_types::PointXYZIRCAEDT;

PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
Expand All @@ -48,19 +48,19 @@ TEST(PointEquality, PointXYZIRCAEDT)
TEST(PointEquality, FloatEq)
{
// test template
EXPECT_TRUE(autoware_point_types::float_eq<float>(1, 1));
EXPECT_TRUE(autoware_point_types::float_eq<double>(1, 1));
EXPECT_TRUE(autoware::point_types::float_eq<float>(1, 1));
EXPECT_TRUE(autoware::point_types::float_eq<double>(1, 1));

// test floating point error
EXPECT_TRUE(autoware_point_types::float_eq<float>(1, 1 + std::numeric_limits<float>::epsilon()));
EXPECT_TRUE(autoware::point_types::float_eq<float>(1, 1 + std::numeric_limits<float>::epsilon()));

// test difference of sign
EXPECT_FALSE(autoware_point_types::float_eq<float>(2, -2));
EXPECT_FALSE(autoware_point_types::float_eq<float>(-2, 2));
EXPECT_FALSE(autoware::point_types::float_eq<float>(2, -2));
EXPECT_FALSE(autoware::point_types::float_eq<float>(-2, 2));

// small value difference
EXPECT_FALSE(autoware_point_types::float_eq<float>(2, 2 + 10e-6));
EXPECT_FALSE(autoware::point_types::float_eq<float>(2, 2 + 10e-6));

// expect same value if epsilon is larger than difference
EXPECT_TRUE(autoware_point_types::float_eq<float>(2, 2 + 10e-6, 10e-5));
EXPECT_TRUE(autoware::point_types::float_eq<float>(2, 2 + 10e-6, 10e-5));
}
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,9 @@

#include "lidar_marker_localizer.hpp"

#include <autoware/point_types/types.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/transform/transforms.hpp>
#include <autoware_point_types/types.hpp>
#include <pcl_ros/transforms.hpp>
#include <rclcpp/qos.hpp>

Expand Down Expand Up @@ -338,20 +338,20 @@ std::vector<landmark_manager::Landmark> LidarMarkerLocalizer::detect_landmarks(
// TODO(YamatoAndo)
// Transform sensor_frame to base_link

pcl::PointCloud<autoware_point_types::PointXYZIRC>::Ptr points_ptr(
new pcl::PointCloud<autoware_point_types::PointXYZIRC>);
pcl::PointCloud<autoware::point_types::PointXYZIRC>::Ptr points_ptr(
new pcl::PointCloud<autoware::point_types::PointXYZIRC>);
pcl::fromROSMsg(*points_msg_ptr, *points_ptr);

if (points_ptr->empty()) {
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No points!");
return std::vector<landmark_manager::Landmark>{};
}

std::vector<pcl::PointCloud<autoware_point_types::PointXYZIRC>> ring_points(128);
std::vector<pcl::PointCloud<autoware::point_types::PointXYZIRC>> ring_points(128);

float min_x = std::numeric_limits<float>::max();
float max_x = std::numeric_limits<float>::lowest();
for (const autoware_point_types::PointXYZIRC & point : points_ptr->points) {
for (const autoware::point_types::PointXYZIRC & point : points_ptr->points) {
ring_points[point.channel].push_back(point);
min_x = std::min(min_x, point.x);
max_x = std::max(max_x, point.x);
Expand All @@ -365,12 +365,12 @@ std::vector<landmark_manager::Landmark> LidarMarkerLocalizer::detect_landmarks(
std::vector<float> min_y(bin_num, std::numeric_limits<float>::max());

// for each channel
for (const pcl::PointCloud<autoware_point_types::PointXYZIRC> & one_ring : ring_points) {
for (const pcl::PointCloud<autoware::point_types::PointXYZIRC> & one_ring : ring_points) {
std::vector<double> intensity_sum(bin_num, 0.0);
std::vector<int> intensity_num(bin_num, 0);
std::vector<double> average_intensity(bin_num, 0.0);

for (const autoware_point_types::PointXYZIRC & point : one_ring.points) {
for (const autoware::point_types::PointXYZIRC & point : one_ring.points) {
const int bin_index = static_cast<int>((point.x - min_x) / param_.resolution);
intensity_sum[bin_index] += point.intensity;
intensity_num[bin_index]++;
Expand Down Expand Up @@ -507,15 +507,15 @@ sensor_msgs::msg::PointCloud2::SharedPtr LidarMarkerLocalizer::extract_marker_po
const geometry_msgs::msg::Pose marker_pose) const
{
// convert from ROSMsg to PCL
pcl::shared_ptr<pcl::PointCloud<autoware_point_types::PointXYZIRC>> points_ptr(
new pcl::PointCloud<autoware_point_types::PointXYZIRC>);
pcl::shared_ptr<pcl::PointCloud<autoware::point_types::PointXYZIRC>> points_ptr(
new pcl::PointCloud<autoware::point_types::PointXYZIRC>);
pcl::fromROSMsg(*points_msg_ptr, *points_ptr);

pcl::shared_ptr<pcl::PointCloud<autoware_point_types::PointXYZIRC>> marker_points_ptr(
new pcl::PointCloud<autoware_point_types::PointXYZIRC>);
pcl::shared_ptr<pcl::PointCloud<autoware::point_types::PointXYZIRC>> marker_points_ptr(
new pcl::PointCloud<autoware::point_types::PointXYZIRC>);

// extract marker pointcloud
for (const autoware_point_types::PointXYZIRC & point : points_ptr->points) {
for (const autoware::point_types::PointXYZIRC & point : points_ptr->points) {
const double xy_distance = std::sqrt(
std::pow(point.x - marker_pose.position.x, 2.0) +
std::pow(point.y - marker_pose.position.y, 2.0));
Expand Down Expand Up @@ -546,8 +546,8 @@ void LidarMarkerLocalizer::save_detected_marker_log(
marker_points_msg_sensor_frame_ptr);

// convert from ROSMsg to PCL
pcl::shared_ptr<pcl::PointCloud<autoware_point_types::PointXYZIRC>>
marker_points_sensor_frame_ptr(new pcl::PointCloud<autoware_point_types::PointXYZIRC>);
pcl::shared_ptr<pcl::PointCloud<autoware::point_types::PointXYZIRC>>
marker_points_sensor_frame_ptr(new pcl::PointCloud<autoware::point_types::PointXYZIRC>);
pcl::fromROSMsg(*marker_points_msg_sensor_frame_ptr, *marker_points_sensor_frame_ptr);

// to csv format
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
#include "../src/distance_based_compare_map_filter/node.hpp"

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

Expand All @@ -24,8 +24,8 @@
#include <gtest/gtest.h>

using autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent;
using autoware_point_types::PointXYZIRC;
using autoware_point_types::PointXYZIRCGenerator;
using autoware::point_types::PointXYZIRC;
using autoware::point_types::PointXYZIRCGenerator;
using point_cloud_msg_wrapper::PointCloud2Modifier;
using sensor_msgs::msg::PointCloud2;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
#include "../src/voxel_based_approximate_compare_map_filter/node.hpp"

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

Expand All @@ -24,8 +24,8 @@
#include <gtest/gtest.h>

using autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent;
using autoware_point_types::PointXYZIRC;
using autoware_point_types::PointXYZIRCGenerator;
using autoware::point_types::PointXYZIRC;
using autoware::point_types::PointXYZIRCGenerator;
using point_cloud_msg_wrapper::PointCloud2Modifier;
using sensor_msgs::msg::PointCloud2;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
#include "../src/voxel_based_compare_map_filter/node.hpp"

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

Expand All @@ -24,8 +24,8 @@
#include <gtest/gtest.h>

using autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent;
using autoware_point_types::PointXYZIRC;
using autoware_point_types::PointXYZIRCGenerator;
using autoware::point_types::PointXYZIRC;
using autoware::point_types::PointXYZIRCGenerator;
using point_cloud_msg_wrapper::PointCloud2Modifier;
using sensor_msgs::msg::PointCloud2;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
#include "../src/voxel_distance_based_compare_map_filter/node.hpp"

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

Expand All @@ -24,8 +24,8 @@
#include <gtest/gtest.h>

using autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent;
using autoware_point_types::PointXYZIRC;
using autoware_point_types::PointXYZIRCGenerator;
using autoware::point_types::PointXYZIRC;
using autoware::point_types::PointXYZIRCGenerator;
using point_cloud_msg_wrapper::PointCloud2Modifier;
using sensor_msgs::msg::PointCloud2;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"

#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>
#include <experimental/random>

#include <sensor_msgs/msg/point_cloud2.hpp>
Expand All @@ -23,7 +23,7 @@

#include <gtest/gtest.h>

using autoware_point_types::PointXYZI;
using autoware::point_types::PointXYZI;
void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud)
{
pointcloud.fields.resize(4);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

#include "autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp"

#include "autoware_point_types/types.hpp"
#include "autoware/point_types/types.hpp"

#include <autoware/image_projection_based_fusion/utils/geometry.hpp>
#include <autoware/image_projection_based_fusion/utils/utils.hpp>
Expand Down Expand Up @@ -293,15 +293,15 @@ void PointPaintingFusionNode::fuseOnSingleImage(
// sensor_msgs::msg::PointCloud2 transformed_pointcloud;
// tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped);

const auto x_offset =
painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointXYZIRCIndex::X))
.offset;
const auto y_offset =
painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointXYZIRCIndex::Y))
.offset;
const auto z_offset =
painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointXYZIRCIndex::Z))
.offset;
const auto x_offset = painted_pointcloud_msg.fields
.at(static_cast<size_t>(autoware::point_types::PointXYZIRCIndex::X))
.offset;
const auto y_offset = painted_pointcloud_msg.fields
.at(static_cast<size_t>(autoware::point_types::PointXYZIRCIndex::Y))
.offset;
const auto z_offset = painted_pointcloud_msg.fields
.at(static_cast<size_t>(autoware::point_types::PointXYZIRCIndex::Z))
.offset;
const auto class_offset = painted_pointcloud_msg.fields.at(4).offset;
const auto p_step = painted_pointcloud_msg.point_step;
// projection matrix
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,11 @@
// limitations under the License.

#include <autoware/image_projection_based_fusion/utils/utils.hpp>
#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>

#include <gtest/gtest.h>

using autoware_point_types::PointXYZI;
using autoware::point_types::PointXYZI;

void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
#include <tf2_eigen/tf2_eigen.hpp>
#endif

#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>

#include <sensor_msgs/msg/point_cloud2.hpp>

Expand All @@ -39,7 +39,7 @@
namespace autoware::lidar_transfusion
{
constexpr std::size_t AFF_MAT_SIZE = 16; // 4x4 matrix
constexpr std::size_t MAX_CLOUD_STEP_SIZE = sizeof(autoware_point_types::PointXYZIRCAEDT);
constexpr std::size_t MAX_CLOUD_STEP_SIZE = sizeof(autoware::point_types::PointXYZIRCAEDT);

class VoxelGenerator
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include "autoware/lidar_transfusion/utils.hpp"

#include <autoware_point_types/types.hpp>
#include <autoware/point_types/types.hpp>

#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>
Expand Down Expand Up @@ -45,10 +45,10 @@ namespace autoware::lidar_transfusion
{
using sensor_msgs::msg::PointField;

CHECK_FIELD(0, float, autoware_point_types::PointXYZIRCAEDT, x);
CHECK_FIELD(4, float, autoware_point_types::PointXYZIRCAEDT, y);
CHECK_FIELD(8, float, autoware_point_types::PointXYZIRCAEDT, z);
CHECK_FIELD(12, uint8_t, autoware_point_types::PointXYZIRCAEDT, intensity);
CHECK_FIELD(0, float, autoware::point_types::PointXYZIRCAEDT, x);
CHECK_FIELD(4, float, autoware::point_types::PointXYZIRCAEDT, y);
CHECK_FIELD(8, float, autoware::point_types::PointXYZIRCAEDT, z);
CHECK_FIELD(12, uint8_t, autoware::point_types::PointXYZIRCAEDT, intensity);

struct CloudInfo
{
Expand All @@ -64,7 +64,7 @@ struct CloudInfo
uint8_t y_count{1};
uint8_t z_count{1};
uint8_t intensity_count{1};
uint32_t point_step{sizeof(autoware_point_types::PointXYZIRCAEDT)};
uint32_t point_step{sizeof(autoware::point_types::PointXYZIRCAEDT)};
bool is_bigendian{false};

bool operator!=(const CloudInfo & rhs) const
Expand Down
Loading

0 comments on commit 94b9ad0

Please sign in to comment.