waypoint interface added

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

Hide file tree

Showing 4 changed files with 66 additions and 2 deletions.
diff --git a/src/websocket/websocket.cc b/src/websocket/websocket.cc
@@ -348,6 +348,15 @@ void RobotWebSocket::ProcessCallback(const QJsonObject& json) {
                      json.value("y").toDouble(),
                      json.value("theta").toDouble(),
                      json.value("map").toString());
+  } else if (type == "set_waypoints") {
+    if (!AllNumericalKeysPresent({"x", "y", "theta"}, json) ||
+        !StringKeyPresent("map", json)) {
+      SendError("Invalid set_waypoints parameters");
+    }
+    SetWaypointsSignal(json.value("x").toDouble(),
+                     json.value("y").toDouble(),
+                     json.value("theta").toDouble(),
+                     json.value("map").toString());
   } else if (type == "reset_nav_goals") {
     ResetNavGoalsSignal();
   } else {

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 SetWaypointsSignal(float x, float y, float theta, QString map);
   void ResetNavGoalsSignal();
 
 private Q_SLOTS:

diff --git a/src/websocket/websocket_main.cc b/src/websocket/websocket_main.cc
@@ -52,13 +52,15 @@ 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_;
+amrl_msgs::Localization2DMsg amrl_waypoint_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 amrl_waypoint_pub_;
 ros::Publisher reset_nav_goals_pub_;
 bool updates_pending_ = false;
 RobotWebSocket *server_ = nullptr;
@@ -198,13 +200,28 @@ void SetNavGoal(float x, float y, float theta, QString map) {
   amrl_nav_goal_pub_.publish(amrl_nav_goal_msg_);
 }
 
+void SetWaypoint(float x, float y, float theta, QString map){
+  if (FLAGS_v > 0) {
+    printf("Set waypoint: %s %f,%f, %f\n",
+        map.toStdString().c_str(), x, y, math_util::RadToDeg(theta));
+  }
+  amrl_waypoint_msg_.header.stamp = ros::Time::now();
+  amrl_waypoint_msg_.map = map.toStdString();
+  amrl_waypoint_msg_.pose.x = x;
+  amrl_waypoint_msg_.pose.y = y;
+  amrl_waypoint_msg_.pose.theta = theta;
+  amrl_waypoint_pub_.publish(amrl_waypoint_msg_);
+}
+
 void *RosThread(void *arg) {
   pthread_detach(pthread_self());
   CHECK_NOTNULL(server_);
   QObject::connect(
       server_, &RobotWebSocket::SetInitialPoseSignal, &SetInitialPose);
   QObject::connect(
       server_, &RobotWebSocket::SetNavGoalSignal, &SetNavGoal);
+  QObject::connect(
+      server_, &RobotWebSocket::SetWaypointsSignal, &SetWaypoint);
   QObject::connect(
       server_, &RobotWebSocket::ResetNavGoalsSignal, &ResetNavGoals);
 
@@ -223,6 +240,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);
+  amrl_waypoint_pub_ =
+      n.advertise<amrl_msgs::Localization2DMsg>("/set_waypoint", 10);
   reset_nav_goals_pub_ =
       n.advertise<std_msgs::Empty>("/reset_nav_goals", 10);
 

diff --git a/webviz.html b/webviz.html
@@ -46,7 +46,8 @@ <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" />
+  <input type="button" id="setWaypoints" onclick="setWaypointsButton()" value="Set Waypoints">
+  <input type="button" id="resetNav" onclick="resetNavButton()" value="Reset Nav" />
   Map:
   <select id="map" value="GDC1" onchange="changeMap()">
     <option value="GDC1">GDC1</option>
@@ -86,6 +87,7 @@ <h1>UT AUTOmata Web Interface</h1>
     var mouseDown = false;
     var setLocation = false;
     var setNavGoal = false;
+    var setWaypoints = false;
     var coords = document.getElementById("coords");
     var mapList = document.getElementById("map");
     var robotPose = undefined;
@@ -413,6 +415,21 @@ <h1>UT AUTOmata Web Interface</h1>
         }));
       }
 
+      // Send waypoints.
+      setWaypoints(x, y, theta) {
+        if (!this.socket) {
+          console.error('Error: not connected');
+          return;
+        }
+        this.socket.send(JSON.stringify({
+          type: "set_waypoints",
+          x: x,
+          y: y,
+          theta: theta,
+          map: currentMapName
+        }));
+      }
+
       close() {
         if (!this.socket) return;
         this.socket.close();
@@ -464,15 +481,24 @@ <h1>UT AUTOmata Web Interface</h1>
     function setLocationButton() {
       setLocation = true;
       setNavGoal = false;
+      setWaypoints = false;
       document.getElementById("setLocation").style = "border-style:inset;";
     }
 
     function setNavGoalButton() {
       setLocation = false;
       setNavGoal = true;
+      setWaypoints = false;
       document.getElementById("setNavGoal").style = "border-style:inset;";
     }
 
+    function setWaypointsButton() {
+      setLocation = false;
+      setNavGoal = false;
+      setWaypoints = true;
+      document.getElementById("setWaypoints").style = "border-style:inset;";
+    }
+
     function resetNavButton() {
       // if (robotPose !== undefined) {
       //   // Extract the current robot position from robotPose
@@ -546,7 +572,7 @@ <h1>UT AUTOmata Web Interface</h1>
       const y_str = w.y.toFixed(3);
       coords.innerHTML = `x=${x_str}, y=${y_str}`;
       if (!mouseDown) return;
-      if (setLocation || setNavGoal) return;
+      if (setLocation || setNavGoal || setWaypoints) return;
       origin.x = prev_origin.x + curMouseLoc.x - mouseDownLoc.x;
       origin.y = prev_origin.y + curMouseLoc.y - mouseDownLoc.y;
     };
@@ -579,6 +605,13 @@ <h1>UT AUTOmata Web Interface</h1>
           let a = Math.atan2(mouseDownLoc.y - loc.y, loc.x - mouseDownLoc.x);
           robot_interface.setInitialPose(w0.x, w0.y, a);
         }
+      } else if (setWaypoints) {
+        // Set waypoints here.
+        if (robot_interface) {
+          let w0 = canvasToWorld(mouseDownLoc);
+          let a = Math.atan2(mouseDownLoc.y - loc.y, loc.x - mouseDownLoc.x);
+          robot_interface.setWaypoints(w0.x, w0.y, a);
+        }
       } else {
         // Pan.
         origin.x = prev_origin.x + loc.x - mouseDownLoc.x;
@@ -587,9 +620,11 @@ <h1>UT AUTOmata Web Interface</h1>
       }
       setNavGoal = false;
       setLocation = false;
+      setWaypoints = false;
       mouseDown = false;
       document.getElementById("setLocation").style = "";
       document.getElementById("setNavGoal").style = "";
+      document.getElementById("setWaypoints").style = "";
     }
 
     function onMouseScroll(event) {