adding reset nav and cleaning viz after robot stops

ut-amrl · Nov 3, 2023 · 123a0e2 · 123a0e2
1 parent e54b078
commit 123a0e2
Show file tree

Hide file tree

Showing 3 changed files with 22 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
@@ -225,13 +225,23 @@ void GoToCallback(const geometry_msgs::PoseStamped& msg) {
   const Vector2f loc(msg.pose.position.x, msg.pose.position.y);
   const float angle =
       2.0 * atan2(msg.pose.orientation.z, msg.pose.orientation.w);
+  if (loc.x() == 1111.0 && loc.y() == 1111.0) {
+    printf("Resetting all nav goals.\n");
+    navigation_.ResetNavGoals();
+    return;
+  }
   printf("Goal: (%f,%f) %f\u00b0\n", loc.x(), loc.y(), angle);
   navigation_.SetNavGoal(loc, angle);
   navigation_.Resume();
 }
 
 void GoToCallbackAMRL(const amrl_msgs::Localization2DMsg& msg) {
   const Vector2f loc(msg.pose.x, msg.pose.y);
+  if (loc.x() == 1111.0 && loc.y() == 1111.0 && msg.pose.theta == 1111.0) {
+    printf("Resetting all nav goals.\n");
+    navigation_.ResetNavGoals();
+    return;
+  }
   printf("Goal: (%f,%f) %f\u00b0\n", loc.x(), loc.y(), msg.pose.theta);
   navigation_.SetNavGoal(loc, msg.pose.theta);
   navigation_.Resume();
@@ -870,8 +880,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();