diff --git a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp index 345703a19bc06..cda3bc5c49a0c 100644 --- a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp +++ b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp @@ -18,7 +18,7 @@ #include #include -std::vector UniformRandom(const size_t max_exclusive, const size_t n) +std::vector inline uniform_random(const size_t max_exclusive, const size_t n) { std::default_random_engine generator; std::uniform_int_distribution distribution(0, max_exclusive - 1); diff --git a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp index 69a66cf52c865..9a31cb5f76aaf 100644 --- a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -26,22 +26,21 @@ #include #include +#include -constexpr size_t N_SAMPLES = 20; +constexpr size_t n_samples = 20; class PcdMapTFGeneratorNode : public rclcpp::Node { public: using PointCloud = pcl::PointCloud; explicit PcdMapTFGeneratorNode(const rclcpp::NodeOptions & options) - : Node("pcd_map_tf_generator", options) + : Node("pcd_map_tf_generator", options), map_frame_(declare_parameter("map_frame")), + viewer_frame_(declare_parameter("viewer_frame")) { - map_frame_ = declare_parameter("map_frame"); - viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&PcdMapTFGeneratorNode::onPointCloud, this, std::placeholders::_1)); + std::bind(&PcdMapTFGeneratorNode::on_point_cloud, this, std::placeholders::_1)); static_broadcaster_ = std::make_shared(this); } @@ -53,7 +52,7 @@ class PcdMapTFGeneratorNode : public rclcpp::Node std::shared_ptr static_broadcaster_; - void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) + void on_point_cloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) { // fix random seed to produce the same viewer position every time // 3939 is just the author's favorite number @@ -62,32 +61,32 @@ class PcdMapTFGeneratorNode : public rclcpp::Node PointCloud clouds; pcl::fromROSMsg(*clouds_ros, clouds); - const std::vector indices = UniformRandom(clouds.size(), N_SAMPLES); + const std::vector indices = uniform_random(clouds.size(), n_samples); double coordinate[3] = {0, 0, 0}; for (const auto i : indices) { coordinate[0] += clouds.points[i].x; coordinate[1] += clouds.points[i].y; coordinate[2] += clouds.points[i].z; } - coordinate[0] = coordinate[0] / indices.size(); - coordinate[1] = coordinate[1] / indices.size(); - coordinate[2] = coordinate[2] / indices.size(); - - geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = this->now(); - static_transformStamped.header.frame_id = map_frame_; - static_transformStamped.child_frame_id = viewer_frame_; - static_transformStamped.transform.translation.x = coordinate[0]; - static_transformStamped.transform.translation.y = coordinate[1]; - static_transformStamped.transform.translation.z = coordinate[2]; + coordinate[0] = coordinate[0] / static_cast(indices.size()); + coordinate[1] = coordinate[1] / static_cast(indices.size()); + coordinate[2] = coordinate[2] / static_cast(indices.size()); + + geometry_msgs::msg::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = this->now(); + static_transform_stamped.header.frame_id = map_frame_; + static_transform_stamped.child_frame_id = viewer_frame_; + static_transform_stamped.transform.translation.x = coordinate[0]; + static_transform_stamped.transform.translation.y = coordinate[1]; + static_transform_stamped.transform.translation.z = coordinate[2]; tf2::Quaternion quat; quat.setRPY(0, 0, 0); - static_transformStamped.transform.rotation.x = quat.x(); - static_transformStamped.transform.rotation.y = quat.y(); - static_transformStamped.transform.rotation.z = quat.z(); - static_transformStamped.transform.rotation.w = quat.w(); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); - static_broadcaster_->sendTransform(static_transformStamped); + static_broadcaster_->sendTransform(static_transform_stamped); RCLCPP_INFO_STREAM( get_logger(), "broadcast static tf. map_frame:" diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index f242254a456a1..df3075bdce1fe 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -29,14 +29,13 @@ class VectorMapTFGeneratorNode : public rclcpp::Node { public: explicit VectorMapTFGeneratorNode(const rclcpp::NodeOptions & options) - : Node("vector_map_tf_generator", options) + : Node("vector_map_tf_generator", options), + map_frame_(declare_parameter("map_frame")), + viewer_frame_(declare_parameter("viewer_frame")) { - map_frame_ = declare_parameter("map_frame"); - viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1)); + std::bind(&VectorMapTFGeneratorNode::on_vector_map, this, std::placeholders::_1)); static_broadcaster_ = std::make_shared(this); } @@ -49,7 +48,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node std::shared_ptr static_broadcaster_; std::shared_ptr lanelet_map_ptr_; - void onVectorMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); @@ -66,27 +65,27 @@ class VectorMapTFGeneratorNode : public rclcpp::Node points_z.push_back(point_z); } const double coordinate_x = - std::accumulate(points_x.begin(), points_x.end(), 0.0) / points_x.size(); + std::accumulate(points_x.begin(), points_x.end(), 0.0) / static_cast(points_x.size()); const double coordinate_y = - std::accumulate(points_y.begin(), points_y.end(), 0.0) / points_y.size(); + std::accumulate(points_y.begin(), points_y.end(), 0.0) / static_cast(points_y.size()); const double coordinate_z = - std::accumulate(points_z.begin(), points_z.end(), 0.0) / points_z.size(); + std::accumulate(points_z.begin(), points_z.end(), 0.0) / static_cast(points_z.size()); - geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = this->now(); - static_transformStamped.header.frame_id = map_frame_; - static_transformStamped.child_frame_id = viewer_frame_; - static_transformStamped.transform.translation.x = coordinate_x; - static_transformStamped.transform.translation.y = coordinate_y; - static_transformStamped.transform.translation.z = coordinate_z; + geometry_msgs::msg::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = this->now(); + static_transform_stamped.header.frame_id = map_frame_; + static_transform_stamped.child_frame_id = viewer_frame_; + static_transform_stamped.transform.translation.x = coordinate_x; + static_transform_stamped.transform.translation.y = coordinate_y; + static_transform_stamped.transform.translation.z = coordinate_z; tf2::Quaternion quat; quat.setRPY(0, 0, 0); - static_transformStamped.transform.rotation.x = quat.x(); - static_transformStamped.transform.rotation.y = quat.y(); - static_transformStamped.transform.rotation.z = quat.z(); - static_transformStamped.transform.rotation.w = quat.w(); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); - static_broadcaster_->sendTransform(static_transformStamped); + static_broadcaster_->sendTransform(static_transform_stamped); RCLCPP_INFO_STREAM( get_logger(), "broadcast static tf. map_frame:" diff --git a/map/map_tf_generator/test/test_uniform_random.cpp b/map/map_tf_generator/test/test_uniform_random.cpp index 455edc2d5dfd2..19e558ebb2add 100644 --- a/map/map_tf_generator/test/test_uniform_random.cpp +++ b/map/map_tf_generator/test/test_uniform_random.cpp @@ -21,10 +21,10 @@ using testing::Each; using testing::Ge; using testing::Lt; -TEST(UniformRandom, UniformRandom) +TEST(uniform_random, uniform_random) { { - const std::vector random = UniformRandom(4, 0); + const std::vector random = uniform_random(4, 0); ASSERT_EQ(random.size(), static_cast(0)); } @@ -35,7 +35,7 @@ TEST(UniformRandom, UniformRandom) const size_t max_exclusive = 4; for (int i = 0; i < 50; i++) { - const std::vector random = UniformRandom(4, 10); + const std::vector random = uniform_random(4, 10); ASSERT_EQ(random.size(), 10U); ASSERT_THAT(random, Each(AllOf(Ge(min_inclusive), Lt(max_exclusive)))); // in range [0, 4) }