Use AirLib GeodeticConverter in pd_position_controller

microsoft · Dec 10, 2021 · 0364c2d · 0364c2d
1 parent c9ea070
commit 0364c2d
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 14 deletions.
diff --git a/ros/src/airsim_ros_pkgs/CMakeLists.txt b/ros/src/airsim_ros_pkgs/CMakeLists.txt
@@ -9,7 +9,7 @@ add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirLib)
 add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom)
 
 set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wnoexcept -Wstrict-null-sentinel -Wno-unused")
+set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wpedantic -Wstrict-null-sentinel -Wno-unused")
 set(CXX_EXP_LIB "-nostdinc++ -I/usr/include/c++/8 -I/usr/include/x86_64-linux-gnu/c++/8 -nodefaultlibs 
 -l/usr/lib/x86_64-linux-gnu/libc++.so -l/usr/lib/x86_64-linux-gnu/libc++abi.so
 -lm -lc -lgcc_s -lgcc  

diff --git a/ros/src/airsim_ros_pkgs/include/pd_position_controller_simple.h b/ros/src/airsim_ros_pkgs/include/pd_position_controller_simple.h
@@ -11,6 +11,7 @@ STRICT_MODE_OFF //todo what does this do?
 
 #include "common/common_utils/FileSystem.hpp"
 #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
+#include "common/GeodeticConverter.hpp"
 
 #include <ros/ros.h>
 #include <tf2/LinearMath/Matrix3x3.h>
@@ -22,7 +23,6 @@ STRICT_MODE_OFF //todo what does this do?
 #include <airsim_ros_pkgs/SetLocalPosition.h>
 #include <airsim_ros_pkgs/SetGPSPosition.h>
 #include <airsim_ros_pkgs/GPSYaw.h>
-#include <geodetic_conv.hpp>
 #include <math_common.h>
 #include <utils.h>
 
@@ -101,7 +101,7 @@ class PIDPositionController
     void check_reached_goal();
 
 private:
-    geodetic_converter::GeodeticConverter geodetic_converter_;
+    GeodeticConverter geodetic_converter_;
     const bool use_eth_lib_for_geodetic_conv_;
 
     ros::NodeHandle nh_;

diff --git a/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp b/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp
@@ -161,7 +161,7 @@ void PIDPositionController::home_geopoint_cb(const airsim_ros_pkgs::GPSYaw& gps_
     gps_home_msg_ = gps_msg;
     has_home_geo_ = true;
     ROS_INFO_STREAM("[PIDPositionController] GPS reference initializing " << gps_msg.latitude << ", " << gps_msg.longitude << ", " << gps_msg.altitude);
-    geodetic_converter_.initialiseReference(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude);
+    geodetic_converter_.setHome(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude);
 }
 
 // todo do relative altitude, or add an option for the same?
@@ -178,19 +178,19 @@ bool PIDPositionController::gps_goal_srv_cb(airsim_ros_pkgs::SetGPSPosition::Req
         msr::airlib::GeoPoint goal_gps_point(request.latitude, request.longitude, request.altitude);
         msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude);
         if (use_eth_lib_for_geodetic_conv_) {
-            double initial_latitude, initial_longitude, initial_altitude;
-            geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude);
+            // double initial_latitude, initial_longitude;
+            // float initial_altitude;
+            // geodetic_converter_.getHome(&initial_latitude, &initial_longitude, &initial_altitude);
+            // ROS_INFO_STREAM("[PIDPositionController] geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude);
             double n, e, d;
             geodetic_converter_.geodetic2Ned(request.latitude, request.longitude, request.altitude, &n, &e, &d);
-            // ROS_INFO_STREAM("[PIDPositionController] geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude);
             target_position_.x = n;
             target_position_.y = e;
             target_position_.z = d;
         }
         else // use airlib::GeodeticToNedFast
         {
-            ROS_INFO_STREAM("[PIDPositionController] home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw="
-                                                                          << "todo");
+            ROS_INFO_STREAM("[PIDPositionController] home geopoint: " << gps_home);
             msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home);
             target_position_.x = ned_goal[0];
             target_position_.y = ned_goal[1];
@@ -227,19 +227,19 @@ bool PIDPositionController::gps_goal_srv_override_cb(airsim_ros_pkgs::SetGPSPosi
     msr::airlib::GeoPoint goal_gps_point(request.latitude, request.longitude, request.altitude);
     msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude);
     if (use_eth_lib_for_geodetic_conv_) {
-        double initial_latitude, initial_longitude, initial_altitude;
-        geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude);
+        // double initial_latitude, initial_longitude;
+        // float initial_altitude;
+        // geodetic_converter_.getHome(&initial_latitude, &initial_longitude, &initial_altitude);
+        // ROS_INFO_STREAM("[PIDPositionController] geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude);
         double n, e, d;
         geodetic_converter_.geodetic2Ned(request.latitude, request.longitude, request.altitude, &n, &e, &d);
-        // ROS_INFO_STREAM("[PIDPositionController] geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude);
         target_position_.x = n;
         target_position_.y = e;
         target_position_.z = d;
     }
     else // use airlib::GeodeticToNedFast
     {
-        ROS_INFO_STREAM("[PIDPositionController] home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw="
-                                                                      << "todo");
+        ROS_INFO_STREAM("[PIDPositionController] home geopoint: " << gps_home);
         msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home);
         target_position_.x = ned_goal[0];
         target_position_.y = ned_goal[1];