Classes | Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
moveit_ros_benchmarks::BenchmarkExecutor Class Reference

#include <BenchmarkExecutor.h>

Classes

struct  BenchmarkRequest
 
struct  PathConstraints
 
struct  StartState
 
struct  TrajectoryConstraints
 

Public Types

typedef std::vector< PlannerRunDataPlannerBenchmarkData
 Structure to hold information for a single planner's benchmark data. More...
 
typedef boost::function< void(const moveit_msgs::MotionPlanRequest &request, PlannerBenchmarkData &benchmark_data)> PlannerCompletionEventFunction
 
typedef std::map< std::string, std::string > PlannerRunData
 Structure to hold information for a single run of a planner. More...
 
typedef boost::function< void(const moveit_msgs::MotionPlanRequest &request, PlannerBenchmarkData &benchmark_data)> PlannerStartEventFunction
 
typedef boost::function< void(const moveit_msgs::MotionPlanRequest &request, const planning_interface::MotionPlanDetailedResponse &response, PlannerRunData &run_data)> PostRunEventFunction
 Definition of a post-run benchmark event function. Invoked immediately after each planner calls solve(). More...
 
typedef boost::function< void(moveit_msgs::MotionPlanRequest &request)> PreRunEventFunction
 Definition of a pre-run benchmark event function. Invoked immediately before each planner calls solve(). More...
 
typedef boost::function< void(const moveit_msgs::MotionPlanRequest &request, planning_scene::PlanningScenePtr)> QueryCompletionEventFunction
 Definition of a query-end benchmark event function. Invoked after a query has finished benchmarking. More...
 
typedef boost::function< void(const moveit_msgs::MotionPlanRequest &request, planning_scene::PlanningScenePtr)> QueryStartEventFunction
 Definition of a query-start benchmark event function. Invoked before a new query is benchmarked. More...
 

Public Member Functions

void addPlannerCompletionEvent (PlannerCompletionEventFunction func)
 
void addPlannerStartEvent (PlannerStartEventFunction func)
 
void addPostRunEvent (PostRunEventFunction func)
 
void addPreRunEvent (PreRunEventFunction func)
 
void addQueryCompletionEvent (QueryCompletionEventFunction func)
 
void addQueryStartEvent (QueryStartEventFunction func)
 
 BenchmarkExecutor (const std::string &robot_description_param="robot_description")
 
virtual void clear ()
 
void initialize (const std::vector< std::string > &plugin_classes)
 
virtual bool runBenchmarks (const BenchmarkOptions &opts)
 
virtual ~BenchmarkExecutor ()
 

Protected Member Functions

virtual void collectMetrics (PlannerRunData &metrics, const planning_interface::MotionPlanDetailedResponse &mp_res, bool solved, double total_time)
 
void createRequestCombinations (const BenchmarkRequest &brequest, const std::vector< StartState > &start_states, const std::vector< PathConstraints > &path_constraints, std::vector< BenchmarkRequest > &combos)
 Duplicate the given benchmark request for all combinations of start states and path constraints. More...
 
virtual bool initializeBenchmarks (const BenchmarkOptions &opts, moveit_msgs::PlanningScene &scene_msg, std::vector< BenchmarkRequest > &queries)
 
bool loadPathConstraints (const std::string &regex, std::vector< PathConstraints > &constraints)
 Load all constraints matching the given regular expression from the warehouse. More...
 
bool loadPlanningScene (const std::string &scene_name, moveit_msgs::PlanningScene &scene_msg)
 Load the planning scene with the given name from the warehouse. More...
 
bool loadQueries (const std::string &regex, const std::string &scene_name, std::vector< BenchmarkRequest > &queries)
 Load all motion plan requests matching the given regular expression from the warehouse. More...
 
bool loadStates (const std::string &regex, std::vector< StartState > &start_states)
 Load all states matching the given regular expression from the warehouse. More...
 
bool loadTrajectoryConstraints (const std::string &regex, std::vector< TrajectoryConstraints > &constraints)
 Load all trajectory constraints from the warehouse that match the given regular expression. More...
 
bool plannerConfigurationsExist (const std::map< std::string, std::vector< std::string >> &planners, const std::string &group_name)
 Check that the desired planner plugins and algorithms exist for the given group. More...
 
bool queriesAndPlannersCompatible (const std::vector< BenchmarkRequest > &requests, const std::map< std::string, std::vector< std::string >> &planners)
 Check that the given requests can be run on the set of planner plugins and algorithms. More...
 
void runBenchmark (moveit_msgs::MotionPlanRequest request, const std::map< std::string, std::vector< std::string >> &planners, int runs)
 Execute the given motion plan request on the set of planners for the set number of runs. More...
 
void shiftConstraintsByOffset (moveit_msgs::Constraints &constraints, const std::vector< double > offset)
 
virtual void writeOutput (const BenchmarkRequest &brequest, const std::string &start_time, double benchmark_duration)
 

Protected Attributes

std::vector< PlannerBenchmarkDatabenchmark_data_
 
moveit_warehouse::ConstraintsStoragecs_
 
warehouse_ros::DatabaseLoader dbloader
 
BenchmarkOptions options_
 
std::vector< PlannerCompletionEventFunctionplanner_completion_fns_
 
std::map< std::string, planning_interface::PlannerManagerPtr > planner_interfaces_
 
std::shared_ptr< pluginlib::ClassLoader< planning_interface::PlannerManager > > planner_plugin_loader_
 
std::vector< PlannerStartEventFunctionplanner_start_fns_
 
planning_scene::PlanningScenePtr planning_scene_
 
std::vector< PostRunEventFunctionpost_event_fns_
 
std::vector< PreRunEventFunctionpre_event_fns_
 
planning_scene_monitor::PlanningSceneMonitorpsm_
 
moveit_warehouse::PlanningSceneStoragepss_
 
moveit_warehouse::PlanningSceneWorldStoragepsws_
 
std::vector< QueryCompletionEventFunctionquery_end_fns_
 
std::vector< QueryStartEventFunctionquery_start_fns_
 
moveit_warehouse::RobotStateStoragers_
 
moveit_warehouse::TrajectoryConstraintsStoragetcs_
 

Detailed Description

A class that executes motion plan requests and aggregates data across multiple runs Note: This class operates outside of MoveGroup and does NOT use PlanningRequestAdapters

Definition at line 63 of file BenchmarkExecutor.h.

Member Typedef Documentation

Structure to hold information for a single planner's benchmark data.

Definition at line 69 of file BenchmarkExecutor.h.

Definition of a planner-switch benchmark event function. Invoked after a planner completes all runs for a particular query.

Definition at line 87 of file BenchmarkExecutor.h.

typedef std::map<std::string, std::string> moveit_ros_benchmarks::BenchmarkExecutor::PlannerRunData

Structure to hold information for a single run of a planner.

Definition at line 67 of file BenchmarkExecutor.h.

Definition of a planner-switch benchmark event function. Invoked before a planner starts any runs for a particular query.

Definition at line 82 of file BenchmarkExecutor.h.

Definition of a post-run benchmark event function. Invoked immediately after each planner calls solve().

Definition at line 96 of file BenchmarkExecutor.h.

Definition of a pre-run benchmark event function. Invoked immediately before each planner calls solve().

Definition at line 90 of file BenchmarkExecutor.h.

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

Definition of a query-end benchmark event function. Invoked after a query has finished benchmarking.

Definition at line 77 of file BenchmarkExecutor.h.

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

Definition of a query-start benchmark event function. Invoked before a new query is benchmarked.

Definition at line 73 of file BenchmarkExecutor.h.

Constructor & Destructor Documentation

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

Definition at line 65 of file BenchmarkExecutor.cpp.

BenchmarkExecutor::~BenchmarkExecutor ( )
virtual

Definition at line 87 of file BenchmarkExecutor.cpp.

Member Function Documentation

void BenchmarkExecutor::addPlannerCompletionEvent ( PlannerCompletionEventFunction  func)

Definition at line 197 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::addPlannerStartEvent ( PlannerStartEventFunction  func)

Definition at line 192 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::addPostRunEvent ( PostRunEventFunction  func)

Definition at line 187 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::addPreRunEvent ( PreRunEventFunction  func)

Definition at line 182 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::addQueryCompletionEvent ( QueryCompletionEventFunction  func)

Definition at line 207 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::addQueryStartEvent ( QueryStartEventFunction  func)

Definition at line 202 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::clear ( void  )
virtual

Definition at line 145 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::collectMetrics ( PlannerRunData metrics,
const planning_interface::MotionPlanDetailedResponse mp_res,
bool  solved,
double  total_time 
)
protectedvirtual

(a + b);

Definition at line 819 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::createRequestCombinations ( const BenchmarkRequest brequest,
const std::vector< StartState > &  start_states,
const std::vector< PathConstraints > &  path_constraints,
std::vector< BenchmarkRequest > &  combos 
)
protected

Duplicate the given benchmark request for all combinations of start states and path constraints.

Definition at line 460 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::initialize ( const std::vector< std::string > &  plugin_classes)

Definition at line 102 of file BenchmarkExecutor.cpp.

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

Definition at line 284 of file BenchmarkExecutor.cpp.

bool BenchmarkExecutor::loadPathConstraints ( const std::string &  regex,
std::vector< PathConstraints > &  constraints 
)
protected

Load all constraints matching the given regular expression from the warehouse.

Definition at line 684 of file BenchmarkExecutor.cpp.

bool BenchmarkExecutor::loadPlanningScene ( const std::string &  scene_name,
moveit_msgs::PlanningScene &  scene_msg 
)
protected

Load the planning scene with the given name from the warehouse.

Definition at line 565 of file BenchmarkExecutor.cpp.

bool BenchmarkExecutor::loadQueries ( const std::string &  regex,
const std::string &  scene_name,
std::vector< BenchmarkRequest > &  queries 
)
protected

Load all motion plan requests matching the given regular expression from the warehouse.

Definition at line 601 of file BenchmarkExecutor.cpp.

bool BenchmarkExecutor::loadStates ( const std::string &  regex,
std::vector< StartState > &  start_states 
)
protected

Load all states matching the given regular expression from the warehouse.

Definition at line 646 of file BenchmarkExecutor.cpp.

bool BenchmarkExecutor::loadTrajectoryConstraints ( const std::string &  regex,
std::vector< TrajectoryConstraints > &  constraints 
)
protected

Load all trajectory constraints from the warehouse that match the given regular expression.

Definition at line 719 of file BenchmarkExecutor.cpp.

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

Check that the desired planner plugins and algorithms exist for the given group.

Definition at line 504 of file BenchmarkExecutor.cpp.

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

Check that the given requests can be run on the set of planner plugins and algorithms.

Definition at line 263 of file BenchmarkExecutor.cpp.

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

Execute the given motion plan request on the set of planners for the set number of runs.

Definition at line 755 of file BenchmarkExecutor.cpp.

bool BenchmarkExecutor::runBenchmarks ( const BenchmarkOptions opts)
virtual

Definition at line 212 of file BenchmarkExecutor.cpp.

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

Definition at line 438 of file BenchmarkExecutor.cpp.

void BenchmarkExecutor::writeOutput ( const BenchmarkRequest brequest,
const std::string &  start_time,
double  benchmark_duration 
)
protectedvirtual

Definition at line 908 of file BenchmarkExecutor.cpp.

Member Data Documentation

std::vector<PlannerBenchmarkData> moveit_ros_benchmarks::BenchmarkExecutor::benchmark_data_
protected

Definition at line 198 of file BenchmarkExecutor.h.

moveit_warehouse::ConstraintsStorage* moveit_ros_benchmarks::BenchmarkExecutor::cs_
protected

Definition at line 187 of file BenchmarkExecutor.h.

warehouse_ros::DatabaseLoader moveit_ros_benchmarks::BenchmarkExecutor::dbloader
protected

Definition at line 190 of file BenchmarkExecutor.h.

BenchmarkOptions moveit_ros_benchmarks::BenchmarkExecutor::options_
protected

Definition at line 193 of file BenchmarkExecutor.h.

std::vector<PlannerCompletionEventFunction> moveit_ros_benchmarks::BenchmarkExecutor::planner_completion_fns_
protected

Definition at line 203 of file BenchmarkExecutor.h.

std::map<std::string, planning_interface::PlannerManagerPtr> moveit_ros_benchmarks::BenchmarkExecutor::planner_interfaces_
protected

Definition at line 196 of file BenchmarkExecutor.h.

std::shared_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > moveit_ros_benchmarks::BenchmarkExecutor::planner_plugin_loader_
protected

Definition at line 195 of file BenchmarkExecutor.h.

std::vector<PlannerStartEventFunction> moveit_ros_benchmarks::BenchmarkExecutor::planner_start_fns_
protected

Definition at line 202 of file BenchmarkExecutor.h.

planning_scene::PlanningScenePtr moveit_ros_benchmarks::BenchmarkExecutor::planning_scene_
protected

Definition at line 191 of file BenchmarkExecutor.h.

std::vector<PostRunEventFunction> moveit_ros_benchmarks::BenchmarkExecutor::post_event_fns_
protected

Definition at line 201 of file BenchmarkExecutor.h.

std::vector<PreRunEventFunction> moveit_ros_benchmarks::BenchmarkExecutor::pre_event_fns_
protected

Definition at line 200 of file BenchmarkExecutor.h.

planning_scene_monitor::PlanningSceneMonitor* moveit_ros_benchmarks::BenchmarkExecutor::psm_
protected

Definition at line 183 of file BenchmarkExecutor.h.

moveit_warehouse::PlanningSceneStorage* moveit_ros_benchmarks::BenchmarkExecutor::pss_
protected

Definition at line 184 of file BenchmarkExecutor.h.

moveit_warehouse::PlanningSceneWorldStorage* moveit_ros_benchmarks::BenchmarkExecutor::psws_
protected

Definition at line 185 of file BenchmarkExecutor.h.

std::vector<QueryCompletionEventFunction> moveit_ros_benchmarks::BenchmarkExecutor::query_end_fns_
protected

Definition at line 205 of file BenchmarkExecutor.h.

std::vector<QueryStartEventFunction> moveit_ros_benchmarks::BenchmarkExecutor::query_start_fns_
protected

Definition at line 204 of file BenchmarkExecutor.h.

moveit_warehouse::RobotStateStorage* moveit_ros_benchmarks::BenchmarkExecutor::rs_
protected

Definition at line 186 of file BenchmarkExecutor.h.

moveit_warehouse::TrajectoryConstraintsStorage* moveit_ros_benchmarks::BenchmarkExecutor::tcs_
protected

Definition at line 188 of file BenchmarkExecutor.h.


The documentation for this class was generated from the following files:


benchmarks
Author(s): Ryan Luna
autogenerated on Sun Oct 18 2020 13:18:44