add a separate reset signal

ut-amrl · Nov 3, 2023 · 0390fc5 · 0390fc5
1 parent db51966
commit 0390fc5
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 2 deletions.
diff --git a/src/websocket/websocket.cc b/src/websocket/websocket.cc
@@ -348,6 +348,8 @@ void RobotWebSocket::ProcessCallback(const QJsonObject& json) {
                      json.value("y").toDouble(),
                      json.value("theta").toDouble(),
                      json.value("map").toString());
+  } else if (type == "reset_nav_goals") {
+    ResetNavGoalsSignal();
   } else {
     SendError("Unrecognized request type");
   }

diff --git a/src/websocket/websocket.h b/src/websocket/websocket.h
@@ -107,6 +107,7 @@ class RobotWebSocket : public QObject {
   void SendDataSignal();
   void SetInitialPoseSignal(float x, float y, float theta, QString map);
   void SetNavGoalSignal(float x, float y, float theta, QString map);
+  void ResetNavGoalsSignal();
 
 private Q_SLOTS:
   void onNewConnection();

diff --git a/src/websocket/websocket_main.cc b/src/websocket/websocket_main.cc
@@ -24,6 +24,7 @@
 
 #include "geometry_msgs/PoseStamped.h"
 #include "geometry_msgs/PoseWithCovarianceStamped.h"
+#include "std_msgs/Empty.h"
 #include "gflags/gflags.h"
 #include "glog/logging.h"
 #include "ros/ros.h"
@@ -51,12 +52,14 @@ geometry_msgs::PoseWithCovarianceStamped initial_pose_msg_;
 geometry_msgs::PoseStamped nav_goal_msg_;
 amrl_msgs::Localization2DMsg amrl_initial_pose_msg_;
 amrl_msgs::Localization2DMsg amrl_nav_goal_msg_;
+std_msgs::Empty reset_nav_goals_msg_;
 Localization2DMsg localization_msg_;
 LaserScan laser_scan_;
 ros::Publisher init_loc_pub_;
 ros::Publisher amrl_init_loc_pub_;
 ros::Publisher nav_goal_pub_;
 ros::Publisher amrl_nav_goal_pub_;
+ros::Publisher reset_nav_goals_pub_;
 bool updates_pending_ = false;
 RobotWebSocket *server_ = nullptr;
 }  // namespace
@@ -169,6 +172,13 @@ void SetInitialPose(float x, float y, float theta, QString map) {
   amrl_init_loc_pub_.publish(amrl_initial_pose_msg_);
 }
 
+void ResetNavGoals() {
+  if (FLAGS_v > 0) {
+    printf("Reset nav goals.\n");
+  }
+  reset_nav_goals_pub_.publish(reset_nav_goals_msg_);
+}
+
 void SetNavGoal(float x, float y, float theta, QString map) {
   if (FLAGS_v > 0) {
     printf("Set nav goal: %s %f,%f, %f\n",
@@ -195,6 +205,8 @@ void *RosThread(void *arg) {
       server_, &RobotWebSocket::SetInitialPoseSignal, &SetInitialPose);
   QObject::connect(
       server_, &RobotWebSocket::SetNavGoalSignal, &SetNavGoal);
+  QObject::connect(
+      server_, &RobotWebSocket::ResetNavGoalsSignal, &ResetNavGoals);
 
   ros::NodeHandle n;
   ros::Subscriber laser_sub =
@@ -211,6 +223,8 @@ void *RosThread(void *arg) {
       n.advertise<amrl_msgs::Localization2DMsg>("/set_pose", 10);
   amrl_nav_goal_pub_ =
       n.advertise<amrl_msgs::Localization2DMsg>("/set_nav_target", 10);
+  reset_nav_goals_pub_ =
+      n.advertise<std_msgs::Empty>("/reset_nav_goals", 10);
 
   RateLoop loop(FLAGS_fps);
   while (ros::ok() && run_) {

diff --git a/webviz.html b/webviz.html
@@ -417,6 +417,18 @@ <h1>UT AUTOmata Web Interface</h1>
         }));
       }
 
+      // Reset all navgoals and waypoints.
+      resetNavGoals() {
+        this.waypoints = [];
+        if (!this.socket) {
+          console.error('Error: not connected');
+          return;
+        }
+        this.socket.send(JSON.stringify({
+          type: "reset_nav_goals"
+        }));
+      }
+
       // Send waypoint.
       async gotoWaypoint(idx) {
         this.setNavGoal(this.waypoints[idx][0], this.waypoints[idx][1], this.waypoints[idx][2]);
@@ -546,11 +558,10 @@ <h1>UT AUTOmata Web Interface</h1>
         resetNavGoals = true;
         setWaypoints = false;
         mouseDown = false;
-        robot_interface.waypoints = [];
         document.getElementById("setLocation").style = "";
         document.getElementById("setNavGoal").style = "";
         document.getElementById("setWaypoints").style = "";
-        robot_interface.setNavGoal(1111, 1111, 1111);  // all 1111 -> indicates reset of nav goals
+        robot_interface.resetNavGoals();
         document.getElementById("resetNav").style = "border-style:inset;";
       } else {
         resetNavGoals = false;