Remove unnecessary debug output

Signed-off-by: Michael X. Grey <[email protected]>

rmf_traffic/include/rmf_traffic/agv/Graph.hpp

@@ -29,6 +29,7 @@
 #include <unordered_map>
 #include <unordered_set>
 #include <optional>
+#include <iostream>
 
 namespace rmf_traffic {
 namespace agv {
@@ -468,8 +469,11 @@ class Graph
       template<typename DerivedExecutor>
       DerivedExecutor& execute(DerivedExecutor& executor) const
       {
-        return static_cast<DerivedExecutor&>(execute(
-            static_cast<Executor&>(executor)));
+        Executor& base_executor = static_cast<Executor&>(executor);
+        std::cout << "executing " << this << " : " << &executor
+          << " -> " << &base_executor << std::endl;
+
+        return static_cast<DerivedExecutor&>(execute(base_executor));
       }
 
       /// Execute this event

rmf_traffic/src/rmf_traffic/agv/Planner.cpp

@@ -1158,8 +1158,6 @@ std::vector<Plan::Start> 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<Plan::Start> 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<Plan::Start> 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<Plan::Start> 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<Plan::Start> 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;
 }

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp

@@ -186,21 +186,6 @@ std::vector<NodePtr> 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<rmf_traffic::agv::Plan::Start> start;
   if (!node_sequence.empty())
   {
@@ -675,7 +660,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 +712,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<std::size_t> 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 +741,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 +755,6 @@ reconstruct_waypoints(
     }
   }
 
-  std::cout << ss.str() << std::endl;
-
   auto plan_waypoints = find_dependencies(
       itinerary, candidates, validator,
       dependency_window, dependency_resolution);