changes

ut-amrl · Oct 30, 2023 · 29dd19d · 29dd19d
1 parent 754e321
commit 29dd19d
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 1 deletion.
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
@@ -46,6 +46,7 @@ <h1>UT AUTOmata Web Interface</h1>
   Status: <label id="status">Disconnected</label><br>
   <input type="button" id="setLocation" onclick="setLocationButton()" value="Set Pose">
   <input type="button" id="setNavGoal" onclick="setNavGoalButton()" value="Set Nav Target">
+  <input type="button" id="resetNav" onclick="resetNavButton()" value="Reset nav" />
   Map:
   <select id="map" value="GDC1" onchange="changeMap()">
     <option value="GDC1">GDC1</option>
@@ -472,6 +473,35 @@ <h1>UT AUTOmata Web Interface</h1>
       document.getElementById("setNavGoal").style = "border-style:inset;";
     }
 
+    function resetNavButton() {
+      // if (robotPose !== undefined) {
+      //   // Extract the current robot position from robotPose
+      //   const x = robotPose.x;
+      //   const y = robotPose.y;
+      //   const theta = robotPose.theta;
+
+      //   if (robot_interface.socket) {
+      //     robot_interface.socket.send(JSON.stringify({
+      //       type: "set_nav_goal",
+      //       x: x,
+      //       y: y,
+      //       theta: theta
+      //     }));
+      //   } else {
+      //     console.error('Error: not connected');
+      //   }
+      // } else {
+      //   console.error('Error: robotPose is undefined');
+      // }
+      if (!robot_interface.socket) {
+          console.error('Error: not connected');
+          return;
+        }
+        robot_interface.socket.send(JSON.stringify({
+          type: "reset_nav_goals"
+        }));
+    }
+
     // Get the "true" mouse location, compensting for scroll, zoom, etc.
     function getMouseLocation(event, element) {
       var posx = 0;
@@ -964,4 +994,4 @@ <h1>UT AUTOmata Web Interface</h1>
   </script>
 </body>
 
-</html>
+</html>