#include <BenchmarkExecutor.h>

Inheritance diagram for moveit_ros_benchmarks::BenchmarkExecutor:

Collaboration diagram for moveit_ros_benchmarks::BenchmarkExecutor:

[legend]

Classes
struct	BenchmarkRequest

struct	PathConstraints

struct	StartState

struct	TrajectoryConstraints

Public Types
typedef std::map< std::string, std::string >	PlannerRunData
	Structure to hold information for a single run of a planner. More...

typedef std::vector< PlannerRunData >	PlannerBenchmarkData
	Structure to hold information for a single planner's benchmark data. More...

typedef std::function< void(const moveit_msgs::msg::MotionPlanRequest &request, planning_scene::PlanningScenePtr)>	QueryStartEventFunction
	Definition of a query-start benchmark event function. Invoked before a new query is benchmarked. More...

typedef std::function< void(const moveit_msgs::msg::MotionPlanRequest &request, planning_scene::PlanningScenePtr)>	QueryCompletionEventFunction
	Definition of a query-end benchmark event function. Invoked after a query has finished benchmarking. More...

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
	Definition of a pre-run benchmark event function. Invoked immediately before each planner calls solve(). More...

typedef std::function< void(const moveit_msgs::msg::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...

Public Member Functions
	BenchmarkExecutor (const rclcpp::Node::SharedPtr &node, const std::string &robot_description_param="robot_description")

virtual	~BenchmarkExecutor ()

void	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 &opts)

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

virtual bool	loadBenchmarkQueryData (const BenchmarkOptions &opts, 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)
	Initialize benchmark query data from start states and constraints. More...

virtual void	collectMetrics (PlannerRunData &metrics, const planning_interface::MotionPlanDetailedResponse &mp_res, 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 &brequest, const std::string &start_time, double benchmark_duration)

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

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...

bool	loadPlanningScene (const std::string &scene_name, moveit_msgs::msg::PlanningScene &scene_msg)
	Load the planning scene with the given name 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	loadPathConstraints (const std::string &regex, std::vector< PathConstraints > &constraints)
	Load all constraints 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	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...

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...

void	runBenchmark (moveit_msgs::msg::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...

Protected Attributes
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_

rclcpp::Node::SharedPtr	node_

warehouse_ros::DatabaseLoader	dbloader

planning_scene::PlanningScenePtr	planning_scene_

BenchmarkOptions	options_

std::map< std::string, planning_pipeline::PlanningPipelinePtr >	planning_pipelines_

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_

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 61 of file BenchmarkExecutor.h.

Member Typedef Documentation

◆ PlannerBenchmarkData

typedef std::vector<PlannerRunData> moveit_ros_benchmarks::BenchmarkExecutor::PlannerBenchmarkData

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

Definition at line 67 of file BenchmarkExecutor.h.

◆ PlannerCompletionEventFunction

typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request, PlannerBenchmarkData& benchmark_data)> moveit_ros_benchmarks::BenchmarkExecutor::PlannerCompletionEventFunction

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

Definition at line 85 of file BenchmarkExecutor.h.

◆ PlannerRunData

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 65 of file BenchmarkExecutor.h.

◆ PlannerStartEventFunction

typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request, PlannerBenchmarkData& benchmark_data)> moveit_ros_benchmarks::BenchmarkExecutor::PlannerStartEventFunction

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

Definition at line 80 of file BenchmarkExecutor.h.

◆ PostRunEventFunction

typedef std::function<void(const moveit_msgs::msg::MotionPlanRequest& request, const planning_interface::MotionPlanDetailedResponse& response, PlannerRunData& run_data)> moveit_ros_benchmarks::BenchmarkExecutor::PostRunEventFunction

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

Definition at line 93 of file BenchmarkExecutor.h.

◆ PreRunEventFunction

typedef std::function<void(moveit_msgs::msg::MotionPlanRequest& request)> moveit_ros_benchmarks::BenchmarkExecutor::PreRunEventFunction

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

Definition at line 88 of file BenchmarkExecutor.h.

◆ QueryCompletionEventFunction

typedef std::function<void(const moveit_msgs::msg::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 75 of file BenchmarkExecutor.h.

◆ QueryStartEventFunction

typedef std::function<void(const moveit_msgs::msg::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 71 of file BenchmarkExecutor.h.

Constructor & Destructor Documentation

◆ BenchmarkExecutor()

BenchmarkExecutor::BenchmarkExecutor	(	const rclcpp::Node::SharedPtr &	node,
		const std::string &	robot_description_param = `"robot_description"`
	)

Definition at line 95 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ ~BenchmarkExecutor()

BenchmarkExecutor::~BenchmarkExecutor ( )

virtual

Definition at line 107 of file BenchmarkExecutor.cpp.

Member Function Documentation

◆ addPlannerCompletionEvent()

void BenchmarkExecutor::addPlannerCompletionEvent ( const PlannerCompletionEventFunction & func )

Definition at line 206 of file BenchmarkExecutor.cpp.

◆ addPlannerStartEvent()

void BenchmarkExecutor::addPlannerStartEvent ( const PlannerStartEventFunction & func )

Definition at line 201 of file BenchmarkExecutor.cpp.

◆ addPostRunEvent()

void BenchmarkExecutor::addPostRunEvent ( const PostRunEventFunction & func )

Definition at line 196 of file BenchmarkExecutor.cpp.

◆ addPreRunEvent()

void BenchmarkExecutor::addPreRunEvent ( const PreRunEventFunction & func )

Definition at line 191 of file BenchmarkExecutor.cpp.

◆ addQueryCompletionEvent()

void BenchmarkExecutor::addQueryCompletionEvent ( const QueryCompletionEventFunction & func )

Definition at line 216 of file BenchmarkExecutor.cpp.

◆ addQueryStartEvent()

void BenchmarkExecutor::addQueryStartEvent ( const QueryStartEventFunction & func )

Definition at line 211 of file BenchmarkExecutor.cpp.

◆ clear()

void BenchmarkExecutor::clear ( )

virtual

Definition at line 154 of file BenchmarkExecutor.cpp.

◆ collectMetrics()

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

protectedvirtual

Definition at line 868 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ computeAveragePathSimilarities()

void BenchmarkExecutor::computeAveragePathSimilarities	(	PlannerBenchmarkData &	planner_data,
		const std::vector< planning_interface::MotionPlanDetailedResponse > &	responses,
		const std::vector< bool > &	solved
	)

protected

Compute the similarity of each (final) trajectory to all other (final) trajectories in the experiment and write the results to planner_data metrics

Definition at line 937 of file BenchmarkExecutor.cpp.

◆ computeTrajectoryDistance()

bool BenchmarkExecutor::computeTrajectoryDistance	(	const robot_trajectory::RobotTrajectory &	traj_first,
		const robot_trajectory::RobotTrajectory &	traj_second,
		double &	result_distance
	)

protected

Helper function used by computeAveragePathSimilarities() for computing a heuristic distance metric between two robot trajectories. This function aligns both trajectories in a greedy fashion and computes the mean waypoint distance averaged over all aligned waypoints. Using a greedy approach is more efficient than dynamic time warping, and seems to be sufficient for similar trajectories.

Definition at line 982 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ createRequestCombinations()

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 481 of file BenchmarkExecutor.cpp.

◆ initialize()

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

Definition at line 117 of file BenchmarkExecutor.cpp.

Here is the caller graph for this function:

◆ initializeBenchmarks()

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

protectedvirtual

Definition at line 294 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ loadBenchmarkQueryData()

bool BenchmarkExecutor::loadBenchmarkQueryData	(	const BenchmarkOptions &	opts,
		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
	)

protectedvirtual

Initialize benchmark query data from start states and constraints.

Reimplemented in moveit_ros_benchmarks::CombinePredefinedPosesBenchmark.

Definition at line 420 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ loadPathConstraints()

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 707 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ loadPlanningScene()

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

protected

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

Definition at line 588 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ loadQueries()

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 624 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ loadStates()

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 669 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ loadTrajectoryConstraints()

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 742 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ plannerConfigurationsExist()

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 529 of file BenchmarkExecutor.cpp.

Here is the caller graph for this function:

◆ queriesAndPlannersCompatible()

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 273 of file BenchmarkExecutor.cpp.

◆ runBenchmark()

void BenchmarkExecutor::runBenchmark	(	moveit_msgs::msg::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 778 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

◆ runBenchmarks()

bool BenchmarkExecutor::runBenchmarks ( const BenchmarkOptions & opts )

virtual

Definition at line 221 of file BenchmarkExecutor.cpp.

Here is the caller graph for this function:

◆ shiftConstraintsByOffset()

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

protected

Definition at line 459 of file BenchmarkExecutor.cpp.

◆ writeOutput()

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

protectedvirtual

Definition at line 1052 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Member Data Documentation

◆ benchmark_data_

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

protected

Definition at line 217 of file BenchmarkExecutor.h.

◆ cs_

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

protected

Definition at line 206 of file BenchmarkExecutor.h.

◆ dbloader

warehouse_ros::DatabaseLoader moveit_ros_benchmarks::BenchmarkExecutor::dbloader

protected

Definition at line 210 of file BenchmarkExecutor.h.

◆ node_

rclcpp::Node::SharedPtr moveit_ros_benchmarks::BenchmarkExecutor::node_

protected

Definition at line 209 of file BenchmarkExecutor.h.

◆ options_

BenchmarkOptions moveit_ros_benchmarks::BenchmarkExecutor::options_

protected

Definition at line 213 of file BenchmarkExecutor.h.

◆ planner_completion_fns_

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

protected

Definition at line 222 of file BenchmarkExecutor.h.

◆ planner_start_fns_

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

protected

Definition at line 221 of file BenchmarkExecutor.h.

◆ planning_pipelines_

std::map<std::string, planning_pipeline::PlanningPipelinePtr> moveit_ros_benchmarks::BenchmarkExecutor::planning_pipelines_

protected

Definition at line 215 of file BenchmarkExecutor.h.

◆ planning_scene_

planning_scene::PlanningScenePtr moveit_ros_benchmarks::BenchmarkExecutor::planning_scene_

protected

Definition at line 211 of file BenchmarkExecutor.h.

◆ post_event_fns_

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

protected

Definition at line 220 of file BenchmarkExecutor.h.

◆ pre_event_fns_

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

protected

Definition at line 219 of file BenchmarkExecutor.h.

◆ psm_

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

protected

Definition at line 202 of file BenchmarkExecutor.h.

◆ pss_

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

protected

Definition at line 203 of file BenchmarkExecutor.h.

◆ psws_

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

protected

Definition at line 204 of file BenchmarkExecutor.h.

◆ query_end_fns_

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

protected

Definition at line 224 of file BenchmarkExecutor.h.

◆ query_start_fns_

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

protected

Definition at line 223 of file BenchmarkExecutor.h.

◆ rs_

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

protected

Definition at line 205 of file BenchmarkExecutor.h.

◆ tcs_

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

protected

Definition at line 207 of file BenchmarkExecutor.h.

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

moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h
moveit_ros/benchmarks/src/BenchmarkExecutor.cpp

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ PlannerBenchmarkData

◆ PlannerCompletionEventFunction

◆ PlannerRunData

◆ PlannerStartEventFunction

◆ PostRunEventFunction

◆ PreRunEventFunction

◆ QueryCompletionEventFunction

◆ QueryStartEventFunction

Constructor & Destructor Documentation

◆ BenchmarkExecutor()

◆ ~BenchmarkExecutor()

Member Function Documentation

◆ addPlannerCompletionEvent()

◆ addPlannerStartEvent()

◆ addPostRunEvent()

◆ addPreRunEvent()

◆ addQueryCompletionEvent()

◆ addQueryStartEvent()

◆ clear()

◆ collectMetrics()

◆ computeAveragePathSimilarities()

◆ computeTrajectoryDistance()

◆ createRequestCombinations()

◆ initialize()

◆ initializeBenchmarks()

◆ loadBenchmarkQueryData()

◆ loadPathConstraints()

◆ loadPlanningScene()

◆ loadQueries()

◆ loadStates()

◆ loadTrajectoryConstraints()

◆ plannerConfigurationsExist()

◆ queriesAndPlannersCompatible()

◆ runBenchmark()

◆ runBenchmarks()

◆ shiftConstraintsByOffset()

◆ writeOutput()

Member Data Documentation

◆ benchmark_data_

◆ cs_

◆ dbloader

◆ node_

◆ options_

◆ planner_completion_fns_

◆ planner_start_fns_

◆ planning_pipelines_

◆ planning_scene_

◆ post_event_fns_

◆ pre_event_fns_

◆ psm_

◆ pss_

◆ psws_

◆ query_end_fns_

◆ query_start_fns_

◆ rs_

◆ tcs_