Skip to content

Commit

Permalink
webviz for intermediate planner
Browse files Browse the repository at this point in the history
  • Loading branch information
@zdeng-UTexas
zdeng-UTexas committed Apr 12, 2024
1 parent b02440e commit 9b43a26
Show file tree
Hide file tree
Showing 6 changed files with 318 additions and 40 deletions.
60 changes: 60 additions & 0 deletions .github/workflows/buildTest.yml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
name: Build-Test
# Run this workflow every time a new commit pushed to your repository
on: push

jobs:
# Set the job key. The key is displayed as the job name
# when a job name is not provided
build-test:
# Name the Job
name: Build test
# Set the type of machine to run on
runs-on: ubuntu-20.04
defaults:
run:
shell: bash -l {0}
container: ros:noetic

steps:
# Run package update
- name: Run package update
run: |
sudo apt update
sudo apt dist-upgrade -y
# Install dependencies
- name: Install dependencies
run: |
sudo apt-get install -y git qt5-default libqt5websockets5-dev ros-noetic-angles ros-noetic-tf python-is-python3 libgtest-dev libgoogle-glog-dev cmake build-essential
# Rosdep
- name: ROSDep Update
run: |
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
sudo rosdep init
rosdep update
# Checks out a copy of your repository
- name: Checkout code
uses: actions/checkout@v2
with:
submodules: 'recursive'


# Checkout and build amrl_msgs
- name: Get amrl_msgs
run: |
source /opt/ros/noetic/setup.bash
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$GITHUB_WORKSPACE:$GITHUB_WORKSPACE/../amrl_msgs
cd $GITHUB_WORKSPACE/..
git clone https://github.com/ut-amrl/amrl_msgs.git
cd amrl_msgs
make
# Compiles the code
- name: Run build
run: |
source /opt/ros/noetic/setup.bash
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$GITHUB_WORKSPACE:$GITHUB_WORKSPACE/../amrl_msgs:$GITHUB_WORKSPACE/../amrl_maps
cd $GITHUB_WORKSPACE
make
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

A low-bandwidth websocket-based direct robot visualizer.

[![Build Status](https://travis-ci.com/ut-amrl/webviz.svg?branch=master)](https://travis-ci.com/ut-amrl/webviz)
[![Build Status](https://github.com/ut-amrl/webviz/actions/workflows/buildTest.yml/badge.svg)](https://github.com/ut-amrl/webviz/actions)


## Dependencies
Expand Down
51 changes: 46 additions & 5 deletions src/websocket/websocket.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include "amrl_msgs/ColoredPoint2D.h"
#include "amrl_msgs/ColoredLine2D.h"
#include "amrl_msgs/ColoredArc2D.h"
#include "amrl_msgs/ColoredText.h"
#include "amrl_msgs/VisualizationMsg.h"
#include "sensor_msgs/LaserScan.h"

Expand All @@ -50,6 +51,7 @@ using amrl_msgs::Point2D;
using amrl_msgs::ColoredArc2D;
using amrl_msgs::ColoredLine2D;
using amrl_msgs::ColoredPoint2D;
using amrl_msgs::ColoredText;
using amrl_msgs::VisualizationMsg;
using sensor_msgs::LaserScan;
using std::vector;
Expand Down Expand Up @@ -140,6 +142,7 @@ DataMessage GenerateTestData(const MessageHeader& h) {
msg.points.resize(h.num_points);
msg.lines.resize(h.num_lines);
msg.arcs.resize(h.num_arcs);
msg.text_annotations.resize(h.num_text_annotations);
for (size_t i = 0; i < msg.laser_scan.size(); ++i) {
msg.laser_scan[i] = 10 * i;
}
Expand Down Expand Up @@ -170,6 +173,15 @@ DataMessage GenerateTestData(const MessageHeader& h) {
}
const uint8_t x = static_cast<uint8_t>(i);
msg.arcs[i].color = (x << 16) | (x << 8) | x;

for (size_t i = 0; i < msg.text_annotations.size(); i++) {
msg.text_annotations[i].start.x = 1.0 * i;
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);
}
}
return msg;
}
Expand All @@ -183,6 +195,7 @@ QByteArray DataMessage::ToByteArray() const {
buf = WriteElementVector(points, buf);
buf = WriteElementVector(lines, buf);
buf = WriteElementVector(arcs, buf);
buf = WriteElementVector(text_annotations, buf);
return data;
}

Expand Down Expand Up @@ -214,7 +227,6 @@ DataMessage DataMessage::FromRosMessages(
msg.laser_scan[i] = static_cast<uint32_t>(laser_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(),
Expand All @@ -236,24 +248,50 @@ DataMessage DataMessage::FromRosMessages(
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();
for (amrl_msgs::ColoredText text : local_msg.text_annotations) {
ColoredTextNative localText;
localText.start = text.start;
localText.color = text.color;
localText.size_em = text.size_em;
size_t size = std::min(sizeof(localText.text) - 1, text.text.size());
strncpy(localText.text, text.text.data(), size);
localText.text[size] = 0;
msg.text_annotations.push_back(localText);
}
for (amrl_msgs::ColoredText text : global_msg.text_annotations) {
ColoredTextNative localText;
localText.start = text.start;
localText.color = text.color;
localText.size_em = text.size_em;
size_t size = std::min(sizeof(localText.text) - 1, text.text.size());
strncpy(localText.text, text.text.data(), size);
localText.text[size] = 0;
msg.text_annotations.push_back(localText);
}

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\n",
"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_arcs,
msg.header.num_local_text_annotations);
}
return msg;
}
Expand Down Expand Up @@ -283,7 +321,7 @@ bool StringKeyPresent(const QString& key,
}

void RobotWebSocket::ProcessCallback(const QJsonObject& json) {
static const bool kDebug = true;
static const bool kDebug = false;
if (kDebug) {
qInfo() << "Callback JSON:\n" << json;
}
Expand All @@ -310,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");
}
Expand All @@ -328,11 +368,12 @@ void RobotWebSocket::processTextMessage(QString message) {
header.num_points = 40;
header.num_lines = 100;
header.num_arcs = 100;
header.num_text_annotations = 5;
header.num_laser_rays = 270 * 4;
header.laser_min_angle = -135;
header.laser_max_angle = 135;
printf("Test message data size: %lu\n", header.GetByteLength());
const DataMessage data_msg = GenerateTestData(header);
printf("Test message data size: %lu\n", header.GetByteLength());
client->sendBinaryMessage(data_msg.ToByteArray());
}
}
Expand Down
45 changes: 29 additions & 16 deletions src/websocket/websocket.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include "amrl_msgs/ColoredPoint2D.h"
#include "amrl_msgs/ColoredLine2D.h"
#include "amrl_msgs/ColoredArc2D.h"
#include "amrl_msgs/ColoredText.h"
#include "amrl_msgs/VisualizationMsg.h"
#include "sensor_msgs/LaserScan.h"

Expand All @@ -42,36 +43,47 @@ class QWebSocket;

struct MessageHeader {
MessageHeader() : nonce(42) {}
uint32_t nonce; // 1
uint32_t num_points; // 2
uint32_t num_lines; // 3
uint32_t num_arcs; // 4
uint32_t num_laser_rays; // 5
uint32_t num_local_points; // 6
uint32_t num_local_lines; // 7
uint32_t num_local_arcs; // 8
float laser_min_angle; // 9
float laser_max_angle; // 10
float loc_x; // 11
float loc_y; // 12
float loc_r; // 13
char map[32]; //
uint32_t nonce; // 1
uint32_t num_points; // 2
uint32_t num_lines; // 3
uint32_t num_arcs; // 4
uint32_t num_text_annotations; // 5
uint32_t num_laser_rays; // 6
uint32_t num_local_points; // 7
uint32_t num_local_lines; // 8
uint32_t num_local_arcs; // 9
uint32_t num_local_text_annotations;// 10
float laser_min_angle; // 11
float laser_max_angle; // 12
float loc_x; // 13
float loc_y; // 14
float loc_r; // 15
char map[32]; //
size_t GetByteLength() const {
const size_t len = 13 * 4 + 32 +
const size_t len = 15 * 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
num_arcs * 6 * 4; // x, y, radius, start_angle, end_angle, color
num_arcs * 6 * 4 + // x, y, radius, start_angle, end_angle, color
num_text_annotations * 4 * 4 * 32; // x, y, color, size, msg
return len;
}
};

struct ColoredTextNative {
amrl_msgs::Point2D start;
uint32_t color;
float size_em;
char text[32];
};

struct DataMessage {
MessageHeader header;
std::vector<uint32_t> laser_scan;
std::vector<amrl_msgs::ColoredPoint2D> points;
std::vector<amrl_msgs::ColoredLine2D> lines;
std::vector<amrl_msgs::ColoredArc2D> arcs;
std::vector<ColoredTextNative> text_annotations;
QByteArray ToByteArray() const;
static DataMessage FromRosMessages(
const sensor_msgs::LaserScan& laser_msg,
Expand All @@ -95,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();
Expand Down
22 changes: 19 additions & 3 deletions src/websocket/websocket_main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -40,6 +41,7 @@ using amrl_msgs::Localization2DMsg;
using sensor_msgs::LaserScan;
using std::vector;

DEFINE_double(fps, 10.0, "Max visualization frames rate.");
DEFINE_double(max_age, 2.0, "Maximum age of a message before it gets dropped.");
DECLARE_int32(v);

Expand All @@ -50,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
Expand Down Expand Up @@ -108,6 +112,7 @@ void MergeMessage(const VisualizationMsg &m1,
MergeVector(m1.points, &m2.points);
MergeVector(m1.lines, &m2.lines);
MergeVector(m1.arcs, &m2.arcs);
MergeVector(m1.text_annotations, &m2.text_annotations);
}

void DropOldMessages() {
Expand Down Expand Up @@ -167,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",
Expand All @@ -193,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 =
Expand All @@ -205,12 +219,14 @@ void *RosThread(void *arg) {
n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
nav_goal_pub_ =
n.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
amrl_init_loc_pub_ =
amrl_init_loc_pub_ =
n.advertise<amrl_msgs::Localization2DMsg>("/set_pose", 10);
amrl_nav_goal_pub_ =
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(10.0);
RateLoop loop(FLAGS_fps);
while (ros::ok() && run_) {
// Consume all pending messages.
ros::spinOnce();
Expand Down
Loading

0 comments on commit 9b43a26

Please sign in to comment.