Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Apr 18, 2022
1 parent c7dd0f2 commit 54ebb07
Show file tree
Hide file tree
Showing 3 changed files with 18 additions and 15 deletions.
14 changes: 8 additions & 6 deletions sensing/gnss_poser/include/gnss_poser/convert.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "gnss_poser/gnss_stat.hpp"

#include <GeographicLib/Geoid.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <GeographicLib/MGRS.hpp>
#include <GeographicLib/UTMUPS.hpp>
#include <geo_pos_conv/geo_pos_conv.hpp>
Expand All @@ -25,7 +26,6 @@
#include <sensor_msgs/msg/nav_sat_fix.hpp>

#include <string>
#include <GeographicLib/LocalCartesian.hpp>
namespace gnss_poser
{
enum class MGRSPrecision {
Expand Down Expand Up @@ -58,7 +58,8 @@ double EllipsoidHeight2OrthometricHeight(
}

GNSSStat NavSatFix2UTM(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, UtmProjectorType utm_projector_type,sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger)
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, UtmProjectorType utm_projector_type,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger)
{
GNSSStat utm;
utm.coordinate_system = CoordinateSystem::UTM;
Expand All @@ -79,8 +80,8 @@ GNSSStat NavSatFix2UTM(
utm.longitude = nav_sat_fix_msg.longitude;
utm.altitude = nav_sat_fix_msg.altitude;
} catch (const GeographicLib::GeographicErr & err) {
RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what());
}
RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what());
}
return utm;
}

Expand Down Expand Up @@ -113,10 +114,11 @@ GNSSStat UTM2MGRS(
}

GNSSStat NavSatFix2MGRS(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, UtmProjectorType & utm_projector_type, sensor_msgs::msg::NavSatFix & nav_sat_fix_origin, const MGRSPrecision & precision,
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, UtmProjectorType & utm_projector_type,
sensor_msgs::msg::NavSatFix & nav_sat_fix_origin, const MGRSPrecision & precision,
const rclcpp::Logger & logger)
{
const auto utm = NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type,nav_sat_fix_origin, logger);
const auto utm = NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type, nav_sat_fix_origin, logger);
const auto mgrs = UTM2MGRS(utm, precision, logger);
return mgrs;
}
Expand Down
2 changes: 1 addition & 1 deletion sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ enum class CoordinateSystem {
PLANE = 2,
};

enum class UtmProjectorType{
enum class UtmProjectorType {
UTMUPS = 0,
LocalCartesian = 1,
};
Expand Down
17 changes: 9 additions & 8 deletions sensing/gnss_poser/src/gnss_poser_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,12 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)

coordinate_system_ = static_cast<CoordinateSystem>(coordinate_system);

int utm_projector_type_ = declare_parameter("utm_projector_type",static_cast<int>(UtmProjectorType::LocalCartesian));
int utm_projector_type_ =
declare_parameter("utm_projector_type", static_cast<int>(UtmProjectorType::LocalCartesian));
utm_projector_type = static_cast<UtmProjectorType>(utm_projector_type_);
nav_sat_fix_origin.latitude = declare_parameter("latitude", 0.0);
nav_sat_fix_origin.latitude = declare_parameter("latitude", 0.0);
nav_sat_fix_origin.longitude = declare_parameter("longitude", 0.0);
nav_sat_fix_origin.altitude = declare_parameter("altitude", 0.0);
nav_sat_fix_origin.altitude = declare_parameter("altitude", 0.0);

int buff_epoch = declare_parameter("buff_epoch", 1);
position_buffer_.set_capacity(buff_epoch);
Expand Down Expand Up @@ -166,14 +167,14 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix
GNSSStat GNSSPoser::convert(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system)
{



GNSSStat gnss_stat;
if (coordinate_system == CoordinateSystem::UTM) {
gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type,nav_sat_fix_origin, this->get_logger());
gnss_stat =
NavSatFix2UTM(nav_sat_fix_msg, utm_projector_type, nav_sat_fix_origin, this->get_logger());
} else if (coordinate_system == CoordinateSystem::MGRS) {
gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, utm_projector_type, nav_sat_fix_origin, MGRSPrecision::_100MICRO_METER, this->get_logger());
gnss_stat = NavSatFix2MGRS(
nav_sat_fix_msg, utm_projector_type, nav_sat_fix_origin, MGRSPrecision::_100MICRO_METER,
this->get_logger());
} else if (coordinate_system == CoordinateSystem::PLANE) {
gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger());
} else {
Expand Down

0 comments on commit 54ebb07

Please sign in to comment.