Skip to content

Commit

Permalink
[Feature] Added gps waypoint visualization and reset ui
Browse files Browse the repository at this point in the history
  • Loading branch information
@artzha
artzha committed Nov 10, 2024
1 parent 079d322 commit 9aadf18
Show file tree
Hide file tree
Showing 4 changed files with 177 additions and 30 deletions.
19 changes: 16 additions & 3 deletions src/websocket/websocket.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ using amrl_msgs::ColoredText;
using amrl_msgs::Localization2DMsg;
using amrl_msgs::Point2D;
using amrl_msgs::VisualizationMsg;
using amrl_msgs::GPSArrayMsg;
using sensor_msgs::LaserScan;
using std::vector;
using std_msgs::Float64MultiArray;
Expand Down Expand Up @@ -193,6 +194,7 @@ DataMessage DataMessage::FromRosMessages(
const VisualizationMsg& local_msg,
const VisualizationMsg& global_msg,
const Localization2DMsg& localization_msg,
const GPSArrayMsg& gps_goals_msg,
const Float64MultiArray& gps_msg) {
static const bool kDebug = false;
DataMessage msg;
Expand All @@ -205,6 +207,14 @@ DataMessage DataMessage::FromRosMessages(
msg.header.lat = gps_msg.data[1];
msg.header.lng = gps_msg.data[2];
msg.header.heading = gps_msg.data[4];
// 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++) {
const auto& route_node = gps_goals_msg.data[i];
msg.header.route[ROUTE_HEADER_SIZE + i*NODE_SIZE] = route_node.latitude;
msg.header.route[ROUTE_HEADER_SIZE + i*NODE_SIZE + 1] = route_node.longitude;
msg.header.route[ROUTE_HEADER_SIZE + i*NODE_SIZE + 2] = route_node.heading;
}
// 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()));
Expand Down Expand Up @@ -343,10 +353,11 @@ void RobotWebSocket::ProcessCallback(const QJsonObject& json) {
json.value("theta").toDouble(),
json.value("map").toString());
} else if (type == "set_gps_goal") {
if (!AllNumericalKeysPresent({"lat", "lon"}, json)) {
if (!AllNumericalKeysPresent({"start_lat", "start_lon", "end_lat", "end_lon"}, json)) {
SendError("Invalid set_gps_goal parameters");
}
SetGPSGoalSignal(json.value("lat").toDouble(), json.value("lon").toDouble());
SetGPSGoalSignal(json.value("start_lat").toDouble(), json.value("start_lon").toDouble(),
json.value("end_lat").toDouble(), json.value("end_lon").toDouble());
} else if (type == "reset_nav_goals") {
ResetNavGoalsSignal();
} else {
Expand Down Expand Up @@ -402,7 +413,7 @@ void RobotWebSocket::SendDataSlot() {
if (clients_.empty()) return;
data_mutex_.lock();
const auto data = DataMessage::FromRosMessages(
laser_scan_, laser_lowbeam_scan_, local_vis_, global_vis_, localization_, gps_pose_);
laser_scan_, laser_lowbeam_scan_, local_vis_, global_vis_, localization_, gps_goals_msg_, gps_pose_);
const auto buffer = data.ToByteArray();
CHECK_EQ(data.header.GetByteLength(), buffer.size());
for (auto c : clients_) {
Expand All @@ -416,13 +427,15 @@ void RobotWebSocket::Send(const VisualizationMsg& local_vis,
const LaserScan& laser_scan,
const LaserScan& laser_lowbeam_scan,
const Localization2DMsg& localization,
const GPSArrayMsg& gps_goals_msg,
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_goals_msg_ = gps_goals_msg;
gps_pose_ = gps_pose;
data_mutex_.unlock();
SendDataSignal();
Expand Down
20 changes: 15 additions & 5 deletions src/websocket/websocket.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,12 +37,18 @@
#include "amrl_msgs/Localization2DMsg.h"
#include "amrl_msgs/Point2D.h"
#include "amrl_msgs/VisualizationMsg.h"
#include "amrl_msgs/GPSArrayMsg.h"
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/Float64MultiArray.h"

class QWebSocketServer;
class QWebSocket;

#define ROUTE_HEADER_SIZE (size_t) 1
#define MAX_ROUTE_NODES (size_t) 30
#define NODE_SIZE (size_t) 3
#define MAX_ROUTE_SIZE (ROUTE_HEADER_SIZE+MAX_ROUTE_NODES*NODE_SIZE)

struct MessageHeader {
MessageHeader() : nonce(42) {}
uint32_t nonce; // 1
Expand All @@ -60,13 +66,14 @@ struct MessageHeader {
float loc_x; // 13
float loc_y; // 14
float loc_r; // 15
float lat; // 16
float lng; // 17
float heading; // 18
float lat; // 16
float lng; // 17
float heading; // 18
float route[MAX_ROUTE_SIZE]; // 19:19+MAX_ROUTE_SIZE
char map[32]; //
size_t GetByteLength() const {
const size_t len =
18 * 4 + 32 + // header fields + map data
18 * 4 + MAX_ROUTE_SIZE*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 Down Expand Up @@ -97,6 +104,7 @@ struct DataMessage {
const amrl_msgs::VisualizationMsg& local_msg,
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);
};

Expand All @@ -110,14 +118,15 @@ class RobotWebSocket : public QObject {
const sensor_msgs::LaserScan& laser_scan,
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);

Q_SIGNALS:
void closed();
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 SetGPSGoalSignal(float start_lat, float start_lon, float end_lat, float end_lon);
void ResetNavGoalsSignal();

private Q_SLOTS:
Expand All @@ -141,6 +150,7 @@ class RobotWebSocket : public QObject {
sensor_msgs::LaserScan laser_scan_;
sensor_msgs::LaserScan laser_lowbeam_scan_;
amrl_msgs::Localization2DMsg localization_;
amrl_msgs::GPSArrayMsg gps_goals_msg_;
std_msgs::Float64MultiArray gps_pose_;
};

Expand Down
58 changes: 44 additions & 14 deletions src/websocket/websocket_main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "amrl_msgs/VisualizationMsg.h"
#include "amrl_msgs/GPSArrayMsg.h"
#include "amrl_msgs/GPSMsg.h"
#include "amrl_msgs/graphNavGPSSrv.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "gflags/gflags.h"
Expand All @@ -44,6 +45,7 @@ using sensor_msgs::LaserScan;
using std::vector;
using amrl_msgs::GPSArrayMsg;
using amrl_msgs::GPSMsg;
using amrl_msgs::graphNavGPSSrv;
using std_msgs::Float64MultiArray;

DEFINE_double(fps, 10.0, "Max visualization frames rate.");
Expand All @@ -55,7 +57,10 @@ 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::GPSMsg robot_gps_msg_;
amrl_msgs::GPSArrayMsg gps_goals_msg_;
ros::ServiceClient client;
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_;
Expand Down Expand Up @@ -161,7 +166,7 @@ void SendUpdate() {
}

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

void SetInitialPose(float x, float y, float theta, QString map) {
Expand All @@ -187,6 +192,7 @@ void ResetNavGoals() {
if (FLAGS_v > 0) {
printf("Reset nav goals.\n");
}
gps_goals_msg_.data.clear();
reset_nav_goals_pub_.publish(reset_nav_goals_msg_);
}

Expand All @@ -209,30 +215,52 @@ 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) {
void SetGPSGoal(float start_lat, float start_lon, float end_lat, float end_lon) {
if (FLAGS_v > 0) {
printf("Set gps goal: %f, %f\n", lat, lon);
printf("Set gps goal: %f, %f\n", end_lat, end_lon);
}
srv.request.start.header.stamp = ros::Time::now();
srv.request.start.latitude = start_lat;
srv.request.start.longitude = start_lon;

gps_goals_msg_.header.stamp = ros::Time::now();
gps_goals_msg_.data.clear();
GPSMsg goal_msg;
goal_msg.header.stamp = ros::Time::now();
goal_msg.latitude = end_lat;
goal_msg.longitude = end_lon;
gps_goals_msg_.data = { goal_msg };

srv.request.goals = gps_goals_msg_;
if (client.call(srv)) {
ROS_INFO("Service call successful");
// Save the response path to the class variable
gps_goals_msg_ = srv.response.plan;

if (FLAGS_v > 0) {
for (const auto& gps_goal : gps_goals_msg_.data) {
ROS_INFO("Path point: %f, %f", gps_goal.latitude, gps_goal.longitude);
}
}
} else {
ROS_ERROR("Service call failed");
}
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_);

ros::NodeHandle n;
client = n.serviceClient<amrl_msgs::graphNavGPSSrv>("graphNavGPSSrv");

QObject::connect(server_, &RobotWebSocket::SetInitialPoseSignal,
&SetInitialPose);
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("/velodyne_2dscan_high_beams", 5, &LaserCallback);
ros::Subscriber laser_lowbeam_sub =
n.subscribe("/velodyne_2dscan_lowbeam", 5, &LaserLowBeamCallback);
Expand All @@ -249,8 +277,8 @@ void *RosThread(void *arg) {
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);
// 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(FLAGS_fps);
Expand Down Expand Up @@ -292,6 +320,8 @@ 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_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;
}
Expand Down
Loading

0 comments on commit 9aadf18

Please sign in to comment.