diff --git a/config/ouster_jackal_default.yaml b/config/ouster_jackal_default.yaml index e9b4b08d..28aea4e8 100644 --- a/config/ouster_jackal_default.yaml +++ b/config/ouster_jackal_default.yaml @@ -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 diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js index 576688ea..2692390e 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js @@ -48,10 +48,11 @@ import DeleteIcon from "@mui/icons-material/Delete"; /// pose graph constants const ROUTE_TYPE_COLOR = ["#ff0000", "#fd4a18", "#ffa500", "#fecd29", "#ffff00", "#6aaf1e", "#008000", "#000000"]; -const ROUTE_TYPE_WIDTH = [0.2, 0.5, 1.0, 1.5, 2.0, 2.5, 10.0, 0.05]; +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; @@ -1157,11 +1158,11 @@ class GraphMap extends React.Component { }); if (this.annotate_polyline === null) { this.annotate_polyline = L.polyline(latlngs, { - color: "#000000", + color: ANNOTATE_LINE_COLOR, opacity: GRAPH_OPACITY, - weight: GRAPH_WEIGHT, + weight: this.metres2pix(ANNOTATE_LINE_WIDTH), pane: "graph", - lineCap: "butt" + lineCap: "butt", }); this.annotate_polyline.addTo(this.map); } else { @@ -1172,13 +1173,15 @@ 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: ROUTE_TYPE_COLOR[type % ROUTE_TYPE_COLOR.length], - weight: this.metres2pix(ROUTE_TYPE_WIDTH[type % ROUTE_TYPE_COLOR.length]), - opacity: ROUTE_TYPE_OPACITY[type % ROUTE_TYPE_COLOR.length], - lineCap: "butt" + color: ANNOTATE_LINE_COLOR, + opacity: GRAPH_OPACITY, + weight: this.metres2pix(ANNOTATE_LINE_WIDTH), + pane: "graph", + lineCap: "butt", }) ); }; @@ -1204,10 +1207,11 @@ class GraphMap extends React.Component { 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], - weight: this.metres2pix(ROUTE_TYPE_WIDTH[type % ROUTE_TYPE_COLOR.length]), - opacity: ROUTE_TYPE_OPACITY[type % ROUTE_TYPE_COLOR.length], - lineCap: "butt" + color: ANNOTATE_LINE_COLOR, + opacity: GRAPH_OPACITY, + weight: this.metres2pix(ANNOTATE_LINE_WIDTH), + pane: "graph", + lineCap: "butt", }) ); } diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/ToolsMenu.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/ToolsMenu.js index 5d862f17..6bf38038 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/ToolsMenu.js +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/ToolsMenu.js @@ -70,14 +70,6 @@ class ToolsMenu extends React.Component { waypointsMap={this.props.waypointsMap} handleUpdateWaypoint={this.props.handleUpdateWaypoint} /> - selectTool("delete_waypoints")} - onCancel={deselectTool} - waypointsMap={this.props.waypointsMap} - handleUpdateWaypoint={this.props.handleUpdateWaypoint} - /> ); } diff --git a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp index d633c3c6..2f1e3806 100644 --- a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp +++ b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp @@ -20,7 +20,6 @@ #include #include -#include #include #include @@ -206,7 +205,7 @@ class GraphMapServer : public tactic::Graph::Callback, const float dist_thres_ = 0.1; bool initial_pose_set_ = false; - boost::circular_buffer> gps_coords_ {2}; + std::deque> gps_coords_ {2}; }; class RvizGraphMapServer : public GraphMapServer, diff --git a/main/src/vtr_navigation/src/graph_map_server.cpp b/main/src/vtr_navigation/src/graph_map_server.cpp index 8eb46193..71008595 100644 --- a/main/src/vtr_navigation/src/graph_map_server.cpp +++ b/main/src/vtr_navigation/src/graph_map_server.cpp @@ -66,6 +66,7 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node, const auto lng = node->declare_parameter("graph_projection.origin_lng", -79.466092); const auto theta = node->declare_parameter("graph_projection.origin_theta", 0.); const auto scale = node->declare_parameter("graph_projection.scale", 1.); + const auto pose_pub_topic_ = node->declare_parameter("graph_projection.gps_topic", "/novatel/fix"); /// Publishers and services callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -89,7 +90,7 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node, update_waypoint_sub_ = node->create_subscription("update_waypoint", rclcpp::QoS(10), std::bind(&GraphMapServer::updateWaypointCallback, this, std::placeholders::_1), sub_opt); // clang-format on - pose_pub_ = node->create_subscription("/novatel/fix", rclcpp::QoS(10), std::bind(&GraphMapServer::poseCallback, this, std::placeholders::_1), sub_opt); + pose_pub_ = node->create_subscription(pose_pub_topic_, rclcpp::QoS(10), std::bind(&GraphMapServer::poseCallback, this, std::placeholders::_1), sub_opt); // initialize graph mapinfo if working on a new map auto map_info = graph->getMapInfo(); @@ -234,6 +235,7 @@ float GraphMapServer::deltaLatToMetres(float lat1, float lat2) { void GraphMapServer::poseCallback(const NavSatFix::ConstSharedPtr msg) { gps_coords_.push_back(std::make_pair(msg->longitude, msg->latitude)); + gps_coords_.pop_front(); const auto graph = getGraph(); auto map_info = graph->getMapInfo();