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 
42 #include <fstream>
43 
44 ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel, const ros::NodeHandle& nh)
45  : nh_(nh)
46  , kmodel_(kmodel)
47  , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
48  , context_manager_(kmodel, constraint_sampler_manager_)
49  , constraints_library_(new ConstraintsLibrary(context_manager_))
50  , use_constraints_approximations_(true)
51  , simplify_solutions_(true)
52 {
53  ROS_INFO("Initializing OMPL interface using ROS parameters");
57 }
58 
59 ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel,
61  const ros::NodeHandle& nh)
62  : nh_(nh)
63  , kmodel_(kmodel)
64  , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
68  , simplify_solutions_(true)
69 {
70  ROS_INFO("Initializing OMPL interface using specified configuration");
71  setPlannerConfigurations(pconfig);
74 }
75 
77 {
78 }
79 
81 {
83 
84  // construct default configurations for planning groups that don't have configs already passed in
85  const std::vector<const robot_model::JointModelGroup*>& groups = kmodel_->getJointModelGroups();
86  for (std::size_t i = 0; i < groups.size(); ++i)
87  {
88  if (pconfig.find(groups[i]->getName()) == pconfig.end())
89  {
91  empty.name = empty.group = groups[i]->getName();
92  pconfig2[empty.name] = empty;
93  }
94  }
95 
97 }
98 
99 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext(
100  const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req) const
101 {
102  moveit_msgs::MoveItErrorCodes dummy;
103  return getPlanningContext(planning_scene, req, dummy);
104 }
105 
106 ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext(
107  const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req,
108  moveit_msgs::MoveItErrorCodes& error_code) const
109 {
110  ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(planning_scene, req, error_code);
111  if (ctx)
112  configureContext(ctx);
113  return ctx;
114 }
115 
116 ompl_interface::ModelBasedPlanningContextPtr
117 ompl_interface::OMPLInterface::getPlanningContext(const std::string& config, const std::string& factory_type) const
118 {
119  ModelBasedPlanningContextPtr ctx = context_manager_.getPlanningContext(config, factory_type);
120  if (ctx)
121  configureContext(ctx);
122  return ctx;
123 }
124 
125 void ompl_interface::OMPLInterface::configureContext(const ModelBasedPlanningContextPtr& context) const
126 {
128  context->setConstraintsApproximations(constraints_library_);
129  else
130  context->setConstraintsApproximations(ConstraintsLibraryPtr());
131  context->simplifySolutions(simplify_solutions_);
132 }
133 
135 {
136  constraints_library_->loadConstraintApproximations(path);
137  std::stringstream ss;
138  constraints_library_->printConstraintApproximations(ss);
139  ROS_INFO_STREAM(ss.str());
140 }
141 
143 {
144  constraints_library_->saveConstraintApproximations(path);
145 }
146 
148 {
149  std::string cpath;
150  if (nh_.getParam("constraint_approximations_path", cpath))
151  {
153  return true;
154  }
155  ROS_WARN("ROS param 'constraint_approximations' not found. Unable to save constraint approximations");
156  return false;
157 }
158 
160 {
161  std::string cpath;
162  if (nh_.getParam("constraint_approximations_path", cpath))
163  {
165  return true;
166  }
167  return false;
168 }
169 
171 {
174 }
175 
177  const std::string& group_name, const std::string& planner_id,
178  const std::map<std::string, std::string>& group_params,
180 {
181  XmlRpc::XmlRpcValue xml_config;
182  if (!nh_.getParam("planner_configs/" + planner_id, xml_config))
183  {
184  ROS_ERROR("Could not find the planner configuration '%s' on the param server", planner_id.c_str());
185  return false;
186  }
187 
188  if (xml_config.getType() != XmlRpc::XmlRpcValue::TypeStruct)
189  {
190  ROS_ERROR("A planning configuration should be of type XmlRpc Struct type (for configuration '%s')",
191  planner_id.c_str());
192  return false;
193  }
194 
195  planner_config.name = group_name + "[" + planner_id + "]";
196  planner_config.group = group_name;
197 
198  // default to specified parameters of the group (overridden by configuration specific parameters)
199  planner_config.config = group_params;
200 
201  // read parameters specific for this configuration
202  for (XmlRpc::XmlRpcValue::iterator it = xml_config.begin(); it != xml_config.end(); ++it)
203  {
204  if (it->second.getType() == XmlRpc::XmlRpcValue::TypeString)
205  planner_config.config[it->first] = static_cast<std::string>(it->second);
206  else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
207  planner_config.config[it->first] = boost::lexical_cast<std::string>(static_cast<double>(it->second));
208  else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeInt)
209  planner_config.config[it->first] = boost::lexical_cast<std::string>(static_cast<int>(it->second));
210  else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeBoolean)
211  planner_config.config[it->first] = boost::lexical_cast<std::string>(static_cast<bool>(it->second));
212  }
213 
214  return true;
215 }
216 
218 {
219  const std::vector<std::string>& group_names = kmodel_->getJointModelGroupNames();
221 
222  // read the planning configuration for each group
223  pconfig.clear();
224  for (std::size_t i = 0; i < group_names.size(); ++i)
225  {
226  // the set of planning parameters that can be specific for the group (inherited by configurations of that group)
227  static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction",
228  "enforce_joint_model_state_space" };
229 
230  // get parameters specific for the robot planning group
231  std::map<std::string, std::string> specific_group_params;
232  for (std::size_t k = 0; k < sizeof(KNOWN_GROUP_PARAMS) / sizeof(std::string); ++k)
233  {
234  if (nh_.hasParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k]))
235  {
236  std::string value;
237  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value))
238  {
239  if (!value.empty())
240  specific_group_params[KNOWN_GROUP_PARAMS[k]] = value;
241  continue;
242  }
243 
244  double value_d;
245  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_d))
246  {
247  specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_d);
248  continue;
249  }
250 
251  int value_i;
252  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_i))
253  {
254  specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_i);
255  continue;
256  }
257 
258  bool value_b;
259  if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_b))
260  {
261  specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_b);
262  continue;
263  }
264  }
265  }
266 
267  // add default planner configuration
269  std::string default_planner_id;
270  if (nh_.getParam(group_names[i] + "/default_planner_config", default_planner_id))
271  {
272  if (!loadPlannerConfiguration(group_names[i], default_planner_id, specific_group_params, default_pc))
273  default_planner_id = "";
274  }
275  if (default_planner_id.empty())
276  {
277  default_pc.group = group_names[i];
278  default_pc.config = specific_group_params;
279  default_pc.config["type"] = "geometric::RRTConnect";
280  }
281  default_pc.name = group_names[i]; // this is the name of the default config
282  pconfig[default_pc.name] = default_pc;
283 
284  // get parameters specific to each planner type
285  XmlRpc::XmlRpcValue config_names;
286  if (nh_.getParam(group_names[i] + "/planner_configs", config_names))
287  {
288  if (config_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
289  {
290  ROS_ERROR("The planner_configs argument of a group configuration "
291  "should be an array of strings (for group '%s')",
292  group_names[i].c_str());
293  continue;
294  }
295 
296  for (int j = 0; j < config_names.size(); ++j)
297  {
298  if (config_names[j].getType() != XmlRpc::XmlRpcValue::TypeString)
299  {
300  ROS_ERROR("Planner configuration names must be of type string (for group '%s')", group_names[i].c_str());
301  continue;
302  }
303  std::string planner_id = static_cast<std::string>(config_names[j]);
304 
306  if (loadPlannerConfiguration(group_names[i], planner_id, specific_group_params, pc))
307  pconfig[pc.name] = pc;
308  }
309  }
310  }
311 
312  for (planning_interface::PlannerConfigurationMap::iterator it = pconfig.begin(); it != pconfig.end(); ++it)
313  {
314  ROS_DEBUG_STREAM_NAMED("parameters", "Parameters for configuration '" << it->first << "'");
315  for (std::map<std::string, std::string>::const_iterator config_it = it->second.config.begin();
316  config_it != it->second.config.end(); ++config_it)
317  ROS_DEBUG_STREAM_NAMED("parameters", " - " << config_it->first << " = " << config_it->second);
318  }
319  setPlannerConfigurations(pconfig);
320 }
321 
323 {
324  ROS_INFO("OMPL ROS interface is running.");
325 }
#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.
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 21 2018 10:37:13