Skip to content

Commit

Permalink
[Feature] Added gps pose visualization support (UTM)
Browse files Browse the repository at this point in the history
  • Loading branch information
@artzha
artzha committed Nov 8, 2024
1 parent c5c0a9e commit 45d19e6
Show file tree
Hide file tree
Showing 4 changed files with 263 additions and 181 deletions.
172 changes: 77 additions & 95 deletions src/websocket/websocket.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,104 +21,93 @@
//========================================================================
#include "websocket.h"

#include <QtWebSockets/qwebsocket.h>
#include <QtWebSockets/qwebsocketserver.h>
#include <string.h>

#include <QtCore/QDebug>
#include <QtCore/QJsonDocument>
#include <QtCore/QJsonObject>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>

#include "glog/logging.h"
#include "gflags/gflags.h"

#include <QtWebSockets/qwebsocketserver.h>
#include <QtWebSockets/qwebsocket.h>
#include <QtCore/QDebug>
#include <QtCore/QJsonDocument>
#include <QtCore/QJsonObject>

#include "amrl_msgs/Localization2DMsg.h"
#include "amrl_msgs/Point2D.h"
#include "amrl_msgs/ColoredPoint2D.h"
#include "amrl_msgs/ColoredLine2D.h"
#include "amrl_msgs/ColoredArc2D.h"
#include "amrl_msgs/ColoredLine2D.h"
#include "amrl_msgs/ColoredPoint2D.h"
#include "amrl_msgs/ColoredText.h"
#include "amrl_msgs/Localization2DMsg.h"
#include "amrl_msgs/Point2D.h"
#include "amrl_msgs/VisualizationMsg.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "sensor_msgs/LaserScan.h"

using amrl_msgs::Localization2DMsg;
using amrl_msgs::Point2D;
using amrl_msgs::ColoredArc2D;
using amrl_msgs::ColoredLine2D;
using amrl_msgs::ColoredPoint2D;
using amrl_msgs::ColoredText;
using amrl_msgs::Localization2DMsg;
using amrl_msgs::Point2D;
using amrl_msgs::VisualizationMsg;
using sensor_msgs::LaserScan;
using std::vector;
using std_msgs::Float64MultiArray;

DEFINE_uint64(max_connections, 4, "Maximum number of websocket connections");

QT_USE_NAMESPACE

RobotWebSocket::RobotWebSocket(uint16_t port) :
QObject(nullptr),
ws_server_(new QWebSocketServer(("Robot Websocket Server"),
QWebSocketServer::NonSecureMode, this)) {
RobotWebSocket::RobotWebSocket(uint16_t port)
: QObject(nullptr),
ws_server_(new QWebSocketServer(("Robot Websocket Server"),
QWebSocketServer::NonSecureMode, this)) {
localization_.header.stamp = ros::Time(0);
localization_.header.seq = 0;
localization_.pose.x = 0;
localization_.pose.y = 0;
localization_.pose.theta = 0;
connect(this,
&RobotWebSocket::SendDataSignal,
this,
gps_pose_.data = {30.2861, -97.7394, 0}; // UT Austin
connect(this, &RobotWebSocket::SendDataSignal, this,
&RobotWebSocket::SendDataSlot);
if (ws_server_->listen(QHostAddress::Any, port)) {
qDebug() << "Listening on port" << port;
connect(ws_server_,
&QWebSocketServer::newConnection,
this, &RobotWebSocket::onNewConnection);
connect(ws_server_,
&QWebSocketServer::closed, this, &RobotWebSocket::closed);
qDebug() << "Listening on port" << port;
connect(ws_server_, &QWebSocketServer::newConnection, this,
&RobotWebSocket::onNewConnection);
connect(ws_server_, &QWebSocketServer::closed, this,
&RobotWebSocket::closed);
}
}

RobotWebSocket::~RobotWebSocket() {
ws_server_->close();
if(clients_.size() > 0) {
if (clients_.size() > 0) {
for (auto c : clients_) {
delete c;
}
}
clients_.clear();
}


void RobotWebSocket::onNewConnection() {
QWebSocket *new_client = ws_server_->nextPendingConnection();
QWebSocket* new_client = ws_server_->nextPendingConnection();
if (clients_.size() >= FLAGS_max_connections) {
// We already have the max number of clients
new_client->sendTextMessage(
"{ \"error\": \"Too many clients\" }");
new_client->sendTextMessage("{ \"error\": \"Too many clients\" }");
qInfo() << "Ignoring new client" << new_client
<< ", too many existing clients:" << clients_.size();
delete new_client;
return;
}
clients_.push_back(new_client);
qInfo() << "New client: " << new_client << ", "
<< clients_.size() << "/" << FLAGS_max_connections;
connect(new_client,
&QWebSocket::textMessageReceived,
this,
qInfo() << "New client: " << new_client << ", " << clients_.size() << "/"
<< FLAGS_max_connections;
connect(new_client, &QWebSocket::textMessageReceived, this,
&RobotWebSocket::processTextMessage);
connect(new_client,
&QWebSocket::binaryMessageReceived,
this,
connect(new_client, &QWebSocket::binaryMessageReceived, this,
&RobotWebSocket::processBinaryMessage);
connect(new_client,
&QWebSocket::disconnected,
this,
connect(new_client, &QWebSocket::disconnected, this,
&RobotWebSocket::socketDisconnected);
}

Expand Down Expand Up @@ -179,8 +168,8 @@ DataMessage GenerateTestData(const MessageHeader& h) {
msg.text_annotations[i].start.y = 2.0 * i;
msg.text_annotations[i].color = (x << 16) | (x << 8) | x;
msg.text_annotations[i].size_em = 3.0 * i;
const char *s = std::to_string(i).c_str();
strncpy(msg.text_annotations[i].text, s, i/10);
const char* s = std::to_string(i).c_str();
strncpy(msg.text_annotations[i].text, s, i / 10);
}
}
return msg;
Expand All @@ -200,10 +189,10 @@ QByteArray DataMessage::ToByteArray() const {
}

DataMessage DataMessage::FromRosMessages(
const LaserScan& laser_msg,
const VisualizationMsg& local_msg,
const VisualizationMsg& global_msg,
const Localization2DMsg& localization_msg) {
const LaserScan& laser_msg, const VisualizationMsg& local_msg,
const VisualizationMsg& global_msg,
const Localization2DMsg& localization_msg,
const Float64MultiArray& gps_msg) {
static const bool kDebug = false;
DataMessage msg;
for (size_t i = 0; i < sizeof(msg.header.map); ++i) {
Expand All @@ -212,15 +201,17 @@ 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;
strncpy(msg.header.map,
localization_msg.map.data(),
msg.header.lat = gps_msg.data[0];
msg.header.lng = gps_msg.data[1];
msg.header.heading = gps_msg.data[2];
strncpy(msg.header.map, localization_msg.map.data(),
std::min(sizeof(msg.header.map) - 1, localization_msg.map.size()));
msg.header.laser_min_angle = laser_msg.angle_min;
msg.header.laser_max_angle = laser_msg.angle_max;
msg.header.num_laser_rays = laser_msg.ranges.size();
msg.laser_scan.resize(laser_msg.ranges.size());
for (size_t i = 0; i < laser_msg.ranges.size(); ++i) {
if (laser_msg.ranges[i] <= laser_msg.range_min ||
if (laser_msg.ranges[i] <= laser_msg.range_min ||
laser_msg.ranges[i] >= laser_msg.range_max) {
msg.laser_scan[i] = 0;
} else {
Expand All @@ -229,27 +220,25 @@ DataMessage DataMessage::FromRosMessages(
}
msg.points = local_msg.points;
msg.header.num_local_points = local_msg.points.size();
msg.points.insert(msg.points.end(),
global_msg.points.begin(),
msg.points.insert(msg.points.end(), global_msg.points.begin(),
global_msg.points.end());

msg.lines = local_msg.lines;
msg.header.num_local_lines = local_msg.lines.size();
msg.lines.insert(msg.lines.end(),
global_msg.lines.begin(),
msg.lines.insert(msg.lines.end(), global_msg.lines.begin(),
global_msg.lines.end());

msg.arcs = local_msg.arcs;
msg.header.num_local_arcs = local_msg.arcs.size();
msg.arcs.insert(msg.arcs.end(),
global_msg.arcs.begin(),
msg.arcs.insert(msg.arcs.end(), global_msg.arcs.begin(),
global_msg.arcs.end());

msg.header.num_points = msg.points.size();
msg.header.num_lines = msg.lines.size();
msg.header.num_arcs = msg.arcs.size();
msg.header.num_local_text_annotations = local_msg.text_annotations.size();
msg.header.num_text_annotations = local_msg.text_annotations.size() + global_msg.text_annotations.size();
msg.header.num_text_annotations =
local_msg.text_annotations.size() + global_msg.text_annotations.size();
for (amrl_msgs::ColoredText text : local_msg.text_annotations) {
ColoredTextNative localText;
localText.start = text.start;
Expand All @@ -272,32 +261,28 @@ DataMessage DataMessage::FromRosMessages(
}

if (kDebug) {
printf("nonce: %d "
"num_points: %d "
"num_lines: %d "
"num_arcs: %d "
"num_text_annotations: %d "
"num_laser_rays: %d "
"num_local_points: %d "
"num_local_lines: %d "
"num_local_arcs: %d "
"num_local_text_annotations: %d\n",
msg.header.nonce,
msg.header.num_points,
msg.header.num_lines,
msg.header.num_arcs,
msg.header.num_text_annotations,
msg.header.num_laser_rays,
msg.header.num_local_points,
msg.header.num_local_lines,
msg.header.num_local_arcs,
msg.header.num_local_text_annotations);
printf(
"nonce: %d "
"num_points: %d "
"num_lines: %d "
"num_arcs: %d "
"num_text_annotations: %d "
"num_laser_rays: %d "
"num_local_points: %d "
"num_local_lines: %d "
"num_local_arcs: %d "
"num_local_text_annotations: %d\n",
msg.header.nonce, msg.header.num_points, msg.header.num_lines,
msg.header.num_arcs, msg.header.num_text_annotations,
msg.header.num_laser_rays, msg.header.num_local_points,
msg.header.num_local_lines, msg.header.num_local_arcs,
msg.header.num_local_text_annotations);
}
return msg;
}

void RobotWebSocket::SendError(const QString& error_val) {
for (auto c: clients_) {
for (auto c : clients_) {
CHECK_NOTNULL(c);
c->sendTextMessage("{ \"error\": \"" + error_val + "\" }");
}
Expand All @@ -313,8 +298,7 @@ bool AllNumericalKeysPresent(const QStringList& expected,
return true;
}

bool StringKeyPresent(const QString& key,
const QJsonObject& json) {
bool StringKeyPresent(const QString& key, const QJsonObject& json) {
if (!json.contains(key)) return false;
const QJsonValue val = json.value(key);
return val.isString();
Expand All @@ -335,17 +319,15 @@ void RobotWebSocket::ProcessCallback(const QJsonObject& json) {
!StringKeyPresent("map", json)) {
SendError("Invalid set_initial_pose parameters");
}
SetInitialPoseSignal(json.value("x").toDouble(),
json.value("y").toDouble(),
SetInitialPoseSignal(json.value("x").toDouble(), json.value("y").toDouble(),
json.value("theta").toDouble(),
json.value("map").toString());
} else if (type == "set_nav_goal") {
} else if (type == "set_nav_goal") {
if (!AllNumericalKeysPresent({"x", "y", "theta"}, json) ||
!StringKeyPresent("map", json)) {
SendError("Invalid set_nav_goal parameters");
}
SetNavGoalSignal(json.value("x").toDouble(),
json.value("y").toDouble(),
SetNavGoalSignal(json.value("x").toDouble(), json.value("y").toDouble(),
json.value("theta").toDouble(),
json.value("map").toString());
} else if (type == "reset_nav_goals") {
Expand All @@ -357,7 +339,7 @@ void RobotWebSocket::ProcessCallback(const QJsonObject& json) {

void RobotWebSocket::processTextMessage(QString message) {
static const bool kSendTestMessage = false;
QWebSocket *client = qobject_cast<QWebSocket *>(sender());
QWebSocket* client = qobject_cast<QWebSocket*>(sender());
CHECK_NOTNULL(client);
QJsonDocument doc = QJsonDocument::fromJson(message.toLocal8Bit());
QJsonObject json = doc.object();
Expand All @@ -379,16 +361,15 @@ void RobotWebSocket::processTextMessage(QString message) {
}

void RobotWebSocket::processBinaryMessage(QByteArray message) {
QWebSocket *client = qobject_cast<QWebSocket *>(sender());
QWebSocket* client = qobject_cast<QWebSocket*>(sender());
qDebug() << "Binary Message received:" << message;
if (client) {
client->sendBinaryMessage(message);
}
}


void RobotWebSocket::socketDisconnected() {
QWebSocket* client = qobject_cast<QWebSocket *>(sender());
QWebSocket* client = qobject_cast<QWebSocket*>(sender());
auto client_iter = std::find(clients_.begin(), clients_.end(), client);
if (client_iter == clients_.end()) {
fprintf(stderr, "ERROR: unknown socket disconnected!\n");
Expand All @@ -404,7 +385,7 @@ void RobotWebSocket::SendDataSlot() {
if (clients_.empty()) return;
data_mutex_.lock();
const auto data = DataMessage::FromRosMessages(
laser_scan_, local_vis_, global_vis_, localization_);
laser_scan_, local_vis_, global_vis_, localization_, gps_pose_);
const auto buffer = data.ToByteArray();
CHECK_EQ(data.header.GetByteLength(), buffer.size());
for (auto c : clients_) {
Expand All @@ -416,13 +397,14 @@ void RobotWebSocket::SendDataSlot() {
void RobotWebSocket::Send(const VisualizationMsg& local_vis,
const VisualizationMsg& global_vis,
const LaserScan& laser_scan,
const Localization2DMsg& localization) {
const Localization2DMsg& localization,
const Float64MultiArray& gps_pose) {
data_mutex_.lock();
localization_ = localization;
local_vis_ = local_vis;
global_vis_ = global_vis;
laser_scan_ = laser_scan;
gps_pose_ = gps_pose;
data_mutex_.unlock();
SendDataSignal();
}

Loading

0 comments on commit 45d19e6

Please sign in to comment.