Skip to content

Commit

Permalink
refactor based on linter
Browse files Browse the repository at this point in the history 
Signed-off-by: a-maumau <[email protected]>
  • Loading branch information
@a-maumau
a-maumau committed Jun 7, 2024
1 parent dd3a7a8 commit 92d48f3
Show file tree
Hide file tree
Showing 4 changed files with 47 additions and 49 deletions.
2 changes: 1 addition & 1 deletion map/map_tf_generator/include/map_tf_generator/uniform_random.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include <random>
#include <vector>

std::vector<size_t> UniformRandom(const size_t max_exclusive, const size_t n)
std::vector<size_t> inline uniform_random(const size_t max_exclusive, const size_t n)
{
std::default_random_engine generator;
std::uniform_int_distribution<size_t> distribution(0, max_exclusive - 1);
Expand Down
47 changes: 23 additions & 24 deletions map/map_tf_generator/src/pcd_map_tf_generator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,22 +26,21 @@

#include <memory>
#include <string>
#include <variant>

constexpr size_t N_SAMPLES = 20;
constexpr size_t n_samples = 20;

class PcdMapTFGeneratorNode : public rclcpp::Node
{
public:
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
explicit PcdMapTFGeneratorNode(const rclcpp::NodeOptions & options)
: Node("pcd_map_tf_generator", options)
: Node("pcd_map_tf_generator", options), map_frame_(declare_parameter<std::string>("map_frame")),
viewer_frame_(declare_parameter<std::string>("viewer_frame"))
{
map_frame_ = declare_parameter<std::string>("map_frame");
viewer_frame_ = declare_parameter<std::string>("viewer_frame");

sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
"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<tf2_ros::StaticTransformBroadcaster>(this);
}
Expand All @@ -53,7 +52,7 @@ class PcdMapTFGeneratorNode : public rclcpp::Node

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> 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
Expand All @@ -62,32 +61,32 @@ class PcdMapTFGeneratorNode : public rclcpp::Node
PointCloud clouds;
pcl::fromROSMsg<pcl::PointXYZ>(*clouds_ros, clouds);

const std::vector<size_t> indices = UniformRandom(clouds.size(), N_SAMPLES);
const std::vector<size_t> 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<double>(indices.size());
coordinate[1] = coordinate[1] / static_cast<double>(indices.size());
coordinate[2] = coordinate[2] / static_cast<double>(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:"
Expand Down
41 changes: 20 additions & 21 deletions map/map_tf_generator/src/vector_map_tf_generator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>("map_frame")),
viewer_frame_(declare_parameter<std::string>("viewer_frame"))
{
map_frame_ = declare_parameter<std::string>("map_frame");
viewer_frame_ = declare_parameter<std::string>("viewer_frame");

sub_ = create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
"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<tf2_ros::StaticTransformBroadcaster>(this);
}
Expand All @@ -49,7 +48,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;
std::shared_ptr<lanelet::LaneletMap> 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::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_);
Expand All @@ -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<double>(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<double>(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<double>(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:"
Expand Down
6 changes: 3 additions & 3 deletions map/map_tf_generator/test/test_uniform_random.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,10 @@ using testing::Each;
using testing::Ge;
using testing::Lt;

TEST(UniformRandom, UniformRandom)
TEST(uniform_random, uniform_random)
{
{
const std::vector<size_t> random = UniformRandom(4, 0);
const std::vector<size_t> random = uniform_random(4, 0);
ASSERT_EQ(random.size(), static_cast<size_t>(0));
}

Expand All @@ -35,7 +35,7 @@ TEST(UniformRandom, UniformRandom)
const size_t max_exclusive = 4;

for (int i = 0; i < 50; i++) {
const std::vector<size_t> random = UniformRandom(4, 10);
const std::vector<size_t> 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)
}
Expand Down

0 comments on commit 92d48f3

Please sign in to comment.