Fix plan fetching printout and don't recache fetched plans

Signed-off-by: methylDragon <[email protected]>

nexus_motion_planner/src/motion_planner_server.cpp

@@ -639,6 +639,7 @@ void MotionPlannerServer::plan_with_move_group(
   else
   {
     MoveGroupInterface::Plan plan;
+    bool fetched_plan;
     interface->constructMotionPlanRequest(plan_req_msg);
 
     if (!_only_use_cached_plans)
@@ -648,7 +649,7 @@ void MotionPlannerServer::plan_with_move_group(
     else
     {
       auto fetch_start = this->now();
-      auto fetched_plan = _motion_plan_cache->fetch_best_matching_plan(
+      fetched_plan = _motion_plan_cache->fetch_best_matching_plan(
         *interface, robot_name, plan_req_msg,
         _cache_start_match_tolerance, _cache_goal_match_tolerance);
       auto fetch_end = this->now();
@@ -661,8 +662,8 @@ void MotionPlannerServer::plan_with_move_group(
           moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
         RCLCPP_INFO(
           this->get_logger(),
-          "Cache fetch took %es, fetched plan planning time was: %es",
-          (fetch_end - fetch_start).seconds(), plan.planning_time);
+          "Cache fetch took %es, fetched plan's planning time was: %es",
+          (fetch_end - fetch_start).seconds(), fetched_plan.planning_time);
       }
       else
       {
@@ -690,7 +691,9 @@ void MotionPlannerServer::plan_with_move_group(
       return;
     }
 
-    if (_use_motion_plan_cache)
+    // Make sure we check if the plan we have was fetched (so we don't have
+    // duplicate caches.)
+    if (_use_motion_plan_cache && !fetched_plan)
     {
       if (!_motion_plan_cache->put_plan(
           *interface, robot_name,