Fix plan fetching printout and don't recache fetched plans

Signed-off-by: methylDragon <[email protected]>
osrf · Sep 27, 2023 · 6475ab9 · 6475ab9
1 parent b3b5656
commit 6475ab9
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 3 deletions.
diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp
@@ -119,6 +119,7 @@ MotionPlanCache::put_plan(
   const moveit_msgs::msg::MotionPlanRequest& plan_request,
   const moveit_msgs::msg::RobotTrajectory& plan,
   double execution_time_s,
+  double planning_time_s,
   bool overwrite)
 {
   // Check pre-conditions
@@ -213,6 +214,7 @@ MotionPlanCache::put_plan(
     bool goal_meta_ok = this->extract_and_append_goal_to_metadata(
       *insert_metadata, move_group, plan_request);
     insert_metadata->append("execution_time_s", execution_time_s);
+    insert_metadata->append("planning_time_s", planning_time_s);
 
     if (!start_meta_ok || !goal_meta_ok)
     {

diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp
@@ -95,6 +95,7 @@ class MotionPlanCache
     const moveit_msgs::msg::MotionPlanRequest& plan_request,
     const moveit_msgs::msg::RobotTrajectory& plan,
     double execution_time_s,
+    double planning_time_s,
     bool overwrite = true);
 
   // QUERY CONSTRUCTION ========================================================

diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp
@@ -639,11 +639,16 @@ void MotionPlannerServer::plan_with_move_group(
   else
   {
     MoveGroupInterface::Plan plan;
+    bool plan_is_from_cache = false;
     interface->constructMotionPlanRequest(plan_req_msg);
 
     if (!_only_use_cached_plans)
     {
       res->result.error_code = interface->plan(plan);
+      RCLCPP_INFO(
+        this->get_logger(),
+        "Plan status: %d, planning time: %es",
+        res->result.error_code.val, plan.planning_time);
     }
     else
     {
@@ -654,15 +659,17 @@ void MotionPlannerServer::plan_with_move_group(
       auto fetch_end = this->now();
       if (fetched_plan)
       {
+        plan_is_from_cache = true;
         plan.start_state = plan_req_msg.start_state;
         plan.trajectory = *fetched_plan;
         plan.planning_time = (fetch_end - fetch_start).seconds();
         res->result.error_code.val =
           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, planning time was: %es",
+          (fetch_end - fetch_start).seconds(),
+          fetched_plan->lookupDouble("planning_time_s"));
       }
       else
       {
@@ -690,7 +697,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 && !plan_is_from_cache)
     {
       if (!_motion_plan_cache->put_plan(
           *interface, robot_name,
@@ -699,6 +708,7 @@ void MotionPlannerServer::plan_with_move_group(
             res->result.trajectory.joint_trajectory.points.back()
             .time_from_start
           ).seconds(),
+          plan.planning_time,
           _overwrite_worse_plans))
       {
         RCLCPP_WARN(this->get_logger(), "Did not put plan into cache.");