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 */

#ifndef MOVEIT_ROS_BENCHMARKS_BENCHMARK_EXECUTOR_
#define MOVEIT_ROS_BENCHMARKS_BENCHMARK_EXECUTOR_

#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/planning_interface/planning_interface.h>
#include <warehouse_ros/database_loader.h>
#include <pluginlib/class_loader.hpp>

#include <map>
#include <vector>
#include <string>
#include <boost/function.hpp>
#include <memory>

namespace moveit_ros_benchmarks
{
class BenchmarkExecutor
{
public:
  typedef std::map<std::string, std::string> PlannerRunData;
  typedef std::vector<PlannerRunData> PlannerBenchmarkData;

  typedef boost::function<void(const moveit_msgs::MotionPlanRequest& request, planning_scene::PlanningScenePtr)>
      QueryStartEventFunction;

  typedef boost::function<void(const moveit_msgs::MotionPlanRequest& request, planning_scene::PlanningScenePtr)>
      QueryCompletionEventFunction;

  typedef boost::function<void(const moveit_msgs::MotionPlanRequest& request, PlannerBenchmarkData& benchmark_data)>
      PlannerStartEventFunction;

  typedef boost::function<void(const moveit_msgs::MotionPlanRequest& request, PlannerBenchmarkData& benchmark_data)>
      PlannerCompletionEventFunction;

  typedef boost::function<void(moveit_msgs::MotionPlanRequest& request)> PreRunEventFunction;

  typedef boost::function<void(const moveit_msgs::MotionPlanRequest& request,
                               const planning_interface::MotionPlanDetailedResponse& response,
                               PlannerRunData& run_data)>
      PostRunEventFunction;

  BenchmarkExecutor(const std::string& robot_description_param = "robot_description");
  virtual ~BenchmarkExecutor();

  // Initialize the benchmark executor by loading planning pipelines from the
  // given set of classes
  void initialize(const std::vector<std::string>& plugin_classes);

  void addPreRunEvent(PreRunEventFunction func);
  void addPostRunEvent(PostRunEventFunction func);
  void addPlannerStartEvent(PlannerStartEventFunction func);
  void addPlannerCompletionEvent(PlannerCompletionEventFunction func);
  void addQueryStartEvent(QueryStartEventFunction func);
  void addQueryCompletionEvent(QueryCompletionEventFunction func);

  virtual void clear();

  virtual bool runBenchmarks(const BenchmarkOptions& opts);

protected:
  struct BenchmarkRequest
  {
    std::string name;
    moveit_msgs::MotionPlanRequest request;
  };

  struct StartState
  {
    moveit_msgs::RobotState state;
    std::string name;
  };

  struct PathConstraints
  {
    std::vector<moveit_msgs::Constraints> constraints;
    std::string name;
  };

  struct TrajectoryConstraints
  {
    moveit_msgs::TrajectoryConstraints constraints;
    std::string name;
  };

  virtual bool initializeBenchmarks(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg,
                                    std::vector<BenchmarkRequest>& queries);

  virtual void collectMetrics(PlannerRunData& metrics, const planning_interface::MotionPlanDetailedResponse& mp_res,
                              bool solved, double total_time);

  virtual void writeOutput(const BenchmarkRequest& brequest, const std::string& start_time, double benchmark_duration);

  void shiftConstraintsByOffset(moveit_msgs::Constraints& constraints, const std::vector<double> offset);

  bool plannerConfigurationsExist(const std::map<std::string, std::vector<std::string>>& planners,
                                  const std::string& group_name);

  bool queriesAndPlannersCompatible(const std::vector<BenchmarkRequest>& requests,
                                    const std::map<std::string, std::vector<std::string>>& planners);

  bool loadPlanningScene(const std::string& scene_name, moveit_msgs::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& brequest, const std::vector<StartState>& start_states,
                                 const std::vector<PathConstraints>& path_constraints,
                                 std::vector<BenchmarkRequest>& combos);

  void runBenchmark(moveit_msgs::MotionPlanRequest request,
                    const std::map<std::string, std::vector<std::string>>& planners, int runs);

  planning_scene_monitor::PlanningSceneMonitor* psm_;
  moveit_warehouse::PlanningSceneStorage* pss_;
  moveit_warehouse::PlanningSceneWorldStorage* psws_;
  moveit_warehouse::RobotStateStorage* rs_;
  moveit_warehouse::ConstraintsStorage* cs_;
  moveit_warehouse::TrajectoryConstraintsStorage* tcs_;

  warehouse_ros::DatabaseLoader dbloader;
  planning_scene::PlanningScenePtr planning_scene_;

  BenchmarkOptions options_;

  std::shared_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader_;
  std::map<std::string, planning_interface::PlannerManagerPtr> planner_interfaces_;

  std::vector<PlannerBenchmarkData> benchmark_data_;

  std::vector<PreRunEventFunction> pre_event_fns_;
  std::vector<PostRunEventFunction> post_event_fns_;
  std::vector<PlannerStartEventFunction> planner_start_fns_;
  std::vector<PlannerCompletionEventFunction> planner_completion_fns_;
  std::vector<QueryStartEventFunction> query_start_fns_;
  std::vector<QueryCompletionEventFunction> query_end_fns_;
};
}

#endif