From 838f06f6abda2e293eda70e417e4c2e5eef7da7d Mon Sep 17 00:00:00 2001
From: Sadanand Modak <sadanandm@utexas.edu>
Date: Mon, 30 Oct 2023 02:41:58 -0500
Subject: [PATCH] reset added

---
 src/navigation/navigation.cc      |  7 +++++++
 src/navigation/navigation.h       |  1 +
 src/navigation/navigation_main.cc | 13 +++++++++++--
 3 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
index 5667df2..7dd2c2a 100644
--- a/src/navigation/navigation.cc
+++ b/src/navigation/navigation.cc
@@ -189,6 +189,13 @@ void Navigation::SetNavGoal(const Vector2f& loc, float angle) {
   plan_path_.clear();
 }
 
+void Navigation::ResetNavGoals() {
+  nav_state_ = NavigationState::kStopped;
+  nav_goal_loc_ = robot_loc_;
+  nav_goal_angle_ = robot_angle_;
+  local_target_.setZero();
+  plan_path_.clear();
+}
 
 void Navigation::SetOverride(const Vector2f& loc, float angle) {
   nav_state_ = NavigationState::kOverride;
diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h
index af465e0..d1600d0 100644
--- a/src/navigation/navigation.h
+++ b/src/navigation/navigation.h
@@ -98,6 +98,7 @@ class Navigation {
                          float* clearance,
                          Eigen::Vector2f* obstruction);
   void SetNavGoal(const Eigen::Vector2f& loc, float angle);
+  void ResetNavGoals();
   void SetOverride(const Eigen::Vector2f& loc, float angle);
   void Resume();
   bool PlanStillValid();
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index 95ed9a3..d49bd7a 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -65,6 +65,7 @@
 #include "shared/util/helpers.h"
 #include "shared/ros/ros_helpers.h"
 #include "std_msgs/Bool.h"
+#include "std_msgs/Empty.h"
 #include "tf/transform_broadcaster.h"
 #include "tf/transform_datatypes.h"
 #include "visualization/visualization.h"
@@ -237,6 +238,10 @@ void GoToCallbackAMRL(const amrl_msgs::Localization2DMsg& msg) {
   navigation_.Resume();
 }
 
+void ResetNavGoalsCallback(const std_msgs::Empty& msg) {
+  navigation_.ResetNavGoals();
+}
+
 bool PlanServiceCb(graphNavSrv::Request &req,
                  graphNavSrv::Response &res) {
   const Vector2f start(req.start.x, req.start.y);
@@ -838,6 +843,8 @@ int main(int argc, char** argv) {
       n.subscribe("/move_base_simple/goal", 1, &GoToCallback);
   ros::Subscriber goto_amrl_sub =
       n.subscribe("/move_base_simple/goal_amrl", 1, &GoToCallbackAMRL);
+  ros::Subscriber reset_nav_goals_sub =
+      n.subscribe("/reset_nav_goals", 1, &ResetNavGoalsCallback);
   ros::Subscriber enabler_sub =
       n.subscribe(CONFIG_enable_topic, 1, &EnablerCallback);
   ros::Subscriber halt_sub =
@@ -870,8 +877,10 @@ int main(int argc, char** argv) {
       // Publish Visualizations
       PublishForwardPredictedPCL(navigation_.GetPredictedCloud());
       DrawRobot();
-      DrawTarget();
-      DrawPathOptions();
+      if (navigation_.GetNavStatusUint8() != static_cast<uint8_t>(navigation::NavigationState::kStopped)) {
+        DrawTarget();
+        DrawPathOptions();
+      }
       PublishVisualizationMarkers();
       PublishPath();
       local_viz_msg_.header.stamp = ros::Time::now();