Skip to content

Commit

Permalink
[Feature] Added support for publishing gps subgoals from webviz
Browse files Browse the repository at this point in the history
  • Loading branch information
@artzha
artzha committed Nov 8, 2024
1 parent 45d19e6 commit 286d6be
Show file tree
Hide file tree
Showing 5 changed files with 130 additions and 39 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ A low-bandwidth websocket-based direct robot visualizer.
## Dependencies

1. [ROS](http://wiki.ros.org/Installation/)
1. [AMRL ROS Messages](https://github.com/ut-amrl/amrl_msgs)
1. [AMRL ROS Messages](https://github.com/ut-amrl/amrl_msgs/tree/a2414dd86b08fea32b982478a9f5945c72fe2c46)
1. QT5 and QT5 WebSockets
```
sudo apt install qt5-default libqt5websockets5-dev
Expand Down
35 changes: 27 additions & 8 deletions src/websocket/websocket.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ RobotWebSocket::RobotWebSocket(uint16_t port)
localization_.pose.x = 0;
localization_.pose.y = 0;
localization_.pose.theta = 0;
gps_pose_.data = {30.2861, -97.7394, 0}; // UT Austin
gps_pose_.data = {0, 30.2861, -97.7394, 0, 0}; // UT Austin
connect(this, &RobotWebSocket::SendDataSignal, this,
&RobotWebSocket::SendDataSlot);
if (ws_server_->listen(QHostAddress::Any, port)) {
Expand Down Expand Up @@ -189,7 +189,8 @@ QByteArray DataMessage::ToByteArray() const {
}

DataMessage DataMessage::FromRosMessages(
const LaserScan& laser_msg, const VisualizationMsg& local_msg,
const LaserScan& laser_msg, const LaserScan& laser_lowbeam_msg,
const VisualizationMsg& local_msg,
const VisualizationMsg& global_msg,
const Localization2DMsg& localization_msg,
const Float64MultiArray& gps_msg) {
Expand All @@ -201,15 +202,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;
msg.header.lat = gps_msg.data[0];
msg.header.lng = gps_msg.data[1];
msg.header.heading = gps_msg.data[2];
msg.header.lat = gps_msg.data[1];
msg.header.lng = gps_msg.data[2];
msg.header.heading = gps_msg.data[4];
// printf("lat %lf lng %lf, heading %lf", msg.header.lat, msg.header.lng, msg.header.heading );
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());
msg.header.num_laser_rays = laser_msg.ranges.size() + laser_lowbeam_msg.ranges.size();
msg.laser_scan.resize(laser_msg.ranges.size() +
laser_lowbeam_msg.ranges.size());
for (size_t i = 0; i < laser_msg.ranges.size(); ++i) {
if (laser_msg.ranges[i] <= laser_msg.range_min ||
laser_msg.ranges[i] >= laser_msg.range_max) {
Expand All @@ -218,6 +221,15 @@ DataMessage DataMessage::FromRosMessages(
msg.laser_scan[i] = static_cast<uint32_t>(laser_msg.ranges[i] * 1000.0);
}
}
for (size_t i = 0; i < laser_lowbeam_msg.ranges.size(); ++i) {
if (laser_lowbeam_msg.ranges[i] <= laser_lowbeam_msg.range_min ||
laser_lowbeam_msg.ranges[i] >= laser_lowbeam_msg.range_max) {
msg.laser_scan[i + laser_msg.ranges.size()] = 0;
} else {
msg.laser_scan[i + laser_msg.ranges.size()] =
static_cast<uint32_t>(laser_lowbeam_msg.ranges[i] * 1000.0);
}
}
msg.points = local_msg.points;
msg.header.num_local_points = local_msg.points.size();
msg.points.insert(msg.points.end(), global_msg.points.begin(),
Expand Down Expand Up @@ -330,6 +342,11 @@ void RobotWebSocket::ProcessCallback(const QJsonObject& json) {
SetNavGoalSignal(json.value("x").toDouble(), json.value("y").toDouble(),
json.value("theta").toDouble(),
json.value("map").toString());
} else if (type == "set_gps_goal") {
if (!AllNumericalKeysPresent({"lat", "lon"}, json)) {
SendError("Invalid set_gps_goal parameters");
}
SetGPSGoalSignal(json.value("lat").toDouble(), json.value("lon").toDouble());
} else if (type == "reset_nav_goals") {
ResetNavGoalsSignal();
} else {
Expand Down Expand Up @@ -385,7 +402,7 @@ void RobotWebSocket::SendDataSlot() {
if (clients_.empty()) return;
data_mutex_.lock();
const auto data = DataMessage::FromRosMessages(
laser_scan_, local_vis_, global_vis_, localization_, gps_pose_);
laser_scan_, laser_lowbeam_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 @@ -397,13 +414,15 @@ void RobotWebSocket::SendDataSlot() {
void RobotWebSocket::Send(const VisualizationMsg& local_vis,
const VisualizationMsg& global_vis,
const LaserScan& laser_scan,
const LaserScan& laser_lowbeam_scan,
const Localization2DMsg& localization,
const Float64MultiArray& gps_pose) {
data_mutex_.lock();
localization_ = localization;
local_vis_ = local_vis;
global_vis_ = global_vis;
laser_scan_ = laser_scan;
laser_lowbeam_scan_ = laser_lowbeam_scan;
gps_pose_ = gps_pose;
data_mutex_.unlock();
SendDataSignal();
Expand Down
14 changes: 9 additions & 5 deletions src/websocket/websocket.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,14 +59,14 @@ struct MessageHeader {
float laser_max_angle; // 12
float loc_x; // 13
float loc_y; // 14
float loc_r; // 15
double lat; // 16
double lng; // 17
double heading; // 18
float loc_r; // 15
float lat; // 16
float lng; // 17
float heading; // 18
char map[32]; //
size_t GetByteLength() const {
const size_t len =
15 * 4 + 3 * 8 + 32 + // header fields + map data
18 * 4 + 32 + // header fields + map data
num_laser_rays * 4 + // each ray is uint32_t
num_points * 3 * 4 + // x, y, color
num_lines * 5 * 4 + // x1, y1, x2, y2, color
Expand All @@ -93,6 +93,7 @@ struct DataMessage {
QByteArray ToByteArray() const;
static DataMessage FromRosMessages(
const sensor_msgs::LaserScan& laser_msg,
const sensor_msgs::LaserScan& laser_lowbeam_msg,
const amrl_msgs::VisualizationMsg& local_msg,
const amrl_msgs::VisualizationMsg& global_msg,
const amrl_msgs::Localization2DMsg& localization_msg,
Expand All @@ -107,6 +108,7 @@ class RobotWebSocket : public QObject {
void Send(const amrl_msgs::VisualizationMsg& local_vis,
const amrl_msgs::VisualizationMsg& global_vis,
const sensor_msgs::LaserScan& laser_scan,
const sensor_msgs::LaserScan& laser_lowbeam_scan,
const amrl_msgs::Localization2DMsg& localization,
const std_msgs::Float64MultiArray& gps_pose);

Expand All @@ -115,6 +117,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 SetGPSGoalSignal(float lat, float lon);
void ResetNavGoalsSignal();

private Q_SLOTS:
Expand All @@ -136,6 +139,7 @@ class RobotWebSocket : public QObject {
amrl_msgs::VisualizationMsg local_vis_;
amrl_msgs::VisualizationMsg global_vis_;
sensor_msgs::LaserScan laser_scan_;
sensor_msgs::LaserScan laser_lowbeam_scan_;
amrl_msgs::Localization2DMsg localization_;
std_msgs::Float64MultiArray gps_pose_;
};
Expand Down
43 changes: 37 additions & 6 deletions src/websocket/websocket_main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@

#include "amrl_msgs/Localization2DMsg.h"
#include "amrl_msgs/VisualizationMsg.h"
#include "amrl_msgs/GPSArrayMsg.h"
#include "amrl_msgs/GPSMsg.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "gflags/gflags.h"
Expand All @@ -40,6 +42,8 @@ using amrl_msgs::Localization2DMsg;
using amrl_msgs::VisualizationMsg;
using sensor_msgs::LaserScan;
using std::vector;
using amrl_msgs::GPSArrayMsg;
using amrl_msgs::GPSMsg;
using std_msgs::Float64MultiArray;

DEFINE_double(fps, 10.0, "Max visualization frames rate.");
Expand All @@ -51,15 +55,18 @@ bool run_ = true;
vector<VisualizationMsg> vis_msgs_;
geometry_msgs::PoseWithCovarianceStamped initial_pose_msg_;
geometry_msgs::PoseStamped nav_goal_msg_;
amrl_msgs::GPSArrayMsg gps_goal_msg_;
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_;
ros::Publisher init_loc_pub_;
ros::Publisher amrl_init_loc_pub_;
ros::Publisher nav_goal_pub_;
ros::Publisher amrl_gps_goal_pub_;
ros::Publisher amrl_nav_goal_pub_;
ros::Publisher reset_nav_goals_pub_;
bool updates_pending_ = false;
Expand All @@ -75,9 +82,14 @@ void LaserCallback(const LaserScan &msg) {
updates_pending_ = true;
}

void LaserLowBeamCallback(const LaserScan &msg) {
laser_lowbeam_scan_ = msg;
updates_pending_ = true;
}


void GPSPoseCallback(const Float64MultiArray &msg) {
gps_pose_msg_ = msg;
updates_pending_ = true;
}

void VisualizationCallback(const VisualizationMsg &msg) {
Expand Down Expand Up @@ -147,8 +159,9 @@ void SendUpdate() {
MergeMessage(m, &local_msgs);
}
}
server_->Send(local_msgs, global_msgs, laser_scan_, localization_msg_,
gps_pose_msg_);

server_->Send(local_msgs, global_msgs, laser_scan_, laser_lowbeam_scan_,
localization_msg_, gps_pose_msg_);
}

void SetInitialPose(float x, float y, float theta, QString map) {
Expand Down Expand Up @@ -196,6 +209,19 @@ void SetNavGoal(float x, float y, float theta, QString map) {
amrl_nav_goal_pub_.publish(amrl_nav_goal_msg_);
}

void SetGPSGoal(float lat, float lon) {
if (FLAGS_v > 0) {
printf("Set gps goal: %f, %f\n", lat, lon);
}
GPSMsg msg;
msg.header.stamp = ros::Time::now();
msg.latitude = lat;
msg.longitude = lon;
gps_goal_msg_.header.stamp = msg.header.stamp;
gps_goal_msg_.data = { msg };
amrl_gps_goal_pub_.publish(gps_goal_msg_);
}

void *RosThread(void *arg) {
pthread_detach(pthread_self());
CHECK_NOTNULL(server_);
Expand All @@ -204,20 +230,25 @@ void *RosThread(void *arg) {
QObject::connect(server_, &RobotWebSocket::SetNavGoalSignal, &SetNavGoal);
QObject::connect(server_, &RobotWebSocket::ResetNavGoalsSignal,
&ResetNavGoals);
QObject::connect(server_, &RobotWebSocket::SetGPSGoalSignal, &SetGPSGoal);

ros::NodeHandle n;
ros::Subscriber laser_sub = n.subscribe("/scan", 5, &LaserCallback);
ros::Subscriber laser_sub = n.subscribe("/velodyne_2dscan_high_beams", 5, &LaserCallback);
ros::Subscriber laser_lowbeam_sub =
n.subscribe("/velodyne_2dscan_lowbeam", 5, &LaserLowBeamCallback);
ros::Subscriber vis_sub =
n.subscribe("/visualization", 10, &VisualizationCallback);
ros::Subscriber localization_sub =
n.subscribe("/localization", 10, &LocalizationCallback);
ros::Subscriber gps_pose_sub = n.subscribe("/gps_pose", 10, &GPSPoseCallback);
ros::Subscriber gps_pose_sub = n.subscribe("/phone/gps_with_heading", 10, &GPSPoseCallback);
init_loc_pub_ =
n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
nav_goal_pub_ =
n.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
amrl_init_loc_pub_ =
n.advertise<amrl_msgs::Localization2DMsg>("/set_pose", 10);
amrl_gps_goal_pub_ =
n.advertise<amrl_msgs::GPSArrayMsg>("/set_gps_goal", 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);
Expand Down Expand Up @@ -261,7 +292,7 @@ void InitMessage() {
initial_pose_msg_.pose.pose.orientation.x = 0;
initial_pose_msg_.pose.pose.orientation.y = 0;
initial_pose_msg_.pose.pose.orientation.z = 0;
gps_pose_msg_.data = {0, 0, 0};
gps_pose_msg_.data = {0, 0, 0, 0, 0};
nav_goal_msg_.header = initial_pose_msg_.header;
}

Expand Down
Loading

0 comments on commit 286d6be

Please sign in to comment.