From 69197cf12c3703eef2b29badf6e79eeb7cae2c99 Mon Sep 17 00:00:00 2001 From: artzha Date: Wed, 13 Nov 2024 17:29:12 -0600 Subject: [PATCH] [Feature] Integrated GPS waypoint global planning --- src/navigation/graph_domain.h | 140 +++++++++++++++++------------- src/navigation/navigation.cc | 90 +++++++++++++++---- src/navigation/navigation.h | 4 + src/navigation/navigation_main.cc | 7 ++ src/navigation/osm_planner.h | 138 ++++++++++++++--------------- src/shared | 2 +- 6 files changed, 228 insertions(+), 153 deletions(-) diff --git a/src/navigation/graph_domain.h b/src/navigation/graph_domain.h index 1779ee5..d4fa4b3 100644 --- a/src/navigation/graph_domain.h +++ b/src/navigation/graph_domain.h @@ -19,18 +19,19 @@ */ //======================================================================== -// NOTE: This file is an exact copy of `src/navigation_map/navigation_map.h` in `https://github.com/ut-amrl/vector_display`. This should be kept in sync. +// NOTE: This file is an exact copy of `src/navigation_map/navigation_map.h` in +// `https://github.com/ut-amrl/vector_display`. This should be kept in sync. // C headers. #include // C++ headers. #include +#include #include #include #include #include -#include // Library headers. #include "eigen3/Eigen/Dense" @@ -43,13 +44,11 @@ #include "math/geometry.h" #include "math/line2d.h" #include "math/math_util.h" +#include "navigation_parameters.h" +#include "nlohmann/json.hpp" #include "ros/ros_helpers.h" #include "util/helpers.h" - #include "vector_map/vector_map.h" -#include "navigation_parameters.h" - -#include "nlohmann/json.hpp" using json = nlohmann::json; #ifndef GRAPH_DOMAIN_H @@ -76,7 +75,8 @@ struct GraphDomain { static State fromJSON(const json& j) { State s; s.id = j["id"].get(); - s.loc = Eigen::Vector2f(j["loc"]["x"].get(), j["loc"]["y"].get()); + s.loc = Eigen::Vector2f(j["loc"]["x"].get(), + j["loc"]["y"].get()); return s; } }; @@ -99,12 +99,15 @@ struct GraphDomain { data["max_clearance"] = max_clearance; data["has_door"] = has_door; data["has_stairs"] = has_stairs; - // no need to save the edge, it's redundant data that can be recovered at load time + // no need to save the edge, it's redundant data that can be recovered at + // load time return data; } - // Constructing a navigation edge from JSON assumes you already know the states that exist in the world - static NavigationEdge fromJSON(const json& j, const std::vector& states) { + // Constructing a navigation edge from JSON assumes you already know the + // states that exist in the world + static NavigationEdge fromJSON(const json& j, + const std::vector& states) { NavigationEdge e; e.s0_id = j["s0_id"].get(); e.s1_id = j["s1_id"].get(); @@ -120,11 +123,10 @@ struct GraphDomain { } }; - GraphDomain() { - this->params_ = new NavigationParameters(); - } + GraphDomain() { this->params_ = new NavigationParameters(); } - explicit GraphDomain(const std::string& map_file, const navigation::NavigationParameters* params) { + explicit GraphDomain(const std::string& map_file, + const navigation::NavigationParameters* params) { // Load graph from file. Load(map_file); this->params_ = params; @@ -135,9 +137,7 @@ struct GraphDomain { return states[key]; } - uint64_t StateToKey(const State& s) const { - return s.id; - } + uint64_t StateToKey(const State& s) const { return s.id; } // Return the edge cost, assuming the two states are indeed connectable. float EdgeCost(const State& s0, const State& s1) const { @@ -166,8 +166,10 @@ struct GraphDomain { CHECK_LT(s_key, states.size()); state_neighbors->clear(); for (const NavigationEdge& e : edges) { - if (e.s0_id == s_key && (!e.has_stairs || params_->can_traverse_stairs)) state_neighbors->push_back(e.s1_id); - if (e.s1_id == s_key && (!e.has_stairs || params_->can_traverse_stairs)) state_neighbors->push_back(e.s0_id); + if (e.s0_id == s_key && (!e.has_stairs || params_->can_traverse_stairs)) + state_neighbors->push_back(e.s1_id); + if (e.s1_id == s_key && (!e.has_stairs || params_->can_traverse_stairs)) + state_neighbors->push_back(e.s0_id); } } @@ -197,14 +199,16 @@ struct GraphDomain { return found; } - bool GetClosestEdge(const Eigen::Vector2f& v, NavigationEdge* closest_edge, float* closest_dist) const { + bool GetClosestEdge(const Eigen::Vector2f& v, NavigationEdge* closest_edge, + float* closest_dist) const { bool found; closest_edge->s0_id = -1; closest_edge->s1_id = -1; if (edges.empty()) return closest_dist; for (const NavigationEdge& e : edges) { const float dist = e.edge.Distance(v); - if (dist < *closest_dist && (!e.has_stairs || params_->can_traverse_stairs)) { + if (dist < *closest_dist && + (!e.has_stairs || params_->can_traverse_stairs)) { *closest_dist = dist; *closest_edge = e; found = true; @@ -213,7 +217,6 @@ struct GraphDomain { return found; } - uint64_t AddState(const Eigen::Vector2f& v) { State s(states.size(), v); GrowIfNeeded(s.id); @@ -249,20 +252,16 @@ struct GraphDomain { // This edge must exist, hence the list can't be empty. CHECK_GT(edges.size(), 0); for (int i = edges.size() - 1; i >= 0; --i) { - if ((edges[i].s0_id == s0 && - edges[i].s1_id == s1) || (edges[i].s0_id == s1 && - edges[i].s1_id == s0)) { + if ((edges[i].s0_id == s0 && edges[i].s1_id == s1) || + (edges[i].s0_id == s1 && edges[i].s1_id == s0)) { edges.erase(edges.begin() + i); } } } - void AddUndirectedEdge(const uint64_t s0, - const uint64_t s1, - const float max_speed, - const float max_clearance, - const bool has_door, - const bool has_stairs) { + void AddUndirectedEdge(const uint64_t s0, const uint64_t s1, + const float max_speed, const float max_clearance, + const bool has_door, const bool has_stairs) { CHECK_LT(s0, states.size()); CHECK_LT(s1, states.size()); NavigationEdge e; @@ -310,20 +309,23 @@ struct GraphDomain { return 0; } // Find the projection of v on to the line segment p0 : p1; - const Eigen::Vector2f pmid = - geometry::ProjectPointOntoLineSegment(v, closest_edge.edge.p0, closest_edge.edge.p1); + const Eigen::Vector2f pmid = geometry::ProjectPointOntoLineSegment( + v, closest_edge.edge.p0, closest_edge.edge.p1); // Add p0 : pmid const uint64_t pmid_id = AddState(pmid); const uint64_t p0_id = closest_edge.s0_id; const uint64_t p1_id = closest_edge.s1_id; - // NOTE: Here, we are assuming that any dynamic edges added are free of stairs and doors. - // Additionally, we adopt the max_speed and max_clearance of the closest edge. - AddUndirectedEdge(p0_id, pmid_id, closest_edge.max_speed, closest_edge.max_clearance, false, false); + // NOTE: Here, we are assuming that any dynamic edges added are free of + // stairs and doors. Additionally, we adopt the max_speed and max_clearance + // of the closest edge. + AddUndirectedEdge(p0_id, pmid_id, closest_edge.max_speed, + closest_edge.max_clearance, false, false); // Add p1 : pmid - AddUndirectedEdge(p1_id, pmid_id, closest_edge.max_speed, closest_edge.max_clearance, false, false); + AddUndirectedEdge(p1_id, pmid_id, closest_edge.max_speed, + closest_edge.max_clearance, false, false); // Delete p0 : p1, since there is a p0 : pmid : p1 pathway now. DeleteUndirectedEdge(p0_id, p1_id); @@ -331,13 +333,15 @@ struct GraphDomain { // Add pmid : v const uint64_t v_id = AddState(v); - // NOTE: Here, we are assuming that any dynamic edges added are free of stairs and doors. - // Additionally, we adopt the max_speed and max_clearance of the closest edge. - AddUndirectedEdge(pmid_id, v_id, closest_edge.max_speed, closest_edge.max_clearance, false, false); + // NOTE: Here, we are assuming that any dynamic edges added are free of + // stairs and doors. Additionally, we adopt the max_speed and max_clearance + // of the closest edge. + AddUndirectedEdge(pmid_id, v_id, closest_edge.max_speed, + closest_edge.max_clearance, false, false); if (kDebug) { - printf("Adding dynamic state %f,%f (%lu) %lu %lu %lu\n", - v.x(), v.y(), v_id, p0_id, p1_id, pmid_id); + printf("Adding dynamic state %f,%f (%lu) %lu %lu %lu\n", v.x(), v.y(), + v_id, p0_id, p1_id, pmid_id); } return v_id; } @@ -346,14 +350,13 @@ struct GraphDomain { bool Save(const std::string& file) { json j; std::vector state_jsons; - for (const State& s: static_states) { + for (const State& s : static_states) { state_jsons.push_back(s.toJSON()); } j["nodes"] = state_jsons; - std::vector edge_jsons; - for (const NavigationEdge& e: edges) { + for (const NavigationEdge& e : edges) { edge_jsons.push_back(e.toJSON()); } j["edges"] = edge_jsons; @@ -370,11 +373,31 @@ struct GraphDomain { printf("%lu: %8.3f,%8.3f\n", s.id, s.loc.x(), s.loc.y()); } printf("===Edges===\n"); - for(const NavigationEdge& e : edges) { + for (const NavigationEdge& e : edges) { printf("%lu <-> %lu\n", e.s0_id, e.s1_id); } }; + // Load from a preplanned set of points and edges. + bool Load(const std::vector& points, + const std::vector& points_edges) { + static const bool kDebug = false; + states.clear(); + edges.clear(); + for (size_t i = 0; i < points.size(); ++i) { + GrowIfNeeded(i); + states[i] = State(i, points[i]); + } + for (size_t i = 0; i < points_edges.size(); ++i) { + AddUndirectedEdge(points_edges[i].x(), points_edges[i].y()); + } + printf("Loaded with %lu states, %lu edges\n", states.size(), edges.size()); + if (kDebug) DrawMap(); + static_edges = edges; + static_states = states; + return true; + } + // Load from a V2 map file. bool Load(const std::string& file) { static const bool kDebug = false; @@ -391,7 +414,7 @@ struct GraphDomain { states.resize(states_json.size()); edges.clear(); - for(const json& j : states_json) { + for (const json& j : states_json) { State s = State::fromJSON(j); states[s.id] = s; } @@ -399,14 +422,12 @@ struct GraphDomain { CHECK(j["edges"].is_array()); auto const edges_json = j["edges"]; - for(const json& j : edges_json) { + for (const json& j : edges_json) { AddUndirectedEdge(j); } - printf("Loaded %s with %lu states, %lu edges\n", - file.c_str(), - states.size(), - edges.size()); + printf("Loaded %s with %lu states, %lu edges\n", file.c_str(), + states.size(), edges.size()); if (kDebug) DrawMap(); @@ -425,9 +446,8 @@ struct GraphDomain { int num_edges = 0; int num_neighbors = 0; states.clear(); - while (valid && - !feof(fid()) && - fscanf(fid(), "%lu, %f, %f, %d", &id, &x, &y, &num_neighbors) == 4) { + while (valid && !feof(fid()) && + fscanf(fid(), "%lu, %f, %f, %d", &id, &x, &y, &num_neighbors) == 4) { GrowIfNeeded(id); states[id] = State(id, x, y); for (int i = 0; i < num_neighbors; ++i) { @@ -442,10 +462,8 @@ struct GraphDomain { } } - printf("Loaded %s with %d states, %d edges\n", - in_file.c_str(), - static_cast(states.size()), - num_edges); + printf("Loaded %s with %d states, %d edges\n", in_file.c_str(), + static_cast(states.size()), num_edges); DrawMap(); @@ -454,8 +472,7 @@ struct GraphDomain { return true; } - void GetClearanceAndSpeedFromLoc(const Eigen::Vector2f& p, - float* clearance, + void GetClearanceAndSpeedFromLoc(const Eigen::Vector2f& p, float* clearance, float* speed) const { if (edges.empty()) return; NavigationEdge closest_edge; @@ -474,6 +491,5 @@ struct GraphDomain { const navigation::NavigationParameters* params_; }; - } // namespace navigation #endif // GRAPH_DOMAIN_H diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index b8e5cd9..26797ea 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -321,6 +321,7 @@ void Navigation::SetNavGoal(const Vector2f& loc, float angle) { nav_goal_loc_ = loc; nav_goal_angle_ = angle; plan_path_.clear(); + if (FLAGS_v > 0) printf("SetNavGoal(): %f %f %f\n", loc.x(), loc.y(), angle); } void Navigation::SetGPSNavGoals(const vector& goals) { @@ -332,6 +333,8 @@ void Navigation::SetGPSNavGoals(const vector& goals) { gps_nav_goals_loc_ = goals; gps_goal_index_ = 0; plan_path_.clear(); + if (FLAGS_v > 0) + printf("SetGPSNavGoals(): %d\n", int(gps_nav_goals_loc_.size())); } void Navigation::ResetNavGoals() { @@ -339,6 +342,7 @@ void Navigation::ResetNavGoals() { nav_goal_loc_ = robot_loc_; nav_goal_angle_ = robot_angle_; local_target_.setZero(); + if (FLAGS_v > 0) printf("ResetNavGoals()\n"); plan_path_.clear(); } @@ -360,6 +364,7 @@ void Navigation::UpdateMap(const string& map_path) { plan_path_.clear(); prev_obstacles_.clear(); costmap_obstacles_.clear(); + if (FLAGS_v > 0) printf("UpdateMap(): %s\n", map_path.c_str()); } void Navigation::UpdateLocation(const Eigen::Vector2f& loc, float angle) { @@ -570,14 +575,31 @@ void Navigation::ObserveImage(cv::Mat image, double time) { vector Navigation::GlobalPlan(const Vector2f& initial, const Vector2f& end) { + const bool kDebug = FLAGS_v > 0; + + if (kDebug) + printf("GlobalPlan(): Planning from %f %f to %f %f\n", initial.x(), + initial.y(), end.x(), end.y()); auto plan = Plan(initial, end); std::vector path; for (auto& node : plan) { path.push_back(node.id); } + if (kDebug) printf("GlobalPlan(): Path size %d\n", int(path.size())); return path; } +std::vector Navigation::GPSRouteToMap( + const std::vector& route) { + std::vector map_route; + for (const auto& point : route) { + Eigen::Vector2f map_point = + gps_translator_.GPSToMetric(point.lat, point.lon).cast(); + map_route.emplace_back(map_point); + } + return map_route; +} + vector Navigation::GlobalPlan(const GPSPoint& inital, const vector& goals) { vector path; @@ -589,6 +611,18 @@ vector Navigation::GlobalPlan(const GPSPoint& inital, } start = subgoal; } + CHECK(!path.empty()); + CHECK(gps_translator_initialized_); + // Override planning domain with GPS points in map frame + printf("Path size: %d\n", int(path.size())); + const auto& nodes = this->GPSRouteToMap(path); + vector edges(nodes.size() - 1); + for (size_t i = 0; i < path.size() - 1; ++i) { + edges[i] = Vector2f(i, i + 1); + } + planning_domain_.ResetDynamicStates(); + planning_domain_.Load(nodes, edges); // ids correspond to indices + return path; } @@ -616,22 +650,24 @@ vector Navigation::Plan(const Vector2f& initial, start.loc.x(), start.loc.y(), goal_id, goal.loc.x(), goal.loc.y()); } const bool found_path = - AStar(start, goal, planning_domain_, &graph_viz, &plan_path_); + AStar(start, goal, planning_domain_, &graph_viz, &path); if (found_path) { - // CHECK(path.size() > 0); - CHECK(plan_path_.size() > 0); + CHECK(path.size() > 0); printf("Path found!\n"); - Vector2f s1 = plan_path_[0].loc; // BUG HERE plan_path_ is empty + Vector2f s1 = path[0].loc; // BUG HERE plan_path_ is empty printf("Path: %f %f\n", s1.x(), s1.y()); - for (size_t i = 1; i < plan_path_.size(); ++i) { - Vector2f s2 = plan_path_[i].loc; + for (size_t i = 1; i < path.size(); ++i) { + Vector2f s2 = path[i].loc; s1 = s2; } } else { printf("No path found!\n"); } - + if (kDebug) { + printf("Plan(): Path size %d\n", int(path.size())); + } if (params_.do_intermed) { + if (kDebug) printf("Plan(): Performing intermediate planning\n"); std::priority_queue, CompareCost> intermediate_queue; // @@ -872,10 +908,14 @@ bool Navigation::GetLocalCarrot(Vector2f& carrot) { } bool Navigation::GetCarrot(Vector2f& carrot, bool global, float carrot_dist) { + const bool kDebug = FLAGS_v > 1; vector plan_path = plan_path_; if (global) { plan_path = global_plan_path_; } + if (kDebug) + printf("GetCarrot(): Global: %d plan_path size %d\n", global, + int(plan_path.size())); const float kSqCarrotDist = Sq(carrot_dist); // CHECK_GE(plan_path.size(), 2u); @@ -1547,23 +1587,37 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, GPSPoint next_nav_goal_loc = gps_nav_goals_loc_[gps_goal_index_]; bool isGPSGoalReached = osm_planner_.isGoalReached(robot_gps_loc_, next_nav_goal_loc); - bool isGoalStillValid = true; // TODO: Implement function to check straight - // line distance to goal line segment + bool isGPSGoalStillValid = true; + if (!plan_path_.empty()) { + isGPSGoalStillValid = PlanStillValid(); + } + if (isGPSGoalReached) { + if (kDebug) printf("GPS Goal reached\n"); if (gps_goal_index_ + 1 < int(gps_nav_goals_loc_.size())) { - gps_goal_index_++; - nav_goal_loc_ = {next_nav_goal_loc.lat, next_nav_goal_loc.lon}; - nav_goal_angle_ = next_nav_goal_loc.heading; + if (kDebug) printf("Switching to next GPS Goal\n"); + ++gps_goal_index_; + nav_goal_loc_ = + gps_translator_ + .GPSToMetric(gps_nav_goals_loc_[gps_goal_index_].lat, + gps_nav_goals_loc_[gps_goal_index_].lon) + .cast(); + nav_goal_angle_ = 0; // TODO: Change this } else { nav_state_ = NavigationState::kStopped; } - } else if (!isGoalStillValid) { + } else if (!isGPSGoalStillValid) { + if (kDebug) printf("GPS Goal invalid\n"); // Slice gps_nav_goals_loc_ from gps_goal_index_ to end gps_nav_goals_loc_.assign(1, gps_nav_goals_loc_.back()); + // Convert robot gps to map frame const auto& route = this->GlobalPlan(robot_gps_loc_, gps_nav_goals_loc_); + const auto& map_route = this->GPSRouteToMap(route); this->SetGPSNavGoals(route); - } else { - nav_goal_loc_ = {next_nav_goal_loc.lat, next_nav_goal_loc.lon}; + nav_goal_loc_ = + gps_translator_ + .GPSToMetric(next_nav_goal_loc.lat, next_nav_goal_loc.lon) + .cast(); nav_goal_angle_ = next_nav_goal_loc.heading; } } @@ -1574,7 +1628,11 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, if ((!params_.do_intermed && !PlanStillValid()) || (params_.do_intermed && (!PlanStillValid() || !IntermediatePlanStillValid()))) { - if (kDebug) printf("Replanning\n"); + if (kDebug) { + printf("Replanning robot_loc_ %f %f to nav_goal_loc_ %f %f\n", + robot_loc_.x(), robot_loc_.y(), nav_goal_loc_.x(), + nav_goal_loc_.y()); + } plan_path_ = Plan(robot_loc_, nav_goal_loc_); } diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h index 8087a51..7bed604 100644 --- a/src/navigation/navigation.h +++ b/src/navigation/navigation.h @@ -193,6 +193,10 @@ class Navigation { Eigen::Vector2f GetIntermediateGoal(); void UpdateRobotLocFromOdom(); + // Converts a route of GPS points to a route of map points + std::vector GPSRouteToMap( + const std::vector& route); + private: // Test 1D TOC motion in a straight line. void TrapezoidTest(Eigen::Vector2f& cmd_vel, float& cmd_angle_vel); diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index c01437c..2043291 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -369,6 +369,13 @@ bool GPSPlanServiceCb(graphNavGPSSrv::Request& req, goal.heading); } const auto& route = navigation_.GlobalPlan(start, goals); + const auto& map_route = navigation_.GPSRouteToMap(route); + global_viz_msg_.lines.clear(); + for (const auto& p : map_route) { + visualization::DrawPoint(Vector2f(p.x(), p.y()), 0xFF0000, global_viz_msg_); + } + viz_pub_.publish(global_viz_msg_); + printf("Goals in osrm plan: %d\n", int(route.size())); navigation_.SetGPSNavGoals(route); GPSArrayMsg gps_goals_msg; diff --git a/src/navigation/osm_planner.h b/src/navigation/osm_planner.h index a33ee4c..56abcc9 100644 --- a/src/navigation/osm_planner.h +++ b/src/navigation/osm_planner.h @@ -22,28 +22,6 @@ using std::string; using std::vector; using namespace gps_util; -struct GPSPoint { - GPSPoint() : time(0), lat(0), lon(0) {} - GPSPoint(double time, double lat, double lon) - : time(time), lat(lat), lon(lon) {} - GPSPoint(double time, double lat, double lon, double heading) - : time(time), lat(lat), lon(lon), heading(heading) {} - GPSPoint(double lat, double lon) : time(0), lat(lat), lon(lon) {} - - bool operator==(const GPSPoint &other) const { - return lat == other.lat && lon == other.lon; - } - - bool operator!=(const GPSPoint &other) const { - return !(*this == other); - } - - double time; - double lat; - double lon; - double heading; // True North -}; - class OSMPlanner { public: OSMPlanner() = default; @@ -102,53 +80,60 @@ class OSMPlanner { vector path_coordinates; if (status == osrm::Status::Ok) { - auto &json_result = result.get(); - auto &routes = json_result.values["routes"].get(); - auto &route = routes.values.at(0).get(); - const auto &geometry_encoded = route.values["geometry"].get().value; - path_coordinates = this->decodePolyline(geometry_encoded); - - // Interpolate additional points to achieve 20-meter spacing - vector dense_path; - - // Ensure the first point is exactly the start location - dense_path.push_back(start); - - double accumulated_distance = 0.0; - for (size_t i = 1; i < path_coordinates.size(); ++i) { - GPSPoint prev_point = path_coordinates[i - 1]; - GPSPoint curr_point = path_coordinates[i]; - - // Compute the distance between previous and current points - auto distance = gpsDistance(prev_point.lat, prev_point.lon, curr_point.lat, curr_point.lon); - - accumulated_distance += distance; - - // If accumulated distance is 20 meters or more, interpolate a point - while (accumulated_distance >= osrm_path_resolution_) { - double ratio = (osrm_path_resolution_ - (accumulated_distance - distance)) / distance; - - // Interpolate latitude and longitude - double interp_lat = prev_point.lat + ratio * (curr_point.lat - prev_point.lat); - double interp_lon = prev_point.lon + ratio * (curr_point.lon - prev_point.lon); - - dense_path.emplace_back(GPSPoint{interp_lat, interp_lon}); - - accumulated_distance -= osrm_path_resolution_; - } + auto &json_result = result.get(); + auto &routes = + json_result.values["routes"].get(); + auto &route = routes.values.at(0).get(); + const auto &geometry_encoded = + route.values["geometry"].get().value; + path_coordinates = this->decodePolyline(geometry_encoded); + + // Interpolate additional points to achieve 20-meter spacing + vector dense_path; + + // Ensure the first point is exactly the start location + dense_path.push_back(start); + + double accumulated_distance = 0.0; + for (size_t i = 1; i < path_coordinates.size(); ++i) { + GPSPoint prev_point = path_coordinates[i - 1]; + GPSPoint curr_point = path_coordinates[i]; + + // Compute the distance between previous and current points + auto distance = gpsDistance(prev_point.lat, prev_point.lon, + curr_point.lat, curr_point.lon); + + accumulated_distance += distance; + + // If accumulated distance is 20 meters or more, interpolate a point + while (accumulated_distance >= osrm_path_resolution_) { + double ratio = + (osrm_path_resolution_ - (accumulated_distance - distance)) / + distance; + + // Interpolate latitude and longitude + double interp_lat = + prev_point.lat + ratio * (curr_point.lat - prev_point.lat); + double interp_lon = + prev_point.lon + ratio * (curr_point.lon - prev_point.lon); + + dense_path.emplace_back(GPSPoint{interp_lat, interp_lon}); + + accumulated_distance -= osrm_path_resolution_; } + } - // Ensure the last point is exactly the end location - if (dense_path.back() != path_coordinates.back()) { - dense_path.push_back(path_coordinates.back()); - } + // Ensure the last point is exactly the end location + if (dense_path.back() != path_coordinates.back()) { + dense_path.push_back(path_coordinates.back()); + } - return dense_path; + return dense_path; } else { - std::cerr << "Error: Failed to retrieve route.\n"; + std::cerr << "Error: Failed to retrieve route.\n"; } return {}; -} + } // vector plan(GPSPoint start, GPSPoint end) { // osrm::RouteParameters params; @@ -164,9 +149,11 @@ class OSMPlanner { // if (status == osrm::Status::Ok) { // auto &json_result = result.get(); - // auto &routes = json_result.values["routes"].get(); - // auto &route = routes.values.at(0).get(); - // const auto &geometry_encoded = route.values["geometry"].get().value; + // auto &routes = + // json_result.values["routes"].get(); auto + // &route = routes.values.at(0).get(); const + // auto &geometry_encoded = + // route.values["geometry"].get().value; // path_coordinates = this->decodePolyline(geometry_encoded); // // Interpolate additional points to achieve 20-meter spacing @@ -179,20 +166,23 @@ class OSMPlanner { // GPSPoint curr_point = path_coordinates[i]; // // Compute the distance between previous and current points - // auto distance = gpsDistance(prev_point.lat, prev_point.lon, curr_point.lat, curr_point.lon); + // auto distance = gpsDistance(prev_point.lat, prev_point.lon, + // curr_point.lat, curr_point.lon); // accumulated_distance += distance; - - // // If accumulated distance is 20 meters or more, interpolate a point - // while (accumulated_distance >= 20.0) { - // double ratio = (20.0 - (accumulated_distance - distance)) / distance; + + // // If accumulated distance is 20 meters or more, interpolate a + // point while (accumulated_distance >= 20.0) { + // double ratio = (20.0 - (accumulated_distance - distance)) / + // distance; // // Interpolate latitude and longitude - // double interp_lat = prev_point.lat + ratio * (curr_point.lat - prev_point.lat); - // double interp_lon = prev_point.lon + ratio * (curr_point.lon - prev_point.lon); + // double interp_lat = prev_point.lat + ratio * + // (curr_point.lat - prev_point.lat); double interp_lon = + // prev_point.lon + ratio * (curr_point.lon - prev_point.lon); // dense_path.emplace_back(GPSPoint{interp_lat, interp_lon}); - + // accumulated_distance -= 20.0; // } // } diff --git a/src/shared b/src/shared index 3e81caa..08ca139 160000 --- a/src/shared +++ b/src/shared @@ -1 +1 @@ -Subproject commit 3e81caa6c38ca432368e8afee4f8e7757c20f24a +Subproject commit 08ca139659076575e59fb3b2599aa36f31a6f98a