From 1a2424d26e8169b34b040045bf16e58fc4f451a9 Mon Sep 17 00:00:00 2001
From: artzha <arthurz@cs.utexas.edu>
Date: Tue, 3 Dec 2024 10:29:32 -0600
Subject: [PATCH] Added gps localization setter and corrected gps angle
 convention

---
 src/navigation/navigation.cc      |  8 +++++---
 src/navigation/navigation.h       |  1 +
 src/navigation/navigation_main.cc | 14 ++++++++++++++
 src/shared                        |  2 +-
 4 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
index b42c79d..da96731 100644
--- a/src/navigation/navigation.cc
+++ b/src/navigation/navigation.cc
@@ -503,7 +503,7 @@ void Navigation::UpdateGPS(const GPSPoint& msg) {
 
   // Global coords relative to initial GPS location, heading is absolute
   robot_loc_ = gpsToGlobalCoord(initial_gps_loc_, robot_gps_loc_).cast<float>();
-  robot_angle_ = robot_gps_loc_.heading;  // degrees
+  robot_angle_ = static_cast<float>(gpsToGlobalHeading(robot_gps_loc_));
 
   if (FLAGS_v > 2) {
     printf("GPS: %lf %lf\n", msg.lat, msg.lon);
@@ -1325,6 +1325,10 @@ float Navigation::GetCarrotDist() { return params_.carrot_dist; }
 
 float Navigation::GetObstacleMargin() { return params_.obstacle_margin; }
 
+Eigen::Vector3f Navigation::GetRobotPose() {
+  return Eigen::Vector3f(robot_loc_.x(), robot_loc_.y(), robot_angle_);
+}
+
 float Navigation::GetRobotWidth() { return params_.robot_width; }
 
 float Navigation::GetRobotLength() { return params_.robot_length; }
@@ -1731,8 +1735,6 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
       }
       // Local Navigation (Convert global to local coordinates)
       local_target_ = Rotation2Df(-robot_angle_) * (carrot - robot_loc_);
-      // Swap x=y y=-x
-      // local_target_ = Vector2f(local_target_.y(), -local_target_.x());
       printf("Local target %f %f\n", local_target_.x(), local_target_.y());
     }
   }
diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h
index 93f56c4..f516ac7 100644
--- a/src/navigation/navigation.h
+++ b/src/navigation/navigation.h
@@ -194,6 +194,7 @@ class Navigation {
   std::vector<Eigen::Vector2f> GetPredictedCloud();
   float GetCarrotDist();
   float GetObstacleMargin();
+  Eigen::Vector3f GetRobotPose();
   float GetRobotWidth();
   float GetRobotLength();
   const cv::Mat& GetVisualizationImage();
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index d6311e2..8899432 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -85,6 +85,7 @@ using amrl_msgs::GPSMsg;
 using amrl_msgs::graphNavGPSSrv;
 using amrl_msgs::NavStatusMsg;
 using amrl_msgs::VisualizationMsg;
+using amrl_msgs::Localization2DMsg;
 using Eigen::Affine3f;
 using Eigen::Vector2f;
 using Eigen::Vector3f;
@@ -172,6 +173,7 @@ ros::Publisher fp_pcl_pub_;
 ros::Publisher path_pub_;
 ros::Publisher carrot_pub_;
 ros::Publisher next_gps_goal_pub_;
+ros::Publisher localization_pub_;
 image_transport::Publisher viz_img_pub_;
 
 // Messages
@@ -529,6 +531,16 @@ nav_msgs::Path CarrotToNavMsgsPath(const Vector2f& carrot) {
   return carrotNav;
 }
 
+void PublishLocalization() {
+  // Publishes robot pose
+  Localization2DMsg loc_msg;
+  loc_msg.header.stamp = ros::Time::now();
+  loc_msg.pose.x = navigation_.GetRobotPose().x();
+  loc_msg.pose.y = navigation_.GetRobotPose().y();
+  loc_msg.pose.theta = navigation_.GetRobotPose().z(); // theta
+  pose_marker_publisher_.publish(loc_msg);
+}
+
 void PublishPath() {
   const auto path = navigation_.GetPlanPath();
   if (path.size() >= 2) {
@@ -1010,6 +1022,7 @@ int main(int argc, char** argv) {
   carrot_pub_ = n.advertise<nav_msgs::Path>("carrot", 1, true);
   next_gps_goal_pub_ = n.advertise<GPSMsg>(
       "next_gps_goal", 1, true);  // Only publish if there is a goal
+  localization_pub_ = n.advertise<Localization2DMsg>("localization", 1);
 
   // Messages
   local_viz_msg_ =
@@ -1102,6 +1115,7 @@ int main(int argc, char** argv) {
         DrawTarget();
         DrawPathOptions();
       }
+      PublishLocalization();
       PublishVisualizationMarkers();
       PublishPath();
       PublishNextGPSGoal();
diff --git a/src/shared b/src/shared
index f138093..3850fd9 160000
--- a/src/shared
+++ b/src/shared
@@ -1 +1 @@
-Subproject commit f1380937bad222aef6f978cee2e465debfb039d1
+Subproject commit 3850fd9862dfa5cc07ba733b26df1f919096f2a2