Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "query_planners_service_capability.h"
00038 #include <moveit/planning_pipeline/planning_pipeline.h>
00039 #include <moveit/move_group/capability_names.h>
00040
00041 move_group::MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("QueryPlannersService")
00042 {
00043 }
00044
00045 void move_group::MoveGroupQueryPlannersService::initialize()
00046 {
00047 query_service_ = root_node_handle_.advertiseService(QUERY_PLANNERS_SERVICE_NAME,
00048 &MoveGroupQueryPlannersService::queryInterface, this);
00049
00050 get_service_ = root_node_handle_.advertiseService(GET_PLANNER_PARAMS_SERVICE_NAME,
00051 &MoveGroupQueryPlannersService::getParams, this);
00052 set_service_ = root_node_handle_.advertiseService(SET_PLANNER_PARAMS_SERVICE_NAME,
00053 &MoveGroupQueryPlannersService::setParams, this);
00054 }
00055
00056 bool move_group::MoveGroupQueryPlannersService::queryInterface(moveit_msgs::QueryPlannerInterfaces::Request& req,
00057 moveit_msgs::QueryPlannerInterfaces::Response& res)
00058 {
00059 const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
00060 if (planner_interface)
00061 {
00062 std::vector<std::string> algs;
00063 planner_interface->getPlanningAlgorithms(algs);
00064 moveit_msgs::PlannerInterfaceDescription pi_desc;
00065 pi_desc.name = planner_interface->getDescription();
00066 planner_interface->getPlanningAlgorithms(pi_desc.planner_ids);
00067 res.planner_interfaces.push_back(pi_desc);
00068 }
00069 return true;
00070 }
00071
00072 bool move_group::MoveGroupQueryPlannersService::getParams(moveit_msgs::GetPlannerParams::Request& req,
00073 moveit_msgs::GetPlannerParams::Response& res)
00074 {
00075 const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
00076 if (planner_interface)
00077 {
00078 std::map<std::string, std::string> config;
00079
00080 const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations();
00081
00082 planning_interface::PlannerConfigurationMap::const_iterator it =
00083 configs.find(req.planner_config);
00084 if (it != configs.end())
00085 config.insert(it->second.config.begin(), it->second.config.end());
00086
00087 if (!req.group.empty())
00088 {
00089 it = configs.find(req.group + "[" + req.planner_config + "]");
00090 if (it != configs.end())
00091 config.insert(it->second.config.begin(), it->second.config.end());
00092 }
00093
00094 for (std::map<std::string, std::string>::const_iterator it = config.begin(), end = config.end(); it != end; ++it)
00095 {
00096 res.params.keys.push_back(it->first);
00097 res.params.values.push_back(it->second);
00098 }
00099 }
00100 return true;
00101 }
00102
00103 bool move_group::MoveGroupQueryPlannersService::setParams(moveit_msgs::SetPlannerParams::Request& req,
00104 moveit_msgs::SetPlannerParams::Response& res)
00105 {
00106 const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
00107 if (req.params.keys.size() != req.params.values.size())
00108 return false;
00109
00110 if (planner_interface)
00111 {
00112 planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations();
00113 std::string config_name = req.group.empty() ? req.planner_config : req.group + "[" + req.planner_config + "]";
00114
00115 planning_interface::PlannerConfigurationSettings& config = configs[config_name];
00116 config.group = req.group;
00117 config.name = config_name;
00118 if (req.replace)
00119 config.config.clear();
00120 for (unsigned int i = 0, end = req.params.keys.size(); i < end; ++i)
00121 config.config[req.params.keys[i]] = req.params.values[i];
00122
00123 planner_interface->setPlannerConfigurations(configs);
00124 }
00125 return true;
00126 }
00127
00128 #include <class_loader/class_loader.h>
00129 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupQueryPlannersService, move_group::MoveGroupCapability)