Better logging

moveit · Nov 3, 2022 · e1e14dc · e1e14dc
1 parent 4c4835b
commit e1e14dc
Showing 1 changed file with 36 additions and 10 deletions.
diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
@@ -445,6 +445,7 @@ bool BenchmarkExecutor::loadBenchmarkQueryData(
       robot_state_storage_ = new moveit_warehouse::RobotStateStorage(warehouse_connection);
       constraints_storage_ = new moveit_warehouse::ConstraintsStorage(warehouse_connection);
       trajectory_constraints_storage_ = new moveit_warehouse::TrajectoryConstraintsStorage(warehouse_connection);
+      RCLCPP_INFO(LOGGER, "Connected to DB");
     }
     else
     {
@@ -458,12 +459,34 @@ bool BenchmarkExecutor::loadBenchmarkQueryData(
     return false;
   }
 
-  return loadPlanningScene(options.getSceneName(), scene_msg) &&
-         loadStates(options.getStartStateRegex(), start_states) &&
-         loadPathConstraints(options.getGoalConstraintRegex(), goal_constraints) &&
-         loadPathConstraints(options.getPathConstraintRegex(), path_constraints) &&
-         loadTrajectoryConstraints(options.getTrajectoryConstraintRegex(), traj_constraints) &&
-         loadQueries(options.getQueryRegex(), options.getSceneName(), queries);
+  RCLCPP_INFO(LOGGER, "Scene name: '%s'", options.getSceneName().c_str());
+
+  if(!loadPlanningScene(options.getSceneName(), scene_msg))
+  {
+      RCLCPP_ERROR(LOGGER, "Failed to load the planning scene");
+      return false;
+  }
+  if(!loadStates(options.getStartStateRegex(), start_states))
+  {
+      RCLCPP_ERROR(LOGGER, "Failed to load the states");
+      return false;
+  }
+  if(!loadPathConstraints(options.getGoalConstraintRegex(), goal_constraints))
+  {
+      RCLCPP_ERROR(LOGGER, "Failed to load the goal constraints");
+  }
+  if(!loadPathConstraints(options.getPathConstraintRegex(), path_constraints))
+  {
+      RCLCPP_ERROR(LOGGER, "Failed to load the path constraints");
+  }
+  if(!loadTrajectoryConstraints(options.getTrajectoryConstraintRegex(), traj_constraints))
+  {
+      RCLCPP_ERROR(LOGGER, "Failed to load the trajectory constraints");
+  }
+  if(!loadQueries(options.getQueryRegex(), options.getSceneName(), queries))
+  {
+      RCLCPP_ERROR(LOGGER, "Failed to get a query regex");
+  }
   return true;
 }
 
@@ -607,6 +630,7 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_
 
       if (!planning_scene_storage_->getPlanningScene(planning_scene_w_metadata, scene_name))
       {
+        RCLCPP_ERROR(LOGGER,"I FAIL");
         RCLCPP_ERROR(LOGGER, "Failed to load planning scene '%s'", scene_name.c_str());
         return false;
       }
@@ -617,16 +641,16 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_
       moveit_warehouse::PlanningSceneWorldWithMetadata pswwm;
       if (!planning_scene_world_storage_->getPlanningSceneWorld(pswwm, scene_name))
       {
-        RCLCPP_ERROR(LOGGER, "Failed to load planning scene '%s'", scene_name.c_str());
+        RCLCPP_ERROR(LOGGER, "Failed to load planning scene world '%s'", scene_name.c_str());
         return false;
       }
       scene_msg.world = static_cast<moveit_msgs::msg::PlanningSceneWorld>(*pswwm);
       scene_msg.robot_model_name =
           "NO ROBOT INFORMATION. ONLY WORLD GEOMETRY";  // this will be fixed when running benchmark
     }
-    else
+    else{
       RCLCPP_ERROR(LOGGER, "Failed to find planning scene '%s'", scene_name.c_str());
-    return false;
+      return false;}
   }
   catch (std::exception& ex)
   {
@@ -640,8 +664,10 @@ bool BenchmarkExecutor::loadPlanningScene(const std::string& scene_name, moveit_
 bool BenchmarkExecutor::loadQueries(const std::string& regex, const std::string& scene_name,
                                     std::vector<BenchmarkRequest>& queries)
 {
-  if (regex.empty())
+  if (regex.empty()){
+    RCLCPP_WARN(LOGGER, "No query regex provided, don't load any queries from the database")
     return true;
+  }
 
   std::vector<std::string> query_names;
   try