Skip to content

Commit

Permalink
refactor(ndt scan matcher): apply static analysis revert (#7398)
Browse files Browse the repository at this point in the history 
Revert "refactor(ndt scan matcher): apply static analysis (#7278)"

This reverts commit 3b90dc0.
  • Loading branch information
@YamatoAndo @KhalilSelyan
YamatoAndo authored and KhalilSelyan committed Jul 22, 2024
1 parent bb5fbc2 commit 3f03945
Show file tree
Hide file tree
Showing 7 changed files with 145 additions and 150 deletions.
16 changes: 8 additions & 8 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,14 @@ class DiagnosticsModule
public:
DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
void clear();
void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg);
void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg);
template <typename T>
void add_key_value(const std::string & key, const T & value);
void update_level_and_message(const int8_t level, const std::string & message);
void addKeyValue(const std::string & key, const T & value);
void updateLevelAndMessage(const int8_t level, const std::string & message);
void publish(const rclcpp::Time & publish_time_stamp);

private:
[[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array(
diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray(
const rclcpp::Time & publish_time_stamp) const;

rclcpp::Clock::SharedPtr clock_;
Expand All @@ -44,17 +44,17 @@ class DiagnosticsModule
};

template <typename T>
void DiagnosticsModule::add_key_value(const std::string & key, const T & value)
void DiagnosticsModule::addKeyValue(const std::string & key, const T & value)
{
diagnostic_msgs::msg::KeyValue key_value;
key_value.key = key;
key_value.value = std::to_string(value);
add_key_value(key_value);
addKeyValue(key_value);
}

template <>
void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value);
void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value);
template <>
void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value);

#endif // NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_
64 changes: 32 additions & 32 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,62 +33,62 @@ struct HyperParameters
{
struct Frame
{
std::string base_frame{};
std::string ndt_base_frame{};
std::string map_frame{};
} frame{};
std::string base_frame;
std::string ndt_base_frame;
std::string map_frame;
} frame;

struct SensorPoints
{
double required_distance{};
} sensor_points{};
double required_distance;
} sensor_points;

pclomp::NdtParams ndt{};
bool ndt_regularization_enable{};
pclomp::NdtParams ndt;
bool ndt_regularization_enable;

struct InitialPoseEstimation
{
int64_t particles_num{};
int64_t n_startup_trials{};
} initial_pose_estimation{};
int64_t particles_num;
int64_t n_startup_trials;
} initial_pose_estimation;

struct Validation
{
double lidar_topic_timeout_sec{};
double initial_pose_timeout_sec{};
double initial_pose_distance_tolerance_m{};
double critical_upper_bound_exe_time_ms{};
} validation{};
double lidar_topic_timeout_sec;
double initial_pose_timeout_sec;
double initial_pose_distance_tolerance_m;
double critical_upper_bound_exe_time_ms;
} validation;

struct ScoreEstimation
{
ConvergedParamType converged_param_type{};
double converged_param_transform_probability{};
double converged_param_nearest_voxel_transformation_likelihood{};
ConvergedParamType converged_param_type;
double converged_param_transform_probability;
double converged_param_nearest_voxel_transformation_likelihood;
struct NoGroundPoints
{
bool enable{};
double z_margin_for_ground_removal{};
} no_ground_points{};
} score_estimation{};
bool enable;
double z_margin_for_ground_removal;
} no_ground_points;
} score_estimation;

struct Covariance
{
std::array<double, 36> output_pose_covariance{};
std::array<double, 36> output_pose_covariance;

struct CovarianceEstimation
{
bool enable{};
std::vector<Eigen::Vector2d> initial_pose_offset_model{};
} covariance_estimation{};
} covariance{};
bool enable;
std::vector<Eigen::Vector2d> initial_pose_offset_model;
} covariance_estimation;
} covariance;

struct DynamicMapLoading
{
double update_distance{};
double map_radius{};
double lidar_radius{};
} dynamic_map_loading{};
double update_distance;
double map_radius;
double lidar_radius;
} dynamic_map_loading;

public:
explicit HyperParameters(rclcpp::Node * node)
Expand Down
2 changes: 2 additions & 0 deletions localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,8 @@ class NDTScanMatcher : public rclcpp::Node

static int count_oscillation(const std::vector<geometry_msgs::msg::Pose> & result_pose_msg_array);

std::array<double, 36> rotate_covariance(
const std::array<double, 36> & src_covariance, const Eigen::Matrix3d & rotation) const;
std::array<double, 36> estimate_covariance(
const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
const rclcpp::Time & sensor_ros_time);
Expand Down
16 changes: 8 additions & 8 deletions localization/ndt_scan_matcher/src/diagnostics_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ void DiagnosticsModule::clear()
diagnostics_status_msg_.message = "";
}

void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg)
void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg)
{
auto it = std::find_if(
std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
Expand All @@ -55,24 +55,24 @@ void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key
}

template <>
void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value)
void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value)
{
diagnostic_msgs::msg::KeyValue key_value;
key_value.key = key;
key_value.value = value;
add_key_value(key_value);
addKeyValue(key_value);
}

template <>
void DiagnosticsModule::add_key_value(const std::string & key, const bool & value)
void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value)
{
diagnostic_msgs::msg::KeyValue key_value;
key_value.key = key;
key_value.value = value ? "True" : "False";
add_key_value(key_value);
addKeyValue(key_value);
}

void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message)
void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message)
{
if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
if (!diagnostics_status_msg_.message.empty()) {
Expand All @@ -87,10 +87,10 @@ void DiagnosticsModule::update_level_and_message(const int8_t level, const std::

void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
{
diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp));
diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp));
}

diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array(
diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray(
const rclcpp::Time & publish_time_stamp) const
{
diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
Expand Down
42 changes: 21 additions & 21 deletions localization/ndt_scan_matcher/src/map_update_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,23 +53,23 @@ void MapUpdateModule::callback_timer(
std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
{
// check is_activated
diagnostics_ptr->add_key_value("is_activated", is_activated);
diagnostics_ptr->addKeyValue("is_activated", is_activated);
if (!is_activated) {
std::stringstream message;
message << "Node is not activated.";
diagnostics_ptr->update_level_and_message(
diagnostics_ptr->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return;
}

// check is_set_last_update_position
const bool is_set_last_update_position = (position != std::nullopt);
diagnostics_ptr->add_key_value("is_set_last_update_position", is_set_last_update_position);
diagnostics_ptr->addKeyValue("is_set_last_update_position", is_set_last_update_position);
if (!is_set_last_update_position) {
std::stringstream message;
message << "Cannot find the reference position for map update."
<< "Please check if the EKF odometry is provided to NDT.";
diagnostics_ptr->update_level_and_message(
diagnostics_ptr->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return;
}
Expand All @@ -92,11 +92,11 @@ bool MapUpdateModule::should_update_map(
const double distance = std::hypot(dx, dy);

// check distance_last_update_position_to_current_position
diagnostics_ptr->add_key_value("distance_last_update_position_to_current_position", distance);
diagnostics_ptr->addKeyValue("distance_last_update_position_to_current_position", distance);
if (distance + param_.lidar_radius > param_.map_radius) {
std::stringstream message;
message << "Dynamic map loading is not keeping up.";
diagnostics_ptr->update_level_and_message(
diagnostics_ptr->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());

// If the map does not keep up with the current position,
Expand All @@ -110,7 +110,7 @@ bool MapUpdateModule::should_update_map(
void MapUpdateModule::update_map(
const geometry_msgs::msg::Point & position, std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
{
diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_);
diagnostics_ptr->addKeyValue("is_need_rebuild", need_rebuild_);

// If the current position is super far from the previous loading position,
// lock and rebuild ndt_ptr_
Expand All @@ -130,14 +130,14 @@ void MapUpdateModule::update_map(
const bool updated = update_ndt(position, *ndt_ptr_, diagnostics_ptr);

// check is_updated_map
diagnostics_ptr->add_key_value("is_updated_map", updated);
diagnostics_ptr->addKeyValue("is_updated_map", updated);
if (!updated) {
std::stringstream message;
message
<< "update_ndt failed. If this happens with initial position estimation, make sure that"
<< "(1) the initial position matches the pcd map and (2) the map_loader is working "
"properly.";
diagnostics_ptr->update_level_and_message(
diagnostics_ptr->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str());
last_update_position_ = position;
Expand All @@ -157,7 +157,7 @@ void MapUpdateModule::update_map(
const bool updated = update_ndt(position, *secondary_ndt_ptr_, diagnostics_ptr);

// check is_updated_map
diagnostics_ptr->add_key_value("is_updated_map", updated);
diagnostics_ptr->addKeyValue("is_updated_map", updated);
if (!updated) {
last_update_position_ = position;
return;
Expand Down Expand Up @@ -189,7 +189,7 @@ bool MapUpdateModule::update_ndt(
const geometry_msgs::msg::Point & position, NdtType & ndt,
std::unique_ptr<DiagnosticsModule> & diagnostics_ptr)
{
diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size());
diagnostics_ptr->addKeyValue("maps_size_before", ndt.getCurrentMapIDs().size());

auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();

Expand All @@ -199,11 +199,11 @@ bool MapUpdateModule::update_ndt(
request->cached_ids = ndt.getCurrentMapIDs();

while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false);
diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false);

std::stringstream message;
message << "Waiting for pcd loader service. Check the pointcloud_map_loader.";
diagnostics_ptr->update_level_and_message(
diagnostics_ptr->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false;
}
Expand All @@ -217,23 +217,23 @@ bool MapUpdateModule::update_ndt(
while (status != std::future_status::ready) {
// check is_succeed_call_pcd_loader
if (!rclcpp::ok()) {
diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false);
diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false);

std::stringstream message;
message << "pcd_loader service is not working.";
diagnostics_ptr->update_level_and_message(
diagnostics_ptr->updateLevelAndMessage(
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
return false; // No update
}
status = result.wait_for(std::chrono::seconds(1));
}
diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true);
diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true);

auto & maps_to_add = result.get()->new_pointcloud_with_ids;
auto & map_ids_to_remove = result.get()->ids_to_remove;

diagnostics_ptr->add_key_value("maps_to_add_size", maps_to_add.size());
diagnostics_ptr->add_key_value("maps_to_remove_size", map_ids_to_remove.size());
diagnostics_ptr->addKeyValue("maps_to_add_size", maps_to_add.size());
diagnostics_ptr->addKeyValue("maps_to_remove_size", map_ids_to_remove.size());

if (maps_to_add.empty() && map_ids_to_remove.empty()) {
return false; // No update
Expand Down Expand Up @@ -261,9 +261,9 @@ bool MapUpdateModule::update_ndt(
const auto duration_micro_sec =
std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
diagnostics_ptr->add_key_value("map_update_execution_time", exe_time);
diagnostics_ptr->add_key_value("maps_size_after", ndt.getCurrentMapIDs().size());
diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true);
diagnostics_ptr->addKeyValue("map_update_execution_time", exe_time);
diagnostics_ptr->addKeyValue("maps_size_after", ndt.getCurrentMapIDs().size());
diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true);
return true; // Updated
}

Expand Down
Loading

0 comments on commit 3f03945

Please sign in to comment.