moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
BenchmarkOptions.h
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ryan Luna */
36
37#pragma once
38
39#include <rclcpp/node.hpp>
40#include <string>
41#include <map>
42#include <vector>
43#include <moveit_msgs/msg/workspace_parameters.hpp>
44
45static constexpr int CARTESIAN_DOF = 6;
46
48{
81{
83 BenchmarkOptions(const rclcpp::Node::SharedPtr& node);
84
86 void getPlanningPipelineNames(std::vector<std::string>& planning_pipeline_names) const;
87
88 /* \brief Get the frame id of the planning workspace */
89 const std::string& getWorkspaceFrameID() const;
90 /* \brief Get the parameter set of the planning workspace */
91 const moveit_msgs::msg::WorkspaceParameters& getWorkspaceParameters() const;
92
93 bool readBenchmarkOptions(const rclcpp::Node::SharedPtr& node);
94 bool readPlannerConfigs(const rclcpp::Node::SharedPtr& node);
95
96 void readWorkspaceParameters(const rclcpp::Node::SharedPtr& node);
97 void readGoalOffset(const rclcpp::Node::SharedPtr& node);
98
100 std::string hostname; // Host address for warehouse
101 int port; // Port name for warehouse
102 std::string scene_name; // Scene name to load for this experiment
103
105 int runs; // Number of experiment runs
106 double timeout; // Experiment timeout
107 std::string benchmark_name; // Experiment name
108 std::string group_name; // Joint group name
109 std::string output_directory; // Output directory for results file
110 std::string query_regex; // Regex for queries in database
111 std::string start_state_regex; // Regex for start_states in database
112 std::string goal_constraint_regex; // Regex for goal_constraints in database
113 std::string path_constraint_regex; // Regex for path_constraints in database
114 std::string trajectory_constraint_regex; // Regex for trajectory_constraint in database
115 std::vector<std::string> predefined_poses; // List of named targets
116 std::string predefined_poses_group; // Group where the predefined poses are specified
117 std::vector<double> goal_offsets =
118 std::vector<double>(CARTESIAN_DOF); // Offset applied to goal constraints: x, y, z, roll, pitch, yaw
119
121 std::map<std::string, std::vector<std::string>> planning_pipelines;
122 std::map<std::string, std::vector<std::pair<std::string, std::string>>> parallel_planning_pipelines;
123
124 moveit_msgs::msg::WorkspaceParameters workspace;
125};
126} // namespace moveit_ros_benchmarks
Options to configure a benchmark experiment. The configuration is provided via ROS2 parameters.
void readGoalOffset(const rclcpp::Node::SharedPtr &node)
const std::string & getWorkspaceFrameID() const
void readWorkspaceParameters(const rclcpp::Node::SharedPtr &node)
std::map< std::string, std::vector< std::string > > planning_pipelines
planner configurations
moveit_msgs::msg::WorkspaceParameters workspace
std::map< std::string, std::vector< std::pair< std::string, std::string > > > parallel_planning_pipelines
void getPlanningPipelineNames(std::vector< std::string > &planning_pipeline_names) const
Get all planning pipeline names.
const moveit_msgs::msg::WorkspaceParameters & getWorkspaceParameters() const
bool readBenchmarkOptions(const rclcpp::Node::SharedPtr &node)
std::string hostname
Warehouse parameters.
bool readPlannerConfigs(const rclcpp::Node::SharedPtr &node)
std::vector< std::string > predefined_poses