reset added

ut-amrl · Oct 30, 2023 · 838f06f · 838f06f
1 parent e54b078
commit 838f06f
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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();