moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
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< 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... | |
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 ®ex, std::vector< StartState > &start_states) |
Load all states matching the given regular expression from the warehouse. More... | |
bool | loadPathConstraints (const std::string ®ex, std::vector< PathConstraints > &constraints) |
Load all constraints matching the given regular expression from the warehouse. More... | |
bool | loadTrajectoryConstraints (const std::string ®ex, std::vector< TrajectoryConstraints > &constraints) |
Load all trajectory constraints from the warehouse that match the given regular expression. More... | |
bool | loadQueries (const std::string ®ex, 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::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_ |
Definition at line 51 of file CombinePredefinedPosesBenchmark.cpp.
|
inline |
Definition at line 54 of file CombinePredefinedPosesBenchmark.cpp.
|
inlineoverridevirtual |
Initialize benchmark query data from start states and constraints.
Reimplemented from moveit_ros_benchmarks::BenchmarkExecutor.
Definition at line 58 of file CombinePredefinedPosesBenchmark.cpp.