Updated websocket with new visualization code

ut-amrl · Jan 8, 2025 · ba43a7f · ba43a7f
1 parent c775861
commit ba43a7f
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 16 deletions.
diff --git a/src/shared b/src/shared
diff --git a/src/websocket/websocket.cc b/src/websocket/websocket.cc
@@ -40,10 +40,12 @@
 #include "amrl_msgs/Localization2DMsg.h"
 #include "amrl_msgs/Point2D.h"
 #include "amrl_msgs/VisualizationMsg.h"
+#include "amrl_msgs/GPSMsg.h"
 #include "gflags/gflags.h"
 #include "glog/logging.h"
 #include "sensor_msgs/LaserScan.h"
 
+using amrl_msgs::GPSMsg;
 using amrl_msgs::ColoredArc2D;
 using amrl_msgs::ColoredLine2D;
 using amrl_msgs::ColoredPoint2D;
@@ -69,7 +71,9 @@ RobotWebSocket::RobotWebSocket(uint16_t port)
   localization_.pose.x = 0;
   localization_.pose.y = 0;
   localization_.pose.theta = 0;
-  gps_pose_.data = {0, 30.2861, -97.7394, 0, 0};  // UT Austin
+  gps_pose_.latitude = 30.2861;
+  gps_pose_.longitude = -97.7394;
+  gps_pose_.heading = 0.0;  // UT Austin
   connect(this, &RobotWebSocket::SendDataSignal, this,
           &RobotWebSocket::SendDataSlot);
   if (ws_server_->listen(QHostAddress::Any, port)) {
@@ -195,7 +199,7 @@ DataMessage DataMessage::FromRosMessages(
     const VisualizationMsg& global_msg,
     const Localization2DMsg& localization_msg,
     const GPSArrayMsg& gps_goals_msg,
-    const Float64MultiArray& gps_msg) {
+    const GPSMsg& gps_msg) {
   static const bool kDebug = false;
   DataMessage msg;
   for (size_t i = 0; i < sizeof(msg.header.map); ++i) {
@@ -204,9 +208,9 @@ DataMessage DataMessage::FromRosMessages(
   msg.header.loc_x = localization_msg.pose.x;
   msg.header.loc_y = localization_msg.pose.y;
   msg.header.loc_r = localization_msg.pose.theta;
-  msg.header.lat = gps_msg.data[1];
-  msg.header.lng = gps_msg.data[2];
-  msg.header.heading = gps_msg.data[4];
+  msg.header.lat = gps_msg.latitude;
+  msg.header.lng = gps_msg.longitude;
+  msg.header.heading = gps_msg.heading;
   // Copy over GPS route
   msg.header.route[0] = gps_goals_msg.data.size();
   for (size_t i = 0; i < gps_goals_msg.data.size(); i++) {
@@ -428,7 +432,7 @@ void RobotWebSocket::Send(const VisualizationMsg& local_vis,
                           const LaserScan& laser_lowbeam_scan,
                           const Localization2DMsg& localization,
                           const GPSArrayMsg& gps_goals_msg,
-                          const Float64MultiArray& gps_pose) {
+                          const GPSMsg& gps_pose) {
   data_mutex_.lock();
   localization_ = localization;
   local_vis_ = local_vis;

diff --git a/src/websocket/websocket.h b/src/websocket/websocket.h
@@ -105,7 +105,7 @@ struct DataMessage {
       const amrl_msgs::VisualizationMsg& global_msg,
       const amrl_msgs::Localization2DMsg& localization_msg,
       const amrl_msgs::GPSArrayMsg& gps_route_msg,
-      const std_msgs::Float64MultiArray& gps_pose_msg);
+      const amrl_msgs::GPSMsg& gps_pose_msg);
 };
 
 class RobotWebSocket : public QObject {
@@ -119,7 +119,7 @@ class RobotWebSocket : public QObject {
             const sensor_msgs::LaserScan& laser_lowbeam_scan,
             const amrl_msgs::Localization2DMsg& localization,
             const amrl_msgs::GPSArrayMsg& gps_goals_msg,
-            const std_msgs::Float64MultiArray& gps_pose);
+            const amrl_msgs::GPSMsg& gps_pose);
 
  Q_SIGNALS:
   void closed();
@@ -151,7 +151,7 @@ class RobotWebSocket : public QObject {
   sensor_msgs::LaserScan laser_lowbeam_scan_;
   amrl_msgs::Localization2DMsg localization_;
   amrl_msgs::GPSArrayMsg gps_goals_msg_;
-  std_msgs::Float64MultiArray gps_pose_;
+  amrl_msgs::GPSMsg gps_pose_;
 };
 
 #endif  // ECHOSERVER_H
diff --git a/src/websocket/websocket_main.cc b/src/websocket/websocket_main.cc
@@ -64,7 +64,6 @@ amrl_msgs::graphNavGPSSrv srv;
 amrl_msgs::Localization2DMsg amrl_initial_pose_msg_;
 amrl_msgs::Localization2DMsg amrl_nav_goal_msg_;
 std_msgs::Empty reset_nav_goals_msg_;
-Float64MultiArray gps_pose_msg_;
 Localization2DMsg localization_msg_;
 LaserScan laser_scan_;
 LaserScan laser_lowbeam_scan_;
@@ -93,8 +92,9 @@ void LaserLowBeamCallback(const LaserScan &msg) {
 }
 
 
-void GPSPoseCallback(const Float64MultiArray &msg) {
-  gps_pose_msg_ = msg;
+void GPSPoseCallback(const GPSMsg &msg) {
+  robot_gps_msg_ = msg;
+  updates_pending_ = true;
 }
 
 void VisualizationCallback(const VisualizationMsg &msg) {
@@ -166,7 +166,7 @@ void SendUpdate() {
   }
 
   server_->Send(local_msgs, global_msgs, laser_scan_, laser_lowbeam_scan_, 
-    localization_msg_, gps_goals_msg_, gps_pose_msg_);
+    localization_msg_, gps_goals_msg_, robot_gps_msg_);
 }
 
 void SetInitialPose(float x, float y, float theta, QString map) {
@@ -268,7 +268,7 @@ void *RosThread(void *arg) {
       n.subscribe("/visualization", 10, &VisualizationCallback);
   ros::Subscriber localization_sub =
       n.subscribe("/localization", 10, &LocalizationCallback);
-  ros::Subscriber gps_pose_sub = n.subscribe("/phone/gps_with_heading", 10, &GPSPoseCallback);
+  ros::Subscriber gps_pose_sub = n.subscribe("/vectornav/GPSHeading", 10, &GPSPoseCallback);
   init_loc_pub_ =
       n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
   nav_goal_pub_ =
@@ -322,7 +322,6 @@ void InitMessage() {
   initial_pose_msg_.pose.pose.orientation.z = 0;
   gps_goals_msg_.header.stamp = ros::Time(0);
   gps_goals_msg_.data.clear();
-  gps_pose_msg_.data = {0, 0, 0, 0, 0};
   nav_goal_msg_.header = initial_pose_msg_.header;
 }
+32 −0		.github/workflows/buildTest.yml
+0 −20		.travis.yml
+2 −1		CMakeLists.txt
+1 −1		README.md
+207 −0		math/gps_util.h
+1 −1		math/line2d.h
+2 −1		math/math_util.h
+92 −0		tests/util/kdtree_tests.cc
+1 −1		util/helpers.h
+322 −0		util/kdtree.h
+6 −4		util/random.h
+33 −0		util/timer.cc
+60 −0		util/timer.h