Skip to content

Commit

Permalink
Fix linter errors
Browse files Browse the repository at this point in the history
Signed-off-by: Fernando <sanz.fernando172@gmail.com>
  • Loading branch information
Fernando-Sanz committed Jan 10, 2025
1 parent 3c0516e commit 6421a3f
Show file tree
Hide file tree
Showing 3 changed files with 12 additions and 22 deletions.
5 changes: 1 addition & 4 deletions beluga_ros/cmake/ament.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,7 @@ find_package(tf2_geometry_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)

add_library(beluga_ros)
target_sources(
beluga_ros
PRIVATE src/amcl.cpp
src/ndt_ellipsoid.cpp)
target_sources(beluga_ros PRIVATE src/amcl.cpp src/ndt_ellipsoid.cpp)
target_include_directories(
beluga_ros PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
Expand Down
9 changes: 3 additions & 6 deletions beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,8 @@

#include <range/v3/view/enumerate.hpp>

#include <beluga/sensor/data/sparse_value_grid.hpp>
#include <beluga/sensor/data/ndt_cell.hpp>
#include <beluga/sensor/data/sparse_value_grid.hpp>

/**
* \file
Expand All @@ -37,10 +37,7 @@ bool use_mean_covariance(
const beluga::NDTCell<3> cell,
beluga_ros::msg::Marker& marker);

void use_cell_size(
const Eigen::Vector<int, 3>& position,
double size,
beluga_ros::msg::Marker& marker);
void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ros::msg::Marker& marker);

/// Assign an ellipsoid to each cell of a SparseValueGrid. A cube is used instead if the distribution of the
/// cell is not suitable for the rotation matrix creation.
Expand Down Expand Up @@ -74,7 +71,7 @@ beluga_ros::msg::MarkerArray assign_obstacle_map(
const auto& [cellCenter, cell] = entry;
beluga_ros::msg::Marker marker;
marker.header.frame_id = "map";
marker.id = index+1;
marker.id = index + 1;
marker.ns = "obstacles";

// Try to create an ellipsoid with values of the cell
Expand Down
20 changes: 8 additions & 12 deletions beluga_ros/src/ndt_ellipsoid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,17 +26,17 @@ inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTra
return std::abs(x - y) < prec;
}

}
} // namespace

bool use_mean_covariance(
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigenSolver,
const beluga::NDTCell<3> cell,
beluga_ros::msg::Marker& marker){
beluga_ros::msg::Marker& marker) {
eigenSolver.compute(cell.covariance);
if(eigenSolver.info() != Eigen::Success){
if (eigenSolver.info() != Eigen::Success) {
return false;
}

// Compute the rotation based on the covariance matrix
const auto eigenvectors = eigenSolver.eigenvectors().real();
const auto eigenvalues = eigenSolver.eigenvalues();
Expand All @@ -46,12 +46,12 @@ bool use_mean_covariance(
rotationMatrix << eigenvectors.col(0), eigenvectors.col(1), eigenvectors.col(2);
scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.y(), eigenvalues.z()};

if(!isApprox(std::abs(rotationMatrix.determinant()), 1.0)) {
if (!isApprox(std::abs(rotationMatrix.determinant()), 1.0)) {
return false;
}

// Permute columns to ensure the rotation is in fact a rotation
if(rotationMatrix.determinant() < 0) {
if (rotationMatrix.determinant() < 0) {
rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2);
scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()};
}
Expand Down Expand Up @@ -84,10 +84,7 @@ bool use_mean_covariance(
return true;
}

void use_cell_size(
const Eigen::Vector<int, 3>& position,
double size,
beluga_ros::msg::Marker& marker){
void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ros::msg::Marker& marker) {
marker.type = beluga_ros::msg::Marker::CUBE;
marker.action = beluga_ros::msg::Marker::ADD;

Expand All @@ -105,5 +102,4 @@ void use_cell_size(
marker.color.a = 1.0f;
}


} // namespace beluga_ros

0 comments on commit 6421a3f

Please sign in to comment.