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

Jump to bottom

Gui Improvements #225

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
Open

Gui Improvements #225

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ea6da77
Path width reflects actual width; feature to select whole path at onc…
Aug 18, 2024
4fd0db8
Maintain a buffer to determine initial path bearing from GPS topic, w…
Aug 20, 2024
862d5c1
Linting GUI code
pateltanmayp Sep 2, 2024
0a912d8
Refactor to snake case, rename comments
pateltanmayp Sep 10, 2024
04da74e
Response to PR comments
pateltanmayp Sep 23, 2024
2b4852b
Full path selection based on BFS
pateltanmayp Sep 24, 2024
4e11f3c
Minor changes to full path selector; resolve warnings
pateltanmayp Sep 26, 2024
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
1 change: 1 addition & 0 deletions config/ouster_jackal_default.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@
origin_lng: -79.4661
origin_theta: 1.3
scale: 1.0
gps_topic: "/novatel/fix"
tactic:
enable_parallelization: true
preprocessing_skippable: false
Expand Down
186 changes: 153 additions & 33 deletions main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ import React from "react";
import "leaflet/dist/leaflet.css";
import L from "leaflet";
import "leaflet-rotatedmarker"; // enable marker rotation
import { Card, IconButton, TextField } from "@mui/material";
import { Card, IconButton, TextField, Switch, Box, FormControlLabel, Typography } from "@mui/material";
import { MapContainer, TileLayer, ZoomControl, ImageOverlay, Marker, Popup } from "react-leaflet";
import { kdTree } from "kd-tree-javascript";

Expand All @@ -47,9 +47,12 @@ import AddIcon from "@mui/icons-material/Add";
import DeleteIcon from "@mui/icons-material/Delete";

/// pose graph constants
const ROUTE_TYPE_COLOR = ["#ff0000", "#fd4a18", "#ffa500", "#fecd29", "#ffff00", "#6aaf1e", "#008000"];
const ROUTE_TYPE_COLOR = ["#ff0000", "#fd4a18", "#ffa500", "#fecd29", "#ffff00", "#6aaf1e", "#008000", "#000000"];
const ROUTE_TYPE_WIDTH = [0.4, 1.0, 2.0, 3.0, 4.0, 5.0, 20.0, 0.05];
const ROUTE_TYPE_OPACITY = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.9];
const ANNOTATE_LINE_COLOR = "#000000";
const ANNOTATE_LINE_WIDTH = 0.5;
const GRAPH_OPACITY = 0.9;
const GRAPH_WEIGHT = 7;

/// robot constants
const ROBOT_OPACITY = 0.8;
Expand Down Expand Up @@ -143,6 +146,8 @@ class GraphMap extends React.Component {
move_robot_vertex: { lng: 0, lat: 0, id: -1 },
// map center
map_center: {lat: 43.78220, lng: -79.4661},
/// whether the path selection will be the whole path (for annotation)
annotate_full: false,
};
this.fetchMapCenter()
/// leaflet map
Expand Down Expand Up @@ -366,6 +371,30 @@ class GraphMap extends React.Component {
// move robot
moveRobotVertex={move_robot_vertex}
/>
<Box
sx={{
position: "absolute",
bottom: 0,
left: "77%",
transform: "translate(-50%, -50%)",
zIndex: 1000,
display: "flex",
flexDirection: "row",
}}
>
<FormControlLabel control={
<Switch
checked={this.state.annotate_full}
onChange={() => this.setState({annotate_full: !this.state.annotate_full})}
inputProps={{ 'aria-label': 'controlled' }}
/>
} label={
<Typography sx={{ color: "#ffffff" }}>
SELECT FULL PATH
</Typography>
}
/>
</Box>
<GoalManager
socket={socket}
currentTool={current_tool}
Expand Down Expand Up @@ -410,6 +439,25 @@ class GraphMap extends React.Component {
});
}

zoomEnd = () => {
this.fixed_routes.forEach((route) => {
route.polyline.setStyle({
color: ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length],
weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
lineCap: "butt",
})
});
this.active_routes.forEach((route) => {
route.polyline.setStyle({
color: ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length],
weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
lineCap: "butt",
})
});
}

/** @brief Leaflet map creationg callback */
mapCreatedCallback(map) {
console.debug("Leaflet map created.");
Expand All @@ -420,6 +468,8 @@ class GraphMap extends React.Component {
//
this.map.on("click", this.handleMapClick.bind(this));
//
this.map.on("zoomend", this.zoomEnd, this);
//
this.fetchGraphState(true);
}

Expand Down Expand Up @@ -469,7 +519,6 @@ class GraphMap extends React.Component {
if (this.state.new_goal_type === "repeat") {
this.setState({ new_goal_waypoints: [...this.state.new_goal_waypoints, best.target.id] });
}
console.log(this.state.waypoints_map);
}
}

Expand All @@ -486,27 +535,27 @@ class GraphMap extends React.Component {
fetchGraphState(center = false) {
console.info("Fetching the current pose graph state (full).");
fetchWithTimeout("/vtr/graph")
.then((response) => {
if (response.status !== 200) throw new Error("Failed to fetch pose graph state: " + response.status);
response.json().then((data) => {
console.info("Received the pose graph state (full): ", data);
this.loadGraphState(data, center);
// fetch robot and following route after graph has been set
this.fetchRobotState();
this.fetchFollowingRoute();
this.fetchServerState();
});
})
.catch((error) => {
console.error(error);
.then((response) => {
if (response.status !== 200) throw new Error("Failed to fetch pose graph state: " + response.status);
response.json().then((data) => {
console.info("Received the pose graph state (full): ", data);
this.loadGraphState(data, center);
// fetch robot and following route after graph has been set
this.fetchRobotState();
this.fetchFollowingRoute();
this.fetchServerState();
});
})
.catch((error) => {
console.error(error);
});
}

graphStateCallback(graph_state) {
console.info("Received graph state: ", graph_state);
this.loadGraphState(graph_state);
}

/** @brief Helper function to convert a pose graph route to a leaflet polyline, and add it to map */
route2Polyline(route) {
// fixed_routes format: [{type: 0, ids: [id, ...]}, ...]
Expand All @@ -516,7 +565,13 @@ class GraphMap extends React.Component {
let v = this.id2vertex.get(id);
return [v.lat, v.lng];
});
let polyline = L.polyline(latlngs, { color: color, opacity: GRAPH_OPACITY, weight: width, pane: "graph" });
let polyline = L.polyline(latlngs, {
color: color,
weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
pane: "graph",
lineCap: "butt",
});
polyline.addTo(this.map);
return polyline;
}
Expand All @@ -538,24 +593,30 @@ class GraphMap extends React.Component {
wps_map.set(v.id, v.name);
}
});
this.setState({waypoints_map: wps_map, display_waypoints_map: wps_map});
this.setState({waypoints_map: wps_map, display_waypoints_map: wps_map});

this.kdtree = new kdTree(graph.vertices, (a, b) => b.distanceTo(a), ["lat", "lng"]);
// fixed routes
this.fixed_routes.forEach((route) => {
route.polyline.remove();
});
this.fixed_routes = graph.fixed_routes.map((route) => {
this.fixed_routes = graph.fixed_routes.flatMap((route) => {
let polyline = this.route2Polyline(route);
return { ...route, polyline: polyline };
let route_centre = structuredClone(route);
route_centre.type = 7;
let polyline_centre = this.route2Polyline(route_centre);
return [{ ...route, polyline: polyline}, {...route_centre, polyline: polyline_centre}];
});
// active routes
this.active_routes.forEach((route) => {
route.polyline.remove();
});
this.active_routes = graph.active_routes.map((route) => {
this.active_routes = graph.active_routes.flatMap((route) => {
let polyline = this.route2Polyline(route);
return { ...route, polyline: polyline };
let route_centre = structuredClone(route);
route_centre.type = 7;
let polyline_centre = this.route2Polyline(route_centre);
return [{ ...route, polyline: polyline}, {...route_centre, polyline: polyline_centre}];
});

// center set to root vertex
Expand Down Expand Up @@ -730,14 +791,23 @@ class GraphMap extends React.Component {
let active_route = { ids: [vf.id], type: vf.type };
active_route = { ...active_route, polyline: this.route2Polyline(active_route) };
this.active_routes.push(active_route);
let active_route_centre = { ids: [vf.id], type: 7 };
active_route_centre = { ...active_route_centre, polyline: this.route2Polyline(active_route_centre) };
this.active_routes.push(active_route_centre);
}
let active_route = this.active_routes[this.active_routes.length - 1];
let active_route = this.active_routes[this.active_routes.length - 2];
active_route.ids.push(vt.id);
active_route.polyline.addLatLng([vt.lat, vt.lng]);
let active_route_centre = this.active_routes[this.active_routes.length - 1];
active_route_centre.ids.push(vt.id);
active_route_centre.polyline.addLatLng([vt.lat, vt.lng]);
if (active_route.type !== vt.type) {
let new_active_route = { ids: [vt.id], type: vt.type };
new_active_route = { ...new_active_route, polyline: this.route2Polyline(new_active_route) };
this.active_routes.push(new_active_route);
let new_active_route_centre = { ids: [vf.id], type: 7 };
new_active_route_centre = { ...new_active_route_centre, polyline: this.route2Polyline(new_active_route_centre) };
this.active_routes.push(new_active_route_centre);
}
}

Expand Down Expand Up @@ -906,9 +976,17 @@ class GraphMap extends React.Component {
let interm_pos = { s: null, c: null, e: null };

let getSelectedPath = () => {
let paths = this.breadthFirstSearch(selector.vertex.c.id, [selector.vertex.s.id, selector.vertex.e.id]);
paths[0].reverse();
return paths[0].concat(paths[1].slice(1));
if (!this.state.annotate_full) {
let paths = this.breadthFirstSearch(selector.vertex.c.id, [selector.vertex.s.id, selector.vertex.e.id]);
paths[0].reverse();
return paths[0].concat(paths[1].slice(1));
}
else {
let end_id = Math.max(...Array.from(this.id2vertex.keys()));
let paths = this.breadthFirstSearch(selector.vertex.c.id, [0, end_id]);
paths[0].reverse();
return paths[0].concat(paths[1].slice(1));
}
};

let getRotationAngle = (p1, p2) => {
Expand Down Expand Up @@ -969,6 +1047,7 @@ class GraphMap extends React.Component {
// set up and add the center marker
let closest_vertices = this.kdtree.nearest(center_pos, 1);
selector.vertex.c = closest_vertices ? closest_vertices[0][0] : center_pos;
selector.vertex.c = this.id2vertex.get(0);
selector.marker.c = L.marker(selector.vertex.c, {
draggable: true,
icon: SELECTOR_CENTER_ICON,
Expand Down Expand Up @@ -1078,10 +1157,11 @@ class GraphMap extends React.Component {
});
if (this.annotate_polyline === null) {
this.annotate_polyline = L.polyline(latlngs, {
color: ROUTE_TYPE_COLOR[0],
color: ANNOTATE_LINE_COLOR,
opacity: GRAPH_OPACITY,
weight: GRAPH_WEIGHT,
weight: this.metres2pix(ANNOTATE_LINE_WIDTH),
pane: "graph",
lineCap: "butt",
});
this.annotate_polyline.addTo(this.map);
} else {
Expand All @@ -1090,10 +1170,28 @@ class GraphMap extends React.Component {
});
};
this.createSelector(this.annotate_route_selector, selectPathCallback);

this.annotateRouteZoomEnd = () => {

let type = this.state.annotate_route_type;
this.setState({ annotate_route_type: type }, () =>
this.annotate_polyline.setStyle({
color: ANNOTATE_LINE_COLOR,
opacity: GRAPH_OPACITY,
weight: this.metres2pix(ANNOTATE_LINE_WIDTH),
pane: "graph",
lineCap: "butt",
})
);
};
this.map.on("zoomend", this.annotateRouteZoomEnd, this);
}

finishAnnotateRoute() {
console.info("Finish annotating route");
this.map.off("zoomend", this.annotateRouteZoomEnd, this);
this.moveGraphZoomEnd = null;

if (this.annotate_polyline !== null) {
this.annotate_polyline.remove();
this.annotate_polyline = null;
Expand All @@ -1107,7 +1205,13 @@ class GraphMap extends React.Component {
handleAnnotateRouteSliderChange(type) {
if (this.annotate_polyline === null) return;
this.setState({ annotate_route_type: type }, () =>
this.annotate_polyline.setStyle({ color: ROUTE_TYPE_COLOR[type % ROUTE_TYPE_COLOR.length] })
this.annotate_polyline.setStyle({
color: ANNOTATE_LINE_COLOR,
opacity: GRAPH_OPACITY,
weight: this.metres2pix(ANNOTATE_LINE_WIDTH),
pane: "graph",
lineCap: "butt",
})
);
}

Expand Down Expand Up @@ -1333,6 +1437,14 @@ class GraphMap extends React.Component {
this.map.getPane("graph").style.zIndex = -100;
};
this.moveGraphZoomEnd = () => {
this.fixed_routes.forEach((route) => {
route.polyline = this.route2Polyline(route);
});
this.active_routes.forEach((route) => {
route.polyline = this.route2Polyline(route);
});
// display markers
this.scale_marker.setOpacity(MOVE_GRAPH_MARKER_OPACITY2);
// Maintain the relative position of rotMarker and transMarker
let trans_loc_p = this.map.latLngToLayerPoint(trans_loc);
let new_rot_loc_p = trans_loc_p.add(diff_rot_p_zoom);
Expand Down Expand Up @@ -1422,6 +1534,14 @@ class GraphMap extends React.Component {
// add the robot marker back
this.robot_marker.marker.addTo(this.map);
}

metres2pix(metres) {
let origin = L.latLng(this.id2vertex.get(this.root_vid));
let origin_plus_metre = this.map.latLngToLayerPoint(L.latLng(origin.lat - 1, origin.lng));
let metre_standard = origin_plus_metre.subtract(this.map.latLngToLayerPoint(origin));
let factor = metre_standard.y * 0.000009;
return metres * factor;
}
}

export default GraphMap;
export default GraphMap;
Loading
Loading