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 committed Jun 13, 2024
1 parent 17aad93 commit 45c993f
Show file tree
Hide file tree
Showing 22 changed files with 88 additions and 66 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ struct GridKey
GridKey() : x(0), y(0) {}
GridKey(float x, float y) : x(std::floor(x / unit_length)), y(std::floor(y / unit_length)) {}

pcl::PointXYZ get_center_point() const
[[nodiscard]] pcl::PointXYZ get_center_point() const
{
pcl::PointXYZ xyz;
xyz.x = unit_length * (static_cast<float>(x) + 0.5f);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,11 @@

#include "pose_estimator_arbiter/rule_helper/grid_key.hpp"

#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include <boost/functional/hash.hpp>

#include <pcl_conversions/pcl_conversions.h>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_
#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_

#include <string>

#include <rclcpp/node.hpp>

#include <sensor_msgs/msg/point_cloud2.hpp>
Expand All @@ -34,7 +36,7 @@ class PcdOccupancy
public:
explicit PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold);

MarkerArray debug_marker_array() const;
[[nodiscard]] MarkerArray debug_marker_array() const;
void init(PointCloud2::ConstSharedPtr msg);
bool ndt_can_operate(
const geometry_msgs::msg::Point & position, std::string * optional_message = nullptr) const;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,14 @@ using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;

struct PoseEstimatorArea::Impl
{
explicit Impl(rclcpp::Logger logger) : logger_(logger) {}
explicit Impl(const rclcpp::Logger& logger) : logger_(logger) {}
std::multimap<std::string, BoostPolygon> bounding_boxes_;

void init(HADMapBin::ConstSharedPtr msg);
bool within(
[[nodiscard]] bool within(
const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const;

MarkerArray debug_marker_array() const { return marker_array_; }
[[nodiscard]] MarkerArray debug_marker_array() const { return marker_array_; }

private:
rclcpp::Logger logger_;
Expand Down Expand Up @@ -105,7 +105,7 @@ void PoseEstimatorArea::Impl::init(HADMapBin::ConstSharedPtr msg)
marker.color.set__r(1.0f).set__g(1.0f).set__b(0.0f).set__a(1.0f);
marker.ns = subtype;
marker.header.frame_id = "map";
marker.id = marker_array_.markers.size();
marker.id = static_cast<int>(marker_array_.markers.size());

// Enqueue the vertices of the polygon to bounding box and visualization marker
BoostPolygon poly;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,10 @@ class PoseEstimatorArea
// This is assumed to be called only once in the vector map callback.
void init(const HADMapBin::ConstSharedPtr msg);

bool within(
[[nodiscard]] bool within(
const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const;

MarkerArray debug_marker_array() const;
[[nodiscard]] MarkerArray debug_marker_array() const;

private:
struct Impl;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,15 @@

#include <tier4_autoware_utils/ros/parameter.hpp>

#include <utility>

namespace pose_estimator_arbiter::switch_rule
{
PcdMapBasedRule::PcdMapBasedRule(
rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
const std::shared_ptr<const SharedData> shared_data)
: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data)
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
std::shared_ptr<const SharedData> shared_data)
: BaseSwitchRule(node), running_estimator_list_(std::move(running_estimator_list)),
shared_data_(std::move(shared_data))
{
const int pcd_density_upper_threshold =
tier4_autoware_utils::getOrDeclareParameter<int>(node, "pcd_density_upper_threshold");
Expand Down Expand Up @@ -63,14 +66,14 @@ std::unordered_map<PoseEstimatorType, bool> PcdMapBasedRule::update()
{PoseEstimatorType::eagleye, false},
{PoseEstimatorType::artag, false},
};
} else {
debug_string_ = "Enable yabloc: ";
return {
{PoseEstimatorType::ndt, false},
{PoseEstimatorType::yabloc, true},
{PoseEstimatorType::eagleye, false},
{PoseEstimatorType::artag, false}};
}

debug_string_ = "Enable yabloc: ";
return {
{PoseEstimatorType::ndt, false},
{PoseEstimatorType::yabloc, true},
{PoseEstimatorType::eagleye, false},
{PoseEstimatorType::artag, false}};
}

} // namespace pose_estimator_arbiter::switch_rule
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ class PcdMapBasedRule : public BaseSwitchRule
{
public:
PcdMapBasedRule(
rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
const std::shared_ptr<const SharedData> shared_data);

std::unordered_map<PoseEstimatorType, bool> update() override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,15 @@

#include <magic_enum.hpp>

#include <utility>

namespace pose_estimator_arbiter::switch_rule
{
VectorMapBasedRule::VectorMapBasedRule(
rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
const std::shared_ptr<const SharedData> shared_data)
: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data)
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
std::shared_ptr<const SharedData> shared_data)
: BaseSwitchRule(node), running_estimator_list_(std::move(running_estimator_list)),
shared_data_(std::move(shared_data))
{
pose_estimator_area_ = std::make_unique<rule_helper::PoseEstimatorArea>(node.get_logger());

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ class VectorMapBasedRule : public BaseSwitchRule
{
public:
VectorMapBasedRule(
rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
const std::shared_ptr<const SharedData> shared_data);
rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
std::shared_ptr<const SharedData> shared_data);

std::unordered_map<PoseEstimatorType, bool> update() override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@

#include <unordered_set>

class MockNode : public ::testing::Test
class PcdMapBasedRuleMockNode : public ::testing::Test
{
protected:
virtual void SetUp()
void SetUp() override
{
rclcpp::init(0, nullptr);
node = std::make_shared<rclcpp::Node>("test_node");
Expand All @@ -46,10 +46,10 @@ class MockNode : public ::testing::Test
std::shared_ptr<pose_estimator_arbiter::SharedData> shared_data_;
std::shared_ptr<pose_estimator_arbiter::switch_rule::PcdMapBasedRule> rule_;

virtual void TearDown() { rclcpp::shutdown(); }
void TearDown() override { rclcpp::shutdown(); }
};

TEST_F(MockNode, pcdMapBasedRule)
TEST_F(PcdMapBasedRuleMockNode, pcdMapBasedRule)
{
// Create dummy pcd and set
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,21 +28,21 @@

#include <unordered_set>

class MockNode : public ::testing::Test
class RuleHelperMockNode : public ::testing::Test
{
protected:
virtual void SetUp()
void SetUp() override
{
rclcpp::init(0, nullptr);
node = std::make_shared<rclcpp::Node>("test_node");
}

std::shared_ptr<rclcpp::Node> node{nullptr};

virtual void TearDown() { rclcpp::shutdown(); }
void TearDown() override { rclcpp::shutdown(); }
};

TEST_F(MockNode, poseEstimatorArea)
TEST_F(RuleHelperMockNode, poseEstimatorArea)
{
auto create_polygon3d = []() -> lanelet::Polygon3d {
lanelet::Polygon3d polygon;
Expand Down Expand Up @@ -73,7 +73,7 @@ TEST_F(MockNode, poseEstimatorArea)
EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "yabloc"));
}

TEST_F(MockNode, pcdOccupancy)
TEST_F(RuleHelperMockNode, pcdOccupancy)
{
using pose_estimator_arbiter::rule_helper::PcdOccupancy;
const int pcd_density_upper_threshold = 20;
Expand All @@ -89,7 +89,7 @@ TEST_F(MockNode, pcdOccupancy)
EXPECT_FALSE(pcd_occupancy.ndt_can_operate(point, &message));
}

TEST_F(MockNode, gridKey)
TEST_F(RuleHelperMockNode, gridKey)
{
using pose_estimator_arbiter::rule_helper::GridKey;
EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@

#include <unordered_set>

class MockNode : public ::testing::Test
class VectorMapBasedRuleMockNode : public ::testing::Test
{
protected:
virtual void SetUp()
void SetUp() override
{
rclcpp::init(0, nullptr);
node = std::make_shared<rclcpp::Node>("test_node");
Expand All @@ -49,10 +49,10 @@ class MockNode : public ::testing::Test
std::shared_ptr<pose_estimator_arbiter::SharedData> shared_data_;
std::shared_ptr<pose_estimator_arbiter::switch_rule::VectorMapBasedRule> rule_;

virtual void TearDown() { rclcpp::shutdown(); }
void TearDown() override { rclcpp::shutdown(); }
};

TEST_F(MockNode, vectorMapBasedRule)
TEST_F(VectorMapBasedRuleMockNode, vectorMapBasedRule)
{
// Create dummy lanelet2 and set
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,6 @@
namespace pose_estimator_arbiter
{
enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 };
}
} // namespace pose_estimator_arbiter

#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -30,29 +30,29 @@ namespace pose_estimator_arbiter
template <typename T>
struct CallbackInvokingVariable
{
CallbackInvokingVariable() {}
CallbackInvokingVariable() = default;

explicit CallbackInvokingVariable(T initial_data) : value(initial_data) {}
explicit CallbackInvokingVariable(T initial_data) : value_(initial_data) {}

// Set data and invoke all callbacks
void set_and_invoke(T new_value)
{
value = new_value;
value_ = new_value;

// Call all callbacks with the new value
for (const auto & callback : callbacks) {
callback(value.value());
for (const auto & callback : callbacks_) {
callback(value_.value());
}
}

// Same as std::optional::has_value()
bool has_value() const { return value.has_value(); }
bool has_value() const { return value_.has_value(); }

// Same as std::optional::value()
const T operator()() const { return value.value(); }
T operator()() const { return value_.value(); }

// Register callback function which is invoked when set_and_invoke() is called
void register_callback(std::function<void(T)> callback) const { callbacks.push_back(callback); }
void register_callback(std::function<void(T)> callback) const { callbacks_.push_back(callback); }

// Create subscription callback function which is used as below:
// auto subscriber = create_subscription<T>("topic_name", 10,
Expand All @@ -64,10 +64,10 @@ struct CallbackInvokingVariable

private:
// The latest data
std::optional<T> value{std::nullopt};
std::optional<T> value_{std::nullopt};

// These functions are expected not to change the value variable
mutable std::vector<std::function<void(T)>> callbacks;
mutable std::vector<std::function<void(T)>> callbacks_;
};

// This structure is handed to several modules as shared_ptr so that all modules can access data.
Expand All @@ -80,7 +80,7 @@ struct SharedData
using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;

SharedData() {}
SharedData() = default;

// Used for stoppers
CallbackInvokingVariable<PoseCovStamped::ConstSharedPtr> eagleye_output_pose_cov;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <rclcpp/rclcpp.hpp>

#include <memory>
#include <utility>

namespace pose_estimator_arbiter::stopper
{
Expand All @@ -28,11 +29,16 @@ class BaseStopper
public:
using SharedPtr = std::shared_ptr<BaseStopper>;

explicit BaseStopper(rclcpp::Node * node, const std::shared_ptr<const SharedData> shared_data)
: logger_(node->get_logger()), shared_data_(shared_data)
explicit BaseStopper(rclcpp::Node * node, std::shared_ptr<const SharedData> shared_data)
: logger_(node->get_logger()), shared_data_(std::move(shared_data))
{
}

virtual ~BaseStopper() = default;
BaseStopper(const BaseStopper& other) = default;
BaseStopper(BaseStopper&& other) noexcept = default;
BaseStopper& operator=(const BaseStopper& other) = default;
BaseStopper& operator=(BaseStopper&& other) noexcept = default;
void enable() { set_enable(true); }
void disable() { set_enable(false); }

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ class StopperArTag : public BaseStopper
using SetBool = std_srvs::srv::SetBool;

public:
explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr<const SharedData> shared_data)
: BaseStopper(node, shared_data)
explicit StopperArTag(rclcpp::Node * node,
const std::shared_ptr<const SharedData> & shared_data) : BaseStopper(node, shared_data)
{
ar_tag_is_enabled_ = true;
pub_image_ = node->create_publisher<Image>("~/output/artag/image", rclcpp::SensorDataQoS());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ class StopperEagleye : public BaseStopper
using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped;

public:
explicit StopperEagleye(rclcpp::Node * node, const std::shared_ptr<const SharedData> shared_data)
: BaseStopper(node, shared_data)
explicit StopperEagleye(rclcpp::Node * node,
const std::shared_ptr<const SharedData> & shared_data) : BaseStopper(node, shared_data)
{
eagleye_is_enabled_ = true;
pub_pose_ = node->create_publisher<PoseCovStamped>("~/output/eagleye/pose_with_covariance", 5);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class StopperNdt : public BaseStopper
using PointCloud2 = sensor_msgs::msg::PointCloud2;

public:
explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr<const SharedData> shared_data)
explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr<const SharedData> & shared_data)
: BaseStopper(node, shared_data)
{
ndt_is_enabled_ = true;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ class StopperYabLoc : public BaseStopper
using SetBool = std_srvs::srv::SetBool;

public:
explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr<const SharedData> shared_data)
explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr<const SharedData> & shared_data)
: BaseStopper(node, shared_data)
{
yabloc_is_enabled_ = true;
Expand Down
Loading

0 comments on commit 45c993f

Please sign in to comment.