moveit2
The MoveIt Motion Planning Framework for ROS 2.
Public Member Functions | List of all members
moveit_ros_benchmarks::CombinePredefinedPosesBenchmark Class Reference
Inheritance diagram for moveit_ros_benchmarks::CombinePredefinedPosesBenchmark:
Inheritance graph
[legend]
Collaboration diagram for moveit_ros_benchmarks::CombinePredefinedPosesBenchmark:
Collaboration graph
[legend]

Public Member Functions

 CombinePredefinedPosesBenchmark (const rclcpp::Node::SharedPtr &node)
 
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) override
 Initialize benchmark query data from start states and constraints. More...
 
- Public Member Functions inherited from moveit_ros_benchmarks::BenchmarkExecutor
 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)
 

Additional Inherited Members

- Public Types inherited from moveit_ros_benchmarks::BenchmarkExecutor
typedef std::map< std::string, std::string > PlannerRunData
 Structure to hold information for a single run of a planner. More...
 
typedef std::vector< PlannerRunDataPlannerBenchmarkData
 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...
 
- Protected Member Functions inherited from moveit_ros_benchmarks::BenchmarkExecutor
virtual bool initializeBenchmarks (const BenchmarkOptions &opts, moveit_msgs::msg::PlanningScene &scene_msg, std::vector< BenchmarkRequest > &queries)
 
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 inherited from moveit_ros_benchmarks::BenchmarkExecutor
planning_scene_monitor::PlanningSceneMonitorpsm_
 
moveit_warehouse::PlanningSceneStoragepss_
 
moveit_warehouse::PlanningSceneWorldStoragepsws_
 
moveit_warehouse::RobotStateStoragers_
 
moveit_warehouse::ConstraintsStoragecs_
 
moveit_warehouse::TrajectoryConstraintsStoragetcs_
 
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< PlannerBenchmarkDatabenchmark_data_
 
std::vector< PreRunEventFunctionpre_event_fns_
 
std::vector< PostRunEventFunctionpost_event_fns_
 
std::vector< PlannerStartEventFunctionplanner_start_fns_
 
std::vector< PlannerCompletionEventFunctionplanner_completion_fns_
 
std::vector< QueryStartEventFunctionquery_start_fns_
 
std::vector< QueryCompletionEventFunctionquery_end_fns_
 

Detailed Description

Definition at line 51 of file CombinePredefinedPosesBenchmark.cpp.

Constructor & Destructor Documentation

◆ CombinePredefinedPosesBenchmark()

moveit_ros_benchmarks::CombinePredefinedPosesBenchmark::CombinePredefinedPosesBenchmark ( const rclcpp::Node::SharedPtr &  node)
inline

Definition at line 54 of file CombinePredefinedPosesBenchmark.cpp.

Member Function Documentation

◆ loadBenchmarkQueryData()

bool moveit_ros_benchmarks::CombinePredefinedPosesBenchmark::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 
)
inlineoverridevirtual

Initialize benchmark query data from start states and constraints.

Reimplemented from moveit_ros_benchmarks::BenchmarkExecutor.

Definition at line 58 of file CombinePredefinedPosesBenchmark.cpp.


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