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 
41 
42 namespace move_group
43 {
44 MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("QueryPlannersService")
45 {
46 }
47 
49 {
52 
57 }
58 
59 bool MoveGroupQueryPlannersService::queryInterface(moveit_msgs::QueryPlannerInterfaces::Request& /*req*/,
60  moveit_msgs::QueryPlannerInterfaces::Response& res)
61 {
62  for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines())
63  {
64  const auto& pipeline_id = planning_pipelines.first;
65  const auto& planning_pipeline = planning_pipelines.second;
66  const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
67  if (planner_interface)
68  {
69  std::vector<std::string> algs;
70  planner_interface->getPlanningAlgorithms(algs);
71  moveit_msgs::PlannerInterfaceDescription pi_desc;
72  pi_desc.name = planner_interface->getDescription();
73  pi_desc.pipeline_id = pipeline_id;
74  planner_interface->getPlanningAlgorithms(pi_desc.planner_ids);
75  res.planner_interfaces.push_back(pi_desc);
76  }
77  }
78  return true;
79 }
80 
81 bool MoveGroupQueryPlannersService::getParams(moveit_msgs::GetPlannerParams::Request& req,
82  moveit_msgs::GetPlannerParams::Response& res)
83 {
84  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req.pipeline_id);
85  if (!planning_pipeline)
86  return false;
87 
88  const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
89  if (planner_interface)
90  {
91  std::map<std::string, std::string> config;
92 
93  const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations();
94 
95  planning_interface::PlannerConfigurationMap::const_iterator it =
96  configs.find(req.planner_config); // fetch default params first
97  if (it != configs.end())
98  config.insert(it->second.config.begin(), it->second.config.end());
99 
100  if (!req.group.empty())
101  { // merge in group-specific params
102  it = configs.find(req.group + "[" + req.planner_config + "]");
103  if (it != configs.end())
104  config.insert(it->second.config.begin(), it->second.config.end());
105  }
106 
107  for (const auto& key_value_pair : config)
108  {
109  res.params.keys.push_back(key_value_pair.first);
110  res.params.values.push_back(key_value_pair.second);
111  }
112  }
113  return true;
114 }
115 
116 bool MoveGroupQueryPlannersService::setParams(moveit_msgs::SetPlannerParams::Request& req,
117  moveit_msgs::SetPlannerParams::Response& /*res*/)
118 {
119  if (req.params.keys.size() != req.params.values.size())
120  return false;
121 
122  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req.pipeline_id);
123  if (!planning_pipeline)
124  return false;
125 
126  const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
127 
128  if (planner_interface)
129  {
130  planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations();
131  const std::string config_name = req.group.empty() ? req.planner_config : req.group + "[" + req.planner_config + "]";
132 
133  planning_interface::PlannerConfigurationSettings& config = configs[config_name];
134  config.group = req.group;
135  config.name = config_name;
136  if (req.replace)
137  config.config.clear();
138  for (unsigned int i = 0, end = req.params.keys.size(); i < end; ++i)
139  config.config[req.params.keys[i]] = req.params.values[i];
140 
141  planner_interface->setPlannerConfigurations(configs);
142  }
143  return true;
144 }
145 } // namespace move_group
146 
move_group::MoveGroupQueryPlannersService::setParams
bool setParams(moveit_msgs::SetPlannerParams::Request &req, moveit_msgs::SetPlannerParams::Response &res)
Definition: query_planners_service_capability.cpp:148
move_group::MoveGroupQueryPlannersService::set_service_
ros::ServiceServer set_service_
Definition: query_planners_service_capability.h:126
move_group::MoveGroupQueryPlannersService::queryInterface
bool queryInterface(moveit_msgs::QueryPlannerInterfaces::Request &req, moveit_msgs::QueryPlannerInterfaces::Response &res)
Definition: query_planners_service_capability.cpp:91
move_group::MoveGroupQueryPlannersService
Definition: query_planners_service_capability.h:78
move_group::MoveGroupCapability
Definition: move_group_capability.h:89
move_group::MoveGroupCapability::context_
MoveGroupContextPtr context_
Definition: move_group_capability.h:132
move_group::MoveGroupQueryPlannersService::get_service_
ros::ServiceServer get_service_
Definition: query_planners_service_capability.h:125
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
planning_interface::PlannerConfigurationMap
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
planning_interface::PlannerConfigurationSettings::name
std::string name
move_group::QUERY_PLANNERS_SERVICE_NAME
static const std::string QUERY_PLANNERS_SERVICE_NAME
Definition: capability_names.h:78
query_planners_service_capability.h
move_group::SET_PLANNER_PARAMS_SERVICE_NAME
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
Definition: capability_names.h:82
move_group::MoveGroupCapability::root_node_handle_
ros::NodeHandle root_node_handle_
Definition: move_group_capability.h:129
move_group::MoveGroupCapability::resolvePlanningPipeline
planning_pipeline::PlanningPipelinePtr resolvePlanningPipeline(const std::string &pipeline_id) const
Definition: move_group_capability.cpp:207
planning_interface::PlannerConfigurationSettings::config
std::map< std::string, std::string > config
move_group::MoveGroupQueryPlannersService::MoveGroupQueryPlannersService
MoveGroupQueryPlannersService()
Definition: query_planners_service_capability.cpp:76
move_group
Definition: capability_names.h:41
class_loader.hpp
planning_pipeline.h
move_group::MoveGroupQueryPlannersService::getParams
bool getParams(moveit_msgs::GetPlannerParams::Request &req, moveit_msgs::GetPlannerParams::Response &res)
Definition: query_planners_service_capability.cpp:113
planning_pipeline
CLASS_LOADER_REGISTER_CLASS
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddIterativeSplineParameterization, planning_request_adapter::PlanningRequestAdapter)
move_group::MoveGroupQueryPlannersService::query_service_
ros::ServiceServer query_service_
Definition: query_planners_service_capability.h:124
planning_interface::PlannerConfigurationSettings
move_group::MoveGroupQueryPlannersService::initialize
void initialize() override
Definition: query_planners_service_capability.cpp:80
planning_interface::PlannerConfigurationSettings::group
std::string group
move_group::GET_PLANNER_PARAMS_SERVICE_NAME
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
Definition: capability_names.h:80
moveit_cpp.h
capability_names.h


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 24 2021 02:24:52