Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

waypoint nav interface added #5

Merged
merged 2 commits into from
Nov 3, 2023
Merged

waypoint nav interface added #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
db51966
waypoint nav interface added
sadanand1120 Nov 3, 2023
0390fc5
add a separate reset signal
sadanand1120 Nov 3, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/websocket/websocket.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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>
Loading