#include <BenchmarkExecutor.hpp>

Classes
struct	BenchmarkRequest
struct	StartState
struct	PathConstraints
struct	TrajectoryConstraints

Public Types
typedef std::map< std::string, std::string >	PlannerRunData
	Structure to hold information for a single run of a planner.
typedef std::vector< PlannerRunData >	PlannerBenchmarkData
	Structure to hold information for a single planner's benchmark data.
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.
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.
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().
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().

Public Member Functions
	BenchmarkExecutor (const rclcpp::Node::SharedPtr &node, const std::string &robot_description_param="robot_description")
virtual	~BenchmarkExecutor ()
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 Member Functions
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)
	Initialize benchmark query data from start states and constraints.
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)
	Check that the desired planning pipelines exist.
bool	loadPlanningScene (const std::string &scene_name, moveit_msgs::msg::PlanningScene &scene_msg)
	Load the planning scene with the given name from the warehouse.
bool	loadStates (const std::string &regex, std::vector< StartState > &start_states)
	Load all states matching the given regular expression from the warehouse.
bool	loadPathConstraints (const std::string &regex, std::vector< PathConstraints > &constraints)
	Load all constraints matching the given regular expression from the warehouse.
bool	loadTrajectoryConstraints (const std::string &regex, std::vector< TrajectoryConstraints > &constraints)
	Load all trajectory constraints from the warehouse that match the given regular expression.
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.
void	createRequestCombinations (const BenchmarkRequest &benchmark_request, 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.
void	runBenchmark (moveit_msgs::msg::MotionPlanRequest request, const BenchmarkOptions &options)
	Execute the given motion plan request on the set of planners for the set number of runs.

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

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

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

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

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

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

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

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

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

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

Constructor & Destructor Documentation

◆ BenchmarkExecutor()

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

Definition at line 98 of file BenchmarkExecutor.cpp.

◆ ~BenchmarkExecutor()

BenchmarkExecutor::~BenchmarkExecutor ( )

virtual

Definition at line 112 of file BenchmarkExecutor.cpp.

Member Function Documentation

◆ addPlannerCompletionEvent()

void BenchmarkExecutor::addPlannerCompletionEvent ( const PlannerCompletionEventFunction & func )

Definition at line 202 of file BenchmarkExecutor.cpp.

◆ addPlannerStartEvent()

void BenchmarkExecutor::addPlannerStartEvent ( const PlannerStartEventFunction & func )

Definition at line 197 of file BenchmarkExecutor.cpp.

◆ addPostRunEvent()

void BenchmarkExecutor::addPostRunEvent ( const PostRunEventFunction & func )

Definition at line 192 of file BenchmarkExecutor.cpp.

◆ addPreRunEvent()

void BenchmarkExecutor::addPreRunEvent ( const PreRunEventFunction & func )

Definition at line 187 of file BenchmarkExecutor.cpp.

◆ addQueryCompletionEvent()

void BenchmarkExecutor::addQueryCompletionEvent ( const QueryCompletionEventFunction & func )

Definition at line 212 of file BenchmarkExecutor.cpp.

◆ addQueryStartEvent()

void BenchmarkExecutor::addQueryStartEvent ( const QueryStartEventFunction & func )

Definition at line 207 of file BenchmarkExecutor.cpp.

◆ clear()

void BenchmarkExecutor::clear ( )

virtual

Definition at line 155 of file BenchmarkExecutor.cpp.

◆ collectMetrics()

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

protectedvirtual

Definition at line 985 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 1056 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ createRequestCombinations()

void BenchmarkExecutor::createRequestCombinations	(	const BenchmarkRequest &	benchmark_request,
		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 487 of file BenchmarkExecutor.cpp.

Here is the caller graph for this function:

◆ initialize()

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

nodiscard

Definition at line 116 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initializeBenchmarks()

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

protectedvirtual

Definition at line 272 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ loadBenchmarkQueryData()

bool BenchmarkExecutor::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 )

protectedvirtual

Initialize benchmark query data from start states and constraints.

Definition at line 403 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 696 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller 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 560 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller 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 602 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller 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 650 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller 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 735 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pipelinesExist()

bool BenchmarkExecutor::pipelinesExist ( const std::map< std::string, std::vector< std::string > > & planners )

protected

Check that the desired planning pipelines exist.

Definition at line 537 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ runBenchmark()

void BenchmarkExecutor::runBenchmark	(	moveit_msgs::msg::MotionPlanRequest	request,
		const BenchmarkOptions &	options )

protected

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

Definition at line 775 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ runBenchmarks()

bool BenchmarkExecutor::runBenchmarks ( const BenchmarkOptions & options )

virtual

Definition at line 217 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

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

Here is the caller graph for this function:

◆ writeOutput()

void BenchmarkExecutor::writeOutput	(	const BenchmarkRequest &	benchmark_request,
		const std::string &	start_time,
		double	benchmark_duration,
		const BenchmarkOptions &	options )

protectedvirtual

Definition at line 1171 of file BenchmarkExecutor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ benchmark_data_

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

protected

Definition at line 210 of file BenchmarkExecutor.hpp.

◆ constraints_storage_

std::shared_ptr<moveit_warehouse::ConstraintsStorage> moveit_ros_benchmarks::BenchmarkExecutor::constraints_storage_

protected

Definition at line 202 of file BenchmarkExecutor.hpp.

◆ db_loader_

warehouse_ros::DatabaseLoader moveit_ros_benchmarks::BenchmarkExecutor::db_loader_

protected

Definition at line 206 of file BenchmarkExecutor.hpp.

◆ moveit_cpp_

std::shared_ptr<moveit_cpp::MoveItCpp> moveit_ros_benchmarks::BenchmarkExecutor::moveit_cpp_

protected

Definition at line 208 of file BenchmarkExecutor.hpp.

◆ node_

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

protected

Definition at line 205 of file BenchmarkExecutor.hpp.

◆ planner_completion_functions_

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

protected

Definition at line 215 of file BenchmarkExecutor.hpp.

◆ planner_start_functions_

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

protected

Definition at line 214 of file BenchmarkExecutor.hpp.

◆ planning_scene_

planning_scene::PlanningScenePtr moveit_ros_benchmarks::BenchmarkExecutor::planning_scene_

protected

Definition at line 207 of file BenchmarkExecutor.hpp.

◆ planning_scene_monitor_

std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> moveit_ros_benchmarks::BenchmarkExecutor::planning_scene_monitor_

protected

Definition at line 198 of file BenchmarkExecutor.hpp.

◆ planning_scene_storage_

std::shared_ptr<moveit_warehouse::PlanningSceneStorage> moveit_ros_benchmarks::BenchmarkExecutor::planning_scene_storage_

protected

Definition at line 199 of file BenchmarkExecutor.hpp.

◆ planning_scene_world_storage_

std::shared_ptr<moveit_warehouse::PlanningSceneWorldStorage> moveit_ros_benchmarks::BenchmarkExecutor::planning_scene_world_storage_

protected

Definition at line 200 of file BenchmarkExecutor.hpp.

◆ post_event_functions_

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

protected

Definition at line 213 of file BenchmarkExecutor.hpp.

◆ pre_event_functions_

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

protected

Definition at line 212 of file BenchmarkExecutor.hpp.

◆ query_end_functions_

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

protected

Definition at line 217 of file BenchmarkExecutor.hpp.

◆ query_start_functions_

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

protected

Definition at line 216 of file BenchmarkExecutor.hpp.

◆ robot_state_storage_

std::shared_ptr<moveit_warehouse::RobotStateStorage> moveit_ros_benchmarks::BenchmarkExecutor::robot_state_storage_

protected

Definition at line 201 of file BenchmarkExecutor.hpp.

◆ trajectory_constraints_storage_

std::shared_ptr<moveit_warehouse::TrajectoryConstraintsStorage> moveit_ros_benchmarks::BenchmarkExecutor::trajectory_constraints_storage_

protected

Definition at line 203 of file BenchmarkExecutor.hpp.

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

moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.hpp
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()

◆ pipelinesExist()

◆ runBenchmark()

◆ runBenchmarks()

◆ shiftConstraintsByOffset()

◆ writeOutput()

Member Data Documentation

◆ benchmark_data_

◆ constraints_storage_

◆ db_loader_

◆ moveit_cpp_

◆ node_

◆ planner_completion_functions_

◆ planner_start_functions_

◆ planning_scene_

◆ planning_scene_monitor_

◆ planning_scene_storage_

◆ planning_scene_world_storage_

◆ post_event_functions_

◆ pre_event_functions_

◆ query_end_functions_

◆ query_start_functions_

◆ robot_state_storage_

◆ trajectory_constraints_storage_