From 53fe350ab92a59c9d21bae16fe0471b56c6faa2f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 29 Nov 2023 12:33:37 +0000 Subject: [PATCH] Remove unnecessary debug output Signed-off-by: Michael X. Grey --- rmf_traffic/include/rmf_traffic/agv/Graph.hpp | 8 ++++-- rmf_traffic/src/rmf_traffic/agv/Planner.cpp | 15 ----------- .../agv/planning/DifferentialDrivePlanner.cpp | 27 ------------------- 3 files changed, 6 insertions(+), 44 deletions(-) diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index 4f9d162b..b8707907 100644 --- a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp +++ b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp @@ -29,6 +29,7 @@ #include #include #include +#include namespace rmf_traffic { namespace agv { @@ -468,8 +469,11 @@ class Graph template DerivedExecutor& execute(DerivedExecutor& executor) const { - return static_cast(execute( - static_cast(executor))); + Executor& base_executor = static_cast(executor); + std::cout << "executing " << this << " : " << &executor + << " -> " << &base_executor << std::endl; + + return static_cast(execute(base_executor)); } /// Execute this event diff --git a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp index c976a4ae..5bd91d46 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp @@ -1158,8 +1158,6 @@ std::vector compute_plan_starts( const double max_merge_lane_distance, const double min_lane_length) { - std::stringstream ss; - ss << "Computing plan starts:"; const Eigen::Vector2d p_location = {pose[0], pose[1]}; const double start_yaw = pose[2]; @@ -1211,9 +1209,6 @@ std::vector compute_plan_starts( const double dp1 = (p_location - p1).norm(); if (dp0 < merge_dist || dp1 < merge_dist) { - ss << "\n -- " << __LINE__ << ": " << wp0.name_or_index() << " -> " - << wp1.name_or_index() - << " | " << dp0 << ", " << dp1 << " vs " << merge_dist; starts.emplace_back( Plan::Start( start_time, lane.exit().waypoint_index(), @@ -1240,9 +1235,6 @@ std::vector compute_plan_starts( if (!raw_starts.insert(entry_waypoint_index).second) continue; - ss << "\n -- " << __LINE__ << ": " << wp0.name_or_index() - << " -> " << wp1.name_or_index() - << " " << dist_to_entry << " vs " << merge_dist; starts.emplace_back( Plan::Start( start_time, entry_waypoint_index, start_yaw, p_location)); @@ -1262,9 +1254,6 @@ std::vector compute_plan_starts( if (!raw_starts.insert(exit_waypoint_index).second) continue; - ss << "\n -- " << __LINE__ << ": " << wp0.name_or_index() - << " -> " << wp1.name_or_index() - << " " << dist_to_exit << " vs " << merge_dist; starts.emplace_back( Plan::Start( start_time, exit_waypoint_index, start_yaw, p_location)); @@ -1282,16 +1271,12 @@ std::vector compute_plan_starts( if (lane_dist < merge_dist) { - ss << "\n -- " << __LINE__ << ": " << wp0.name_or_index() - << " -> " << wp1.name_or_index() - << " " << lane_dist << " vs " << max_merge_lane_distance; starts.emplace_back( Plan::Start( start_time, exit_waypoint_index, start_yaw, p_location, i)); } } } - std::cout << ss.str() << std::endl; return starts; } diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp index b6f3f8aa..36e3ec5e 100644 --- a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp @@ -186,21 +186,6 @@ std::vector reconstruct_nodes( const double rotational_threshold) { auto node_sequence = reconstruct_nodes(finish_node); - std::stringstream ss; - ss << " ---------- Plan Nodes"; - for (const auto& node : node_sequence) - { - ss << "\n -- line " << node->line; - if (node->waypoint.has_value()) - ss << " wp:" << *node->waypoint; - else - ss << " wp:(null)"; - ss << " " << node->position.transpose() << " approach: "; - for (const auto& l : node->approach_lanes) - ss << " " << l; - } - std::cout << ss.str() << std::endl; - std::optional start; if (!node_sequence.empty()) { @@ -312,7 +297,6 @@ std::vector find_dependencies( // There may be duplicate insertions because of event waypoints, but // that's okay. We just use the first relevant plan waypoint and allow the // insertion to fail for the rest. - std::cout << "inserting candidate " << c.route_id << ":" << c.checkpoint_id << std::endl; checkpoint_maps.at(c.route_id).insert({c.checkpoint_id, i}); } } @@ -675,7 +659,6 @@ reconstruct_waypoints( { // The last itinerary did not have enough waypoints, so we should // discard it. - std::cout << "popping back route " << itinerary.size()-1 << std::endl; const std::size_t remove = itinerary.size() - 1; itinerary.pop_back(); for (auto& candidate : candidates) @@ -728,25 +711,19 @@ reconstruct_waypoints( candidate.velocity).it->index(); candidate.waypoint.arrival.push_back({itinerary.size()-1, index}); - std::cout << __LINE__ << " push candidate " << candidate.waypoint.arrival.back().route_id - << ":" << candidate.waypoint.arrival.back().checkpoint_id << std::endl; } } candidates.back().waypoint.arrival .push_back({itinerary.size()-1, itinerary.back().trajectory().size()-1}); - std::cout << __LINE__ << " push candidate " << candidates.back().waypoint.arrival.back().route_id - << ":" << candidates.back().waypoint.arrival.back().checkpoint_id << std::endl; } } - std::stringstream ss; std::vector removals; for (std::size_t i=0; i < itinerary.size(); ++i) { if (itinerary[i].trajectory().size() < 2) { - ss << "\n -- removing " << i; removals.push_back(i); } } @@ -763,14 +740,12 @@ reconstruct_waypoints( Plan::Checkpoint& c = *c_it; if (c.route_id == remove) { - ss << "\n -- erasing " << c.route_id << ":" << c.checkpoint_id; candidate.waypoint.arrival.erase(c_it); continue; } if (c.route_id > remove) { - ss << "\n -- decrementing " << c.route_id << ":" << c.checkpoint_id; --c.route_id; } @@ -779,8 +754,6 @@ reconstruct_waypoints( } } - std::cout << ss.str() << std::endl; - auto plan_waypoints = find_dependencies( itinerary, candidates, validator, dependency_window, dependency_resolution);