query_planners_service_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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, Robert Haschke */
36 
40 
42 {
43 }
44 
46 {
49 
54 }
55 
56 bool move_group::MoveGroupQueryPlannersService::queryInterface(moveit_msgs::QueryPlannerInterfaces::Request& req,
57  moveit_msgs::QueryPlannerInterfaces::Response& res)
58 {
59  const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
60  if (planner_interface)
61  {
62  std::vector<std::string> algs;
63  planner_interface->getPlanningAlgorithms(algs);
64  moveit_msgs::PlannerInterfaceDescription pi_desc;
65  pi_desc.name = planner_interface->getDescription();
66  planner_interface->getPlanningAlgorithms(pi_desc.planner_ids);
67  res.planner_interfaces.push_back(pi_desc);
68  }
69  return true;
70 }
71 
72 bool move_group::MoveGroupQueryPlannersService::getParams(moveit_msgs::GetPlannerParams::Request& req,
73  moveit_msgs::GetPlannerParams::Response& res)
74 {
75  const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
76  if (planner_interface)
77  {
78  std::map<std::string, std::string> config;
79 
80  const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations();
81 
82  planning_interface::PlannerConfigurationMap::const_iterator it =
83  configs.find(req.planner_config); // fetch default params first
84  if (it != configs.end())
85  config.insert(it->second.config.begin(), it->second.config.end());
86 
87  if (!req.group.empty())
88  { // merge in group-specific params
89  it = configs.find(req.group + "[" + req.planner_config + "]");
90  if (it != configs.end())
91  config.insert(it->second.config.begin(), it->second.config.end());
92  }
93 
94  for (std::map<std::string, std::string>::const_iterator it = config.begin(), end = config.end(); it != end; ++it)
95  {
96  res.params.keys.push_back(it->first);
97  res.params.values.push_back(it->second);
98  }
99  }
100  return true;
101 }
102 
103 bool move_group::MoveGroupQueryPlannersService::setParams(moveit_msgs::SetPlannerParams::Request& req,
104  moveit_msgs::SetPlannerParams::Response& res)
105 {
106  const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
107  if (req.params.keys.size() != req.params.values.size())
108  return false;
109 
110  if (planner_interface)
111  {
112  planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations();
113  std::string config_name = req.group.empty() ? req.planner_config : req.group + "[" + req.planner_config + "]";
114 
115  planning_interface::PlannerConfigurationSettings& config = configs[config_name];
116  config.group = req.group;
117  config.name = config_name;
118  if (req.replace)
119  config.config.clear();
120  for (unsigned int i = 0, end = req.params.keys.size(); i < end; ++i)
121  config.config[req.params.keys[i]] = req.params.values[i];
122 
123  planner_interface->setPlannerConfigurations(configs);
124  }
125  return true;
126 }
127 
bool queryInterface(moveit_msgs::QueryPlannerInterfaces::Request &req, moveit_msgs::QueryPlannerInterfaces::Response &res)
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
std::map< std::string, std::string > config
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool setParams(moveit_msgs::SetPlannerParams::Request &req, moveit_msgs::SetPlannerParams::Response &res)
static const std::string QUERY_PLANNERS_SERVICE_NAME
bool getParams(moveit_msgs::GetPlannerParams::Request &req, moveit_msgs::GetPlannerParams::Response &res)
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:14