Skip to content

Commit

Permalink
Uncrustify
Browse files Browse the repository at this point in the history
  • Loading branch information
wico-molina committed Aug 29, 2023
1 parent bdfb190 commit f1387c7
Showing 1 changed file with 18 additions and 18 deletions.
36 changes: 18 additions & 18 deletions mavros/src/plugins/global_position.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,11 +99,11 @@ class GlobalPositionPlugin : public plugin::PluginBase {
Subscriptions get_subscriptions() override
{
return {
make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
// GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
make_handler(&GlobalPositionPlugin::handle_global_position_int),
make_handler(&GlobalPositionPlugin::handle_gps_global_origin),
make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset)
make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
// GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
make_handler(&GlobalPositionPlugin::handle_global_position_int),
make_handler(&GlobalPositionPlugin::handle_gps_global_origin),
make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset)
};
}

Expand Down Expand Up @@ -181,7 +181,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {

// With mavlink v2.0 use accuracies reported by sensor
if (msg->magic == MAVLINK_STX &&
raw_gps.h_acc > 0 && raw_gps.v_acc > 0) {
raw_gps.h_acc > 0 && raw_gps.v_acc > 0) {
gps_cov.diagonal() << std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.h_acc / 1E3, 2), std::pow(raw_gps.v_acc / 1E3, 2);
fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
}
Expand All @@ -199,7 +199,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
raw_fix_pub.publish(fix);

if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = angles::from_degrees(raw_gps.cog / 1E2); // rad

Expand Down Expand Up @@ -306,7 +306,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {

// Linear velocity
tf::vectorEigenToMsg(Eigen::Vector3d(gpos.vy, gpos.vx, gpos.vz) / 1E2,
odom->twist.twist.linear);
odom->twist.twist.linear);

// Velocity covariance unknown
ftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
Expand All @@ -325,7 +325,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
* on Gazebo)
*/
GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::Constants::WGS84_f());

/**
* @brief Checks if the "map" origin is set.
Expand All @@ -336,15 +336,15 @@ class GlobalPositionPlugin : public plugin::PluginBase {
*/
// Current fix to ECEF
map.Forward(fix->latitude, fix->longitude, fix->altitude,
map_point.x(), map_point.y(), map_point.z());
map_point.x(), map_point.y(), map_point.z());

// Set the current fix as the "map" origin if it's not set
if (!is_map_init && fix->status.status >= sensor_msgs::NavSatStatus::STATUS_FIX) {
map_origin.x() = fix->latitude;
map_origin.y() = fix->longitude;
map_origin.z() = fix->altitude;

ecef_origin = map_point; // Local position is zero
ecef_origin = map_point;// Local position is zero
is_map_init = true;
}
}
Expand Down Expand Up @@ -372,9 +372,9 @@ class GlobalPositionPlugin : public plugin::PluginBase {
pos_cov_out.setZero();
pos_cov_out.block<3, 3>(0, 0) = gps_cov;
pos_cov_out.block<3, 3>(3, 3).diagonal() <<
rot_cov,
rot_cov,
rot_cov;
rot_cov,
rot_cov,
rot_cov;

// publish
gp_fix_pub.publish(fix);
Expand Down Expand Up @@ -409,8 +409,8 @@ class GlobalPositionPlugin : public plugin::PluginBase {

auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(offset.x, offset.y, offset.z));
auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
ftf::transform_orientation_ned_enu(
ftf::quaternion_from_rpy(offset.roll, offset.pitch, offset.yaw)));
ftf::transform_orientation_ned_enu(
ftf::quaternion_from_rpy(offset.roll, offset.pitch, offset.yaw)));

tf::pointEigenToMsg(enu_position, global_offset->pose.position);
tf::quaternionEigenToMsg(enu_baselink_orientation, global_offset->pose.orientation);
Expand Down Expand Up @@ -481,11 +481,11 @@ class GlobalPositionPlugin : public plugin::PluginBase {
* @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed)
*/
GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::Constants::WGS84_f());

// map_origin to ECEF
map.Forward(map_origin.x(), map_origin.y(), map_origin.z(),
ecef_origin.x(), ecef_origin.y(), ecef_origin.z());
ecef_origin.x(), ecef_origin.y(), ecef_origin.z());
}
catch (const std::exception& e) {
ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);
Expand Down

0 comments on commit f1387c7

Please sign in to comment.