ompl_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36 
43 #include <fstream>
44 #include <locale>
45 
46 ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel, const ros::NodeHandle& nh)
47  : nh_(nh)
48  , kmodel_(kmodel)
49  , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
50  , context_manager_(kmodel, constraint_sampler_manager_)
51  , constraints_library_(new ConstraintsLibrary(context_manager_))
52  , use_constraints_approximations_(true)
53  , simplify_solutions_(true)
54 {
55  ROS_INFO("Initializing OMPL interface using ROS parameters");
59 }
60 
61 ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel,
63  const ros::NodeHandle& nh)
64  : nh_(nh)
65  , kmodel_(kmodel)
66  , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
70  , simplify_solutions_(true)
71 {
72  ROS_INFO("Initializing OMPL interface using specified configuration");
73  setPlannerConfigurations(pconfig);
76 }
77 
79 {
80 }
81 
83 {
85 
86  // construct default configurations for planning groups that don't have configs already passed in
87  const std::vector<const robot_model::JointModelGroup*>& groups = kmodel_->getJointModelGroups();
88  for (std::size_t i = 0; i < groups.size(); ++i)
89  {
90  if (pconfig.find(groups[i]->getName()) == pconfig.end())
91  {
93  empty.name = empty.group = groups[i]->getName();
94  pconfig2[empty.name] = empty;
95  }
96  }
97 
99 }
100 
101 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext(
102  const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req) const
103 {
104  moveit_msgs::MoveItErrorCodes dummy;
105  return getPlanningContext(planning_scene, req, dummy);
106 }
107 
108 ompl_interface::ModelBasedPlanningContextPtr
109 ompl_interface::OMPLInterface::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
111  moveit_msgs::MoveItErrorCodes& error_code) const
112 {
113  ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(planning_scene, req, error_code);
114  if (ctx)
115  configureContext(ctx);
116  return ctx;
117 }
118 
119 ompl_interface::ModelBasedPlanningContextPtr
120 ompl_interface::OMPLInterface::getPlanningContext(const std::string& config, const std::string& factory_type) const
121 {
122  ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(config, factory_type);
123  if (ctx)
124  configureContext(ctx);
125  return ctx;
126 }
127 
128 void ompl_interface::OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const
129 {
131  context->setConstraintsApproximations(constraints_library_);
132  else
133  context->setConstraintsApproximations(ConstraintsLibraryPtr());
134  context->simplifySolutions(simplify_solutions_);
135 }
136 
138 {
139  constraints_library_->loadConstraintApproximations(path);
140  std::stringstream ss;
141  constraints_library_->printConstraintApproximations(ss);
142  ROS_INFO_STREAM(ss.str());
143 }
144 
146 {
147  constraints_library_->saveConstraintApproximations(path);
148 }
149 
151 {
152  std::string cpath;
153  if (nh_.getParam("constraint_approximations_path", cpath))
154  {
156  return true;
157  }
158  ROS_WARN("ROS param 'constraint_approximations' not found. Unable to save constraint approximations");
159  return false;
160 }
161 
163 {
164  std::string cpath;
165  if (nh_.getParam("constraint_approximations_path", cpath))
166  {
168  return true;
169  }
170  return false;
171 }
172 
174 {
177 }
178 
180  const std::string& group_name, const std::string& planner_id,
181  const std::map<std::string, std::string>& group_params,
183 {
184  XmlRpc::XmlRpcValue xml_config;
185  if (!nh_.getParam("planner_configs/" + planner_id, xml_config))
186  {
187  ROS_ERROR("Could not find the planner configuration '%s' on the param server", planner_id.c_str());
188  return false;
189  }
190 
191  if (xml_config.getType() != XmlRpc::XmlRpcValue::TypeStruct)
192  {
193  ROS_ERROR("A planning configuration should be of type XmlRpc Struct type (for configuration '%s')",
194  planner_id.c_str());
195  return false;
196  }
197 
198  planner_config.name = group_name + "[" + planner_id + "]";
199  planner_config.group = group_name;
200 
201  // default to specified parameters of the group (overridden by configuration specific parameters)
202  planner_config.config = group_params;
203 
204  // read parameters specific for this configuration
205  for (XmlRpc::XmlRpcValue::iterator it = xml_config.begin(); it != xml_config.end(); ++it)
206  {
207  if (it->second.getType() == XmlRpc::XmlRpcValue::TypeString)
208  planner_config.config[it->first] = static_cast<std::string>(it->second);
209  else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
210  planner_config.config[it->first] = moveit::core::toString(static_cast<double>(it->second));
211  else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeInt)
212  planner_config.config[it->first] = std::to_string(static_cast<int>(it->second));
213  else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeBoolean)
214  planner_config.config[it->first] = boost::lexical_cast<std::string>(static_cast<bool>(it->second));
215  }
216 
217  return true;
218 }
219 
221 {
222  const std::vector<std::string>& group_names = kmodel_->getJointModelGroupNames();
224 
225  // read the planning configuration for each group
226  pconfig.clear();
227  for (std::size_t i = 0; i < group_names.size(); ++i)
228  {
229  // the set of planning parameters that can be specific for the group (inherited by configurations of that group)
230  static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction",
231  "enforce_joint_model_state_space" };
232 
233  // get parameters specific for the robot planning group
234  std::map<std::string, std::string> specific_group_params;
235  for (std::size_t k = 0; k < sizeof(KNOWN_GROUP_PARAMS) / sizeof(std::string); ++k)
236  {
237  if (nh_.hasParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k]))
238  {
239  std::string value;
240  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value))
241  {
242  if (!value.empty())
243  specific_group_params[KNOWN_GROUP_PARAMS[k]] = value;
244  continue;
245  }
246 
247  double value_d;
248  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_d))
249  {
250  // convert to string using no locale
251  specific_group_params[KNOWN_GROUP_PARAMS[k]] = moveit::core::toString(value_d);
252  continue;
253  }
254 
255  int value_i;
256  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_i))
257  {
258  specific_group_params[KNOWN_GROUP_PARAMS[k]] = std::to_string(value_i);
259  continue;
260  }
261 
262  bool value_b;
263  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_b))
264  {
265  specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_b);
266  continue;
267  }
268  }
269  }
270 
271  // add default planner configuration
273  std::string default_planner_id;
274  if (nh_.getParam(group_names[i] + "/default_planner_config", default_planner_id))
275  {
276  if (!loadPlannerConfiguration(group_names[i], default_planner_id, specific_group_params, default_pc))
277  default_planner_id = "";
278  }
279  if (default_planner_id.empty())
280  {
281  default_pc.group = group_names[i];
282  default_pc.config = specific_group_params;
283  default_pc.config["type"] = "geometric::RRTConnect";
284  }
285  default_pc.name = group_names[i]; // this is the name of the default config
286  pconfig[default_pc.name] = default_pc;
287 
288  // get parameters specific to each planner type
289  XmlRpc::XmlRpcValue config_names;
290  if (nh_.getParam(group_names[i] + "/planner_configs", config_names))
291  {
292  if (config_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
293  {
294  ROS_ERROR("The planner_configs argument of a group configuration "
295  "should be an array of strings (for group '%s')",
296  group_names[i].c_str());
297  continue;
298  }
299 
300  for (int j = 0; j < config_names.size(); ++j)
301  {
302  if (config_names[j].getType() != XmlRpc::XmlRpcValue::TypeString)
303  {
304  ROS_ERROR("Planner configuration names must be of type string (for group '%s')", group_names[i].c_str());
305  continue;
306  }
307  std::string planner_id = static_cast<std::string>(config_names[j]);
308 
310  if (loadPlannerConfiguration(group_names[i], planner_id, specific_group_params, pc))
311  pconfig[pc.name] = pc;
312  }
313  }
314  }
315 
316  for (planning_interface::PlannerConfigurationMap::iterator it = pconfig.begin(); it != pconfig.end(); ++it)
317  {
318  ROS_DEBUG_STREAM_NAMED("parameters", "Parameters for configuration '" << it->first << "'");
319  for (std::map<std::string, std::string>::const_iterator config_it = it->second.config.begin();
320  config_it != it->second.config.end(); ++config_it)
321  ROS_DEBUG_STREAM_NAMED("parameters", " - " << config_it->first << " = " << config_it->second);
322  }
323  setPlannerConfigurations(pconfig);
324 }
325 
327 {
328  ROS_INFO("OMPL ROS interface is running.");
329 }
#define ROS_DEBUG_STREAM_NAMED(name, args)
ValueStruct::iterator iterator
ModelBasedPlanningContextPtr getPlanningContext(const std::string &config, const std::string &factory_type="") const
bool loadPlannerConfiguration(const std::string &group_name, const std::string &planner_id, const std::map< std::string, std::string > &group_params, planning_interface::PlannerConfigurationSettings &planner_config)
Load planner configurations for specified group into planner_config.
int size() const
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_
ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req) const
void configureContext(const ModelBasedPlanningContextPtr &context) const
std::map< std::string, std::string > config
Type const & getType() const
bool loadConstraintApproximations()
Look up param server &#39;constraint_approximations&#39; and use its value as the path to load constraint app...
#define ROS_WARN(...)
void loadPlannerConfigurations()
Configure the planners.
robot_model::RobotModelConstPtr kmodel_
The ROS node handle.
void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
Specify configurations for the planners.
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
ConstraintsLibraryPtr constraints_library_
PlanningContextManager context_manager_
void printStatus()
Print the status of this node.
#define ROS_INFO(...)
void loadConstraintSamplers()
Load the additional plugins for sampling constraints.
OMPLInterface(const robot_model::RobotModelConstPtr &kmodel, const ros::NodeHandle &nh=ros::NodeHandle("~"))
Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the speci...
void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
Specify configurations for the planners.
std::string toString(double d)
moveit_msgs::MotionPlanRequest MotionPlanRequest
bool saveConstraintApproximations()
Look up param server &#39;constraint_approximations&#39; and use its value as the path to save constraint app...
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
bool hasParam(const std::string &key) const
#define ROS_ERROR(...)


ompl
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:01