BenchmarkExecutor.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Rice University
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Rice University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Ryan Luna */
 
#pragma once
 
#include <moveit/benchmarks/BenchmarkOptions.h>
 
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 
#include <moveit/warehouse/planning_scene_storage.h>
#include <moveit/warehouse/planning_scene_world_storage.h>
#include <moveit/warehouse/state_storage.h>
#include <moveit/warehouse/constraints_storage.h>
#include <moveit/warehouse/trajectory_constraints_storage.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <warehouse_ros/database_loader.h>
#include <pluginlib/class_loader.hpp>
 
#include <map>
#include <vector>
#include <string>
#include <functional>
 
namespace moveit_ros_benchmarks
{
class BenchmarkExecutor
{
public:
  typedef std::map<std::string, std::string> PlannerRunData;
  typedef std::vector<PlannerRunData> PlannerBenchmarkData;
 
  typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request, planning_scene::PlanningScenePtr)>
      QueryStartEventFunction;
 
  typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request, planning_scene::PlanningScenePtr)>
      QueryCompletionEventFunction;
 
  typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request, PlannerBenchmarkData& benchmark_data)>
      PlannerStartEventFunction;
 
  typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request, PlannerBenchmarkData& benchmark_data)>
      PlannerCompletionEventFunction;
 
  typedef std::function<void(moveit_msgs::msg::MotionPlanRequest& request)> PreRunEventFunction;
 
  typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request,
                             const planning_interface::MotionPlanDetailedResponse& response, PlannerRunData& run_data)>
      PostRunEventFunction;
 
  BenchmarkExecutor(const rclcpp::Node::SharedPtr& node,
                    const std::string& robot_description_param = "robot_description");
  virtual ~BenchmarkExecutor();
 
  // Initialize the benchmark executor by loading planning pipelines from the
  // given set of classes
  [[nodiscard]] bool initialize(const std::vector<std::string>& plugin_classes);
 
  void addPreRunEvent(const PreRunEventFunction& func);
  void addPostRunEvent(const PostRunEventFunction& func);
  void addPlannerStartEvent(const PlannerStartEventFunction& func);
  void addPlannerCompletionEvent(const PlannerCompletionEventFunction& func);
  void addQueryStartEvent(const QueryStartEventFunction& func);
  void addQueryCompletionEvent(const QueryCompletionEventFunction& func);
 
  virtual void clear();
 
  virtual bool runBenchmarks(const BenchmarkOptions& options);
 
protected:
  struct BenchmarkRequest
  {
    std::string name;
    moveit_msgs::msg::MotionPlanRequest request;
  };
 
  struct StartState
  {
    moveit_msgs::msg::RobotState state;
    std::string name;
  };
 
  struct PathConstraints
  {
    std::vector<moveit_msgs::msg::Constraints> constraints;
    std::string name;
  };
 
  struct TrajectoryConstraints
  {
    moveit_msgs::msg::TrajectoryConstraints constraints;
    std::string name;
  };
 
  virtual bool initializeBenchmarks(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg,
                                    std::vector<BenchmarkRequest>& queries);
 
  virtual bool loadBenchmarkQueryData(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg,
                                      std::vector<StartState>& start_states,
                                      std::vector<PathConstraints>& path_constraints,
                                      std::vector<PathConstraints>& goal_constraints,
                                      std::vector<TrajectoryConstraints>& traj_constraints,
                                      std::vector<BenchmarkRequest>& queries);
 
  virtual void collectMetrics(PlannerRunData& metrics,
                              const planning_interface::MotionPlanDetailedResponse& motion_plan_response, bool solved,
                              double total_time);
 
  void computeAveragePathSimilarities(PlannerBenchmarkData& planner_data,
                                      const std::vector<planning_interface::MotionPlanDetailedResponse>& responses,
                                      const std::vector<bool>& solved);
 
  bool computeTrajectoryDistance(const robot_trajectory::RobotTrajectory& traj_first,
                                 const robot_trajectory::RobotTrajectory& traj_second, double& result_distance);
 
  virtual void writeOutput(const BenchmarkRequest& benchmark_request, const std::string& start_time,
                           double benchmark_duration, const BenchmarkOptions& options);
 
  void shiftConstraintsByOffset(moveit_msgs::msg::Constraints& constraints, const std::vector<double>& offset);
 
  bool pipelinesExist(const std::map<std::string, std::vector<std::string>>& planners);
 
  bool loadPlanningScene(const std::string& scene_name, moveit_msgs::msg::PlanningScene& scene_msg);
 
  bool loadStates(const std::string& regex, std::vector<StartState>& start_states);
 
  bool loadPathConstraints(const std::string& regex, std::vector<PathConstraints>& constraints);
 
  bool loadTrajectoryConstraints(const std::string& regex, std::vector<TrajectoryConstraints>& constraints);
 
  bool loadQueries(const std::string& regex, const std::string& scene_name, std::vector<BenchmarkRequest>& queries);
 
  void createRequestCombinations(const BenchmarkRequest& benchmark_request, const std::vector<StartState>& start_states,
                                 const std::vector<PathConstraints>& path_constraints,
                                 std::vector<BenchmarkRequest>& combos);
 
  void runBenchmark(moveit_msgs::msg::MotionPlanRequest request, const BenchmarkOptions& options);
 
  std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
  std::shared_ptr<moveit_warehouse::PlanningSceneStorage> planning_scene_storage_;
  std::shared_ptr<moveit_warehouse::PlanningSceneWorldStorage> planning_scene_world_storage_;
  std::shared_ptr<moveit_warehouse::RobotStateStorage> robot_state_storage_;
  std::shared_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
  std::shared_ptr<moveit_warehouse::TrajectoryConstraintsStorage> trajectory_constraints_storage_;
 
  rclcpp::Node::SharedPtr node_;
  warehouse_ros::DatabaseLoader db_loader_;
  planning_scene::PlanningScenePtr planning_scene_;
  std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_;
 
  std::vector<PlannerBenchmarkData> benchmark_data_;
 
  std::vector<PreRunEventFunction> pre_event_functions_;
  std::vector<PostRunEventFunction> post_event_functions_;
  std::vector<PlannerStartEventFunction> planner_start_functions_;
  std::vector<PlannerCompletionEventFunction> planner_completion_functions_;
  std::vector<QueryStartEventFunction> query_start_functions_;
  std::vector<QueryCompletionEventFunction> query_end_functions_;
};
}  // namespace moveit_ros_benchmarks