Skip to content

Commit

Permalink
Merge pull request #5 from ut-amrl/waypoint_beta2
Browse files Browse the repository at this point in the history 
waypoint nav interface added
  • Loading branch information
@sadanand1120
sadanand1120 authored Nov 3, 2023
2 parents 754e321 + 0390fc5 commit e0a92bd
Show file tree
Hide file tree
Showing 4 changed files with 127 additions and 6 deletions.
2 changes: 2 additions & 0 deletions src/websocket/websocket.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -348,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 Down
1 change: 1 addition & 0 deletions src/websocket/websocket.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,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
14 changes: 14 additions & 0 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 Down Expand Up @@ -51,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 @@ -169,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 @@ -195,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 @@ -211,6 +223,8 @@ void *RosThread(void *arg) {
n.advertise<amrl_msgs::Localization2DMsg>("/set_pose", 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);
while (ros::ok() && run_) {
Expand Down
116 changes: 110 additions & 6 deletions webviz.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ <h1>UT AUTOmata Web Interface</h1>
Status: <label id="status">Disconnected</label><br>
<input type="button" id="setLocation" onclick="setLocationButton()" value="Set Pose">
<input type="button" id="setNavGoal" onclick="setNavGoalButton()" value="Set Nav Target">
<input type="button" id="setWaypoints" onclick="setWaypointsButton()" value="Set Waypoints">
<input type="button" id="resetNav" onclick="resetNavButton()" value="Reset Nav" />
Map:
<select id="map" value="GDC1" onchange="changeMap()">
<option value="GDC1">GDC1</option>
Expand Down Expand Up @@ -85,6 +87,8 @@ <h1>UT AUTOmata Web Interface</h1>
var mouseDown = false;
var setLocation = false;
var setNavGoal = false;
var setWaypoints = false;
var resetNavGoals = false;
var coords = document.getElementById("coords");
var mapList = document.getElementById("map");
var robotPose = undefined;
Expand Down Expand Up @@ -177,7 +181,7 @@ <h1>UT AUTOmata Web Interface</h1>
let end_of_str = false;
for (let i = 0; i < 32; i++) {
const c = this.readUint8();
if(c==0 || end_of_str) {
if (c == 0 || end_of_str) {
end_of_str = true;
} else {
s += String.fromCharCode(c)
Expand Down Expand Up @@ -309,6 +313,7 @@ <h1>UT AUTOmata Web Interface</h1>
// Constructor: set up callbacks.
constructor() {
this.socket = null;
this.waypoints = []; // list of tuples (x, y, theta) waypoints
// Set up callbacks.
this.callbacks = {
onopen: null,
Expand Down Expand Up @@ -412,6 +417,46 @@ <h1>UT AUTOmata Web Interface</h1>
}));
}

// Reset all navgoals and waypoints.
resetNavGoals() {
this.waypoints = [];
if (!this.socket) {
console.error('Error: not connected');
return;
}
this.socket.send(JSON.stringify({
type: "reset_nav_goals"
}));
}

// Send waypoint.
async gotoWaypoint(idx) {
this.setNavGoal(this.waypoints[idx][0], this.waypoints[idx][1], this.waypoints[idx][2]);
while (true) {
if (resetNavGoals) {
break;
}
if (comparePoses([robotPose.x, robotPose.y, robotPose.theta], this.waypoints[idx])) {
console.log('Reached the waypoint');
break; // Exit the loop if the robot has reached the waypoint.
}
await new Promise(resolve => setTimeout(resolve, 100)); // Wait for 100 ms.
}
}

async gotoWaypoints() {
for (let i = 0; i < this.waypoints.length; i++) {
if (resetNavGoals) {
break;
}
await this.gotoWaypoint(i);
await new Promise(resolve => setTimeout(resolve, 10000)); // Wait for 10 seconds.
if (i == this.waypoints.length - 1) {
i = -1;
}
}
}

close() {
if (!this.socket) return;
this.socket.close();
Expand Down Expand Up @@ -460,18 +505,70 @@ <h1>UT AUTOmata Web Interface</h1>
xhr.send();
}

function comparePoses(pose1, pose2) {
const [x1, y1, theta1] = pose1;
const [x2, y2, theta2] = pose2;
const positionThreshold = 0.1;
const orientationThreshold = 0.05;

return (
Math.abs(x1 - x2) < positionThreshold &&
Math.abs(y1 - y2) < positionThreshold &&
Math.abs(theta1 - theta2) < orientationThreshold
);
}

function setLocationButton() {
setLocation = true;
setNavGoal = false;
resetNavGoals = false;
setWaypoints = false;
document.getElementById("resetNav").style = "";
document.getElementById("setLocation").style = "border-style:inset;";
}

function setNavGoalButton() {
setLocation = false;
setNavGoal = true;
resetNavGoals = false;
setWaypoints = false;
document.getElementById("resetNav").style = "";
document.getElementById("setNavGoal").style = "border-style:inset;";
}

function setWaypointsButton() {
if (!setWaypoints) {
setLocation = false;
setNavGoal = false;
resetNavGoals = false;
setWaypoints = true;
document.getElementById("resetNav").style = "";
document.getElementById("setWaypoints").style = "border-style:inset;";
} else {
setWaypoints = false;
document.getElementById("setWaypoints").style = "";
robot_interface.gotoWaypoints();
}
}

function resetNavButton() {
if (!resetNavGoals) {
setNavGoal = false;
setLocation = false;
resetNavGoals = true;
setWaypoints = false;
mouseDown = false;
document.getElementById("setLocation").style = "";
document.getElementById("setNavGoal").style = "";
document.getElementById("setWaypoints").style = "";
robot_interface.resetNavGoals();
document.getElementById("resetNav").style = "border-style:inset;";
} else {
resetNavGoals = false;
document.getElementById("resetNav").style = "";
}
}

// Get the "true" mouse location, compensting for scroll, zoom, etc.
function getMouseLocation(event, element) {
var posx = 0;
Expand Down Expand Up @@ -516,7 +613,7 @@ <h1>UT AUTOmata Web Interface</h1>
const y_str = w.y.toFixed(3);
coords.innerHTML = `x=${x_str}, y=${y_str}`;
if (!mouseDown) return;
if (setLocation || setNavGoal) return;
if (setLocation || setNavGoal || setWaypoints) return;
origin.x = prev_origin.x + curMouseLoc.x - mouseDownLoc.x;
origin.y = prev_origin.y + curMouseLoc.y - mouseDownLoc.y;
};
Expand Down Expand Up @@ -549,6 +646,13 @@ <h1>UT AUTOmata Web Interface</h1>
let a = Math.atan2(mouseDownLoc.y - loc.y, loc.x - mouseDownLoc.x);
robot_interface.setInitialPose(w0.x, w0.y, a);
}
} else if (setWaypoints) {
// Set waypoints here.
if (robot_interface) {
let w0 = canvasToWorld(mouseDownLoc);
let a = Math.atan2(mouseDownLoc.y - loc.y, loc.x - mouseDownLoc.x);
robot_interface.waypoints.push([w0.x, w0.y, a]);
}
} else {
// Pan.
origin.x = prev_origin.x + loc.x - mouseDownLoc.x;
Expand Down Expand Up @@ -600,10 +704,10 @@ <h1>UT AUTOmata Web Interface</h1>
}

function drawTextAnnotation(text, x, y, color, size_em) {
ctx.font = `${Math.round(size_em*12)}pt Arial`;
ctx.font = `${Math.round(size_em * 12)}pt Arial`;
ctx.fillStyle = color;
ctx.strokeStyle = color;
const p = worldToCanvas({x: x, y: y})
const p = worldToCanvas({ x: x, y: y })
ctx.fillText(text, p.x, p.y);
}

Expand Down Expand Up @@ -792,7 +896,7 @@ <h1>UT AUTOmata Web Interface</h1>
if (visMsg == undefined) return;
visMsg.textAnnotations.forEach(function (m, i) {
let p = m.start;
if(i < visMsg.header.num_local_text_annotations) {
if (i < visMsg.header.num_local_text_annotations) {
p = robotToMap(p);
}
drawTextAnnotation(m.str, p.x, p.y, m.color, m.size_em);
Expand Down Expand Up @@ -964,4 +1068,4 @@ <h1>UT AUTOmata Web Interface</h1>
</script>
</body>

</html>
</html>

0 comments on commit e0a92bd

Please sign in to comment.