BenchmarkOptions.cpp
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 
38 
39 using namespace moveit_ros_benchmarks;
40 
42 {
43 }
44 
45 BenchmarkOptions::BenchmarkOptions(const std::string& ros_namespace)
46 {
47  readBenchmarkOptions(ros_namespace);
48 }
49 
51 {
52 }
53 
54 void BenchmarkOptions::setNamespace(const std::string& ros_namespace)
55 {
56  readBenchmarkOptions(ros_namespace);
57 }
58 
59 void BenchmarkOptions::readBenchmarkOptions(const std::string& ros_namespace)
60 {
61  ros::NodeHandle nh(ros_namespace);
62 
63  XmlRpc::XmlRpcValue benchmark_config;
64  if (nh.getParam("benchmark_config", benchmark_config))
65  {
69  }
70  else
71  {
72  ROS_WARN("No benchmark_config found on param server");
73  }
74 }
75 
76 const std::string& BenchmarkOptions::getHostName() const
77 {
78  return hostname_;
79 }
80 
82 {
83  return port_;
84 }
85 
86 const std::string& BenchmarkOptions::getSceneName() const
87 {
88  return scene_name_;
89 }
90 
92 {
93  return runs_;
94 }
95 
97 {
98  return timeout_;
99 }
100 
101 const std::string& BenchmarkOptions::getBenchmarkName() const
102 {
103  return benchmark_name_;
104 }
105 
106 const std::string& BenchmarkOptions::getGroupName() const
107 {
108  return group_name_;
109 }
110 
111 const std::string& BenchmarkOptions::getOutputDirectory() const
112 {
113  return output_directory_;
114 }
115 
116 const std::string& BenchmarkOptions::getQueryRegex() const
117 {
118  return query_regex_;
119 }
120 
121 const std::string& BenchmarkOptions::getStartStateRegex() const
122 {
123  return start_state_regex_;
124 }
125 
127 {
128  return goal_constraint_regex_;
129 }
130 
132 {
133  return path_constraint_regex_;
134 }
135 
137 {
139 }
140 
141 void BenchmarkOptions::getGoalOffsets(std::vector<double>& offsets) const
142 {
143  offsets.resize(6);
144  memcpy(&offsets[0], goal_offsets, 6 * sizeof(double));
145 }
146 
147 const std::map<std::string, std::vector<std::string>>& BenchmarkOptions::getPlannerConfigurations() const
148 {
149  return planners_;
150 }
151 
152 void BenchmarkOptions::getPlannerPluginList(std::vector<std::string>& plugin_list) const
153 {
154  plugin_list.clear();
155  for (std::map<std::string, std::vector<std::string>>::const_iterator it = planners_.begin(); it != planners_.end();
156  ++it)
157  plugin_list.push_back(it->first);
158 }
159 
160 const std::string& BenchmarkOptions::getWorkspaceFrameID() const
161 {
162  return workspace_.header.frame_id;
163 }
164 
165 const moveit_msgs::WorkspaceParameters& BenchmarkOptions::getWorkspaceParameters() const
166 {
167  return workspace_;
168 }
169 
171 {
172  nh.param(std::string("benchmark_config/warehouse/host"), hostname_, std::string("127.0.0.1"));
173  nh.param(std::string("benchmark_config/warehouse/port"), port_, 33829);
174 
175  if (!nh.getParam("benchmark_config/warehouse/scene_name", scene_name_))
176  ROS_WARN("Benchmark scene_name NOT specified");
177 
178  ROS_INFO("Benchmark host: %s", hostname_.c_str());
179  ROS_INFO("Benchmark port: %d", port_);
180  ROS_INFO("Benchmark scene: %s", scene_name_.c_str());
181 }
182 
184 {
185  nh.param(std::string("benchmark_config/parameters/name"), benchmark_name_, std::string(""));
186  nh.param(std::string("benchmark_config/parameters/runs"), runs_, 10);
187  nh.param(std::string("benchmark_config/parameters/timeout"), timeout_, 10.0);
188  nh.param(std::string("benchmark_config/parameters/output_directory"), output_directory_, std::string(""));
189  nh.param(std::string("benchmark_config/parameters/queries"), query_regex_, std::string(".*"));
190  nh.param(std::string("benchmark_config/parameters/start_states"), start_state_regex_, std::string(""));
191  nh.param(std::string("benchmark_config/parameters/goal_constraints"), goal_constraint_regex_, std::string(""));
192  nh.param(std::string("benchmark_config/parameters/path_constraints"), path_constraint_regex_, std::string(""));
193  nh.param(std::string("benchmark_config/parameters/trajectory_constraints"), trajectory_constraint_regex_,
194  std::string(""));
195 
196  if (!nh.getParam(std::string("benchmark_config/parameters/group"), group_name_))
197  ROS_WARN("Benchmark group NOT specified");
198 
199  if (nh.hasParam("benchmark_config/parameters/workspace"))
201 
202  // Reading in goal_offset (or defaulting to zero)
203  nh.param(std::string("benchmark_config/parameters/goal_offset/x"), goal_offsets[0], 0.0);
204  nh.param(std::string("benchmark_config/parameters/goal_offset/y"), goal_offsets[1], 0.0);
205  nh.param(std::string("benchmark_config/parameters/goal_offset/z"), goal_offsets[2], 0.0);
206  nh.param(std::string("benchmark_config/parameters/goal_offset/roll"), goal_offsets[3], 0.0);
207  nh.param(std::string("benchmark_config/parameters/goal_offset/pitch"), goal_offsets[4], 0.0);
208  nh.param(std::string("benchmark_config/parameters/goal_offset/yaw"), goal_offsets[5], 0.0);
209 
210  ROS_INFO("Benchmark name: '%s'", benchmark_name_.c_str());
211  ROS_INFO("Benchmark #runs: %d", runs_);
212  ROS_INFO("Benchmark timeout: %f secs", timeout_);
213  ROS_INFO("Benchmark group: %s", group_name_.c_str());
214  ROS_INFO("Benchmark query regex: '%s'", query_regex_.c_str());
215  ROS_INFO("Benchmark start state regex: '%s':", start_state_regex_.c_str());
216  ROS_INFO("Benchmark goal constraint regex: '%s':", goal_constraint_regex_.c_str());
217  ROS_INFO("Benchmark path constraint regex: '%s':", path_constraint_regex_.c_str());
218  ROS_INFO("Benchmark goal offsets (%f %f %f, %f %f %f)", goal_offsets[0], goal_offsets[1], goal_offsets[2],
220  ROS_INFO("Benchmark output directory: %s", output_directory_.c_str());
221  ROS_INFO_STREAM("Benchmark workspace: " << workspace_);
222 }
223 
225 {
226  // Make sure all params exist
227  if (!nh.getParam("benchmark_config/parameters/workspace/frame_id", workspace_.header.frame_id))
228  ROS_WARN("Workspace frame_id not specified in benchmark config");
229 
230  nh.param(std::string("benchmark_config/parameters/workspace/min_corner/x"), workspace_.min_corner.x, 0.0);
231  nh.param(std::string("benchmark_config/parameters/workspace/min_corner/y"), workspace_.min_corner.y, 0.0);
232  nh.param(std::string("benchmark_config/parameters/workspace/min_corner/z"), workspace_.min_corner.z, 0.0);
233 
234  nh.param(std::string("benchmark_config/parameters/workspace/max_corner/x"), workspace_.max_corner.x, 0.0);
235  nh.param(std::string("benchmark_config/parameters/workspace/max_corner/y"), workspace_.max_corner.y, 0.0);
236  nh.param(std::string("benchmark_config/parameters/workspace/max_corner/z"), workspace_.max_corner.z, 0.0);
237 
238  workspace_.header.stamp = ros::Time::now();
239 }
240 
242 {
243  planners_.clear();
244 
245  XmlRpc::XmlRpcValue planner_configs;
246  if (nh.getParam("benchmark_config/planners", planner_configs))
247  {
248  if (planner_configs.getType() != XmlRpc::XmlRpcValue::TypeArray)
249  {
250  ROS_ERROR("Expected a list of planner configurations to benchmark");
251  return;
252  }
253 
254  for (int i = 0; i < planner_configs.size(); ++i)
255  {
256  if (planner_configs[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
257  {
258  ROS_WARN("Improper YAML type for planner configurations");
259  continue;
260  }
261  if (!planner_configs[i].hasMember("plugin") || !planner_configs[i].hasMember("planners"))
262  {
263  ROS_WARN("Malformed YAML for planner configurations");
264  continue;
265  }
266 
267  if (planner_configs[i]["planners"].getType() != XmlRpc::XmlRpcValue::TypeArray)
268  {
269  ROS_WARN("Expected a list of planners to benchmark");
270  continue;
271  }
272 
273  std::string plugin = planner_configs[i]["plugin"];
274  ROS_INFO("Reading in planner names for plugin '%s'", plugin.c_str());
275 
276  std::vector<std::string> planners;
277  for (int j = 0; j < planner_configs[i]["planners"].size(); ++j)
278  planners.push_back(planner_configs[i]["planners"][j]);
279 
280  for (std::size_t j = 0; j < planners.size(); ++j)
281  ROS_INFO(" [%lu]: %s", j, planners[j].c_str());
282 
283  planners_[plugin] = planners;
284  }
285  }
286 }
const std::string & getSceneName() const
std::string hostname_
warehouse parameters
const std::string & getOutputDirectory() const
const std::string & getGroupName() const
int size() const
const std::string & getPathConstraintRegex() const
void readWarehouseOptions(ros::NodeHandle &nh)
const std::string & getHostName() const
void readBenchmarkParameters(ros::NodeHandle &nh)
moveit_msgs::WorkspaceParameters workspace_
const std::string & getWorkspaceFrameID() const
void readPlannerConfigs(ros::NodeHandle &nh)
const std::string & getGoalConstraintRegex() const
Type const & getType() const
#define ROS_WARN(...)
void readBenchmarkOptions(const std::string &ros_namespace)
const std::string & getStartStateRegex() const
void getGoalOffsets(std::vector< double > &offsets) const
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void setNamespace(const std::string &ros_namespace)
const std::string & getTrajectoryConstraintRegex() const
#define ROS_INFO_STREAM(args)
const std::string & getQueryRegex() const
bool getParam(const std::string &key, std::string &s) const
const std::string & getBenchmarkName() const
static Time now()
const moveit_msgs::WorkspaceParameters & getWorkspaceParameters() const
bool hasParam(const std::string &key) const
void readWorkspaceParameters(ros::NodeHandle &nh)
#define ROS_ERROR(...)
const std::map< std::string, std::vector< std::string > > & getPlannerConfigurations() const
std::map< std::string, std::vector< std::string > > planners_
planner configurations
void getPlannerPluginList(std::vector< std::string > &plugin_list) const


benchmarks
Author(s): Ryan Luna
autogenerated on Wed Jul 10 2019 04:04:08