moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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::map< std::string, std::string > PlannerRunData
 Structure to hold information for a single run of a planner.
 
typedef std::vector< PlannerRunDataPlannerBenchmarkData
 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::PlanningSceneMonitorplanning_scene_monitor_
 
std::shared_ptr< moveit_warehouse::PlanningSceneStorageplanning_scene_storage_
 
std::shared_ptr< moveit_warehouse::PlanningSceneWorldStorageplanning_scene_world_storage_
 
std::shared_ptr< moveit_warehouse::RobotStateStoragerobot_state_storage_
 
std::shared_ptr< moveit_warehouse::ConstraintsStorageconstraints_storage_
 
std::shared_ptr< moveit_warehouse::TrajectoryConstraintsStoragetrajectory_constraints_storage_
 
rclcpp::Node::SharedPtr node_
 
warehouse_ros::DatabaseLoader db_loader_
 
planning_scene::PlanningScenePtr planning_scene_
 
std::shared_ptr< moveit_cpp::MoveItCppmoveit_cpp_
 
std::vector< PlannerBenchmarkDatabenchmark_data_
 
std::vector< PreRunEventFunctionpre_event_functions_
 
std::vector< PostRunEventFunctionpost_event_functions_
 
std::vector< PlannerStartEventFunctionplanner_start_functions_
 
std::vector< PlannerCompletionEventFunctionplanner_completion_functions_
 
std::vector< QueryStartEventFunctionquery_start_functions_
 
std::vector< QueryCompletionEventFunctionquery_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.h.

Member Typedef Documentation

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

◆ ~BenchmarkExecutor()

BenchmarkExecutor::~BenchmarkExecutor ( )
virtual

Definition at line 113 of file BenchmarkExecutor.cpp.

Member Function Documentation

◆ addPlannerCompletionEvent()

void BenchmarkExecutor::addPlannerCompletionEvent ( const PlannerCompletionEventFunction func)

Definition at line 203 of file BenchmarkExecutor.cpp.

◆ addPlannerStartEvent()

void BenchmarkExecutor::addPlannerStartEvent ( const PlannerStartEventFunction func)

Definition at line 198 of file BenchmarkExecutor.cpp.

◆ addPostRunEvent()

void BenchmarkExecutor::addPostRunEvent ( const PostRunEventFunction func)

Definition at line 193 of file BenchmarkExecutor.cpp.

◆ addPreRunEvent()

void BenchmarkExecutor::addPreRunEvent ( const PreRunEventFunction func)

Definition at line 188 of file BenchmarkExecutor.cpp.

◆ addQueryCompletionEvent()

void BenchmarkExecutor::addQueryCompletionEvent ( const QueryCompletionEventFunction func)

Definition at line 213 of file BenchmarkExecutor.cpp.

◆ addQueryStartEvent()

void BenchmarkExecutor::addQueryStartEvent ( const QueryStartEventFunction func)

Definition at line 208 of file BenchmarkExecutor.cpp.

◆ clear()

void BenchmarkExecutor::clear ( )
virtual

Definition at line 156 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 986 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 488 of file BenchmarkExecutor.cpp.

Here is the caller graph for this function:

◆ initialize()

bool 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 options,
moveit_msgs::msg::PlanningScene &  scene_msg,
std::vector< BenchmarkRequest > &  queries 
)
protectedvirtual

Definition at line 273 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 404 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 697 of file BenchmarkExecutor.cpp.

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

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

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

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

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

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 776 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 218 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 465 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 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.h.

◆ constraints_storage_

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

Definition at line 202 of file BenchmarkExecutor.h.

◆ db_loader_

warehouse_ros::DatabaseLoader moveit_ros_benchmarks::BenchmarkExecutor::db_loader_
protected

Definition at line 206 of file BenchmarkExecutor.h.

◆ moveit_cpp_

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

Definition at line 208 of file BenchmarkExecutor.h.

◆ node_

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

Definition at line 205 of file BenchmarkExecutor.h.

◆ planner_completion_functions_

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

Definition at line 215 of file BenchmarkExecutor.h.

◆ planner_start_functions_

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

Definition at line 214 of file BenchmarkExecutor.h.

◆ planning_scene_

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

Definition at line 207 of file BenchmarkExecutor.h.

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

◆ planning_scene_storage_

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

Definition at line 199 of file BenchmarkExecutor.h.

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

◆ post_event_functions_

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

Definition at line 213 of file BenchmarkExecutor.h.

◆ pre_event_functions_

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

Definition at line 212 of file BenchmarkExecutor.h.

◆ query_end_functions_

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

Definition at line 217 of file BenchmarkExecutor.h.

◆ query_start_functions_

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

Definition at line 216 of file BenchmarkExecutor.h.

◆ robot_state_storage_

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

Definition at line 201 of file BenchmarkExecutor.h.

◆ trajectory_constraints_storage_

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

Definition at line 203 of file BenchmarkExecutor.h.


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