Skip to content

Commit

Permalink
Merge pull request moveit#53 from AcutronicRobotics/query_planner
Browse files Browse the repository at this point in the history 
Port query_planners_service_capability
  • Loading branch information
@ahcorde
ahcorde committed Apr 17, 2019
2 parents 688c034 + 417722e commit bb4448a
Show file tree
Hide file tree
Showing 3 changed files with 60 additions and 35 deletions.
2 changes: 1 addition & 1 deletion moveit_ros/move_group/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ add_library(moveit_move_group_default_capabilities
src/default_capabilities/move_action_capability.cpp
# src/default_capabilities/plan_service_capability.cpp
# src/default_capabilities/execute_trajectory_action_capability.cpp
# src/default_capabilities/query_planners_service_capability.cpp
src/default_capabilities/query_planners_service_capability.cpp
# src/default_capabilities/kinematics_service_capability.cpp
# src/default_capabilities/state_validation_service_capability.cpp
# src/default_capabilities/cartesian_path_service_capability.cpp
Expand Down
67 changes: 43 additions & 24 deletions moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,17 +44,34 @@ move_group::MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : Mov

void move_group::MoveGroupQueryPlannersService::initialize()
{
query_service_ = root_node_handle_.advertiseService(QUERY_PLANNERS_SERVICE_NAME,
&MoveGroupQueryPlannersService::queryInterface, this);
std::function<bool( std::shared_ptr<rmw_request_id_t>,
std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Request>,
std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Response>)> cb_query_interface = std::bind(
&move_group::MoveGroupQueryPlannersService::queryInterface, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

get_service_ = root_node_handle_.advertiseService(GET_PLANNER_PARAMS_SERVICE_NAME,
&MoveGroupQueryPlannersService::getParams, this);
set_service_ = root_node_handle_.advertiseService(SET_PLANNER_PARAMS_SERVICE_NAME,
&MoveGroupQueryPlannersService::setParams, this);
std::function<bool( std::shared_ptr<rmw_request_id_t>,
std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Request>,
std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Response>)> cb_get_params = std::bind(
&move_group::MoveGroupQueryPlannersService::getParams, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

std::function<bool( std::shared_ptr<rmw_request_id_t>,
std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Request>,
std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Response>)> cb_set_params = std::bind(
&move_group::MoveGroupQueryPlannersService::setParams, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

query_service_ = node_->create_service<moveit_msgs::srv::QueryPlannerInterfaces>(QUERY_PLANNERS_SERVICE_NAME,
cb_query_interface);

get_service_ = node_->create_service<moveit_msgs::srv::GetPlannerParams>(GET_PLANNER_PARAMS_SERVICE_NAME,
cb_get_params);

set_service_ = node_->create_service<moveit_msgs::srv::SetPlannerParams>(SET_PLANNER_PARAMS_SERVICE_NAME,
cb_set_params);
}

bool move_group::MoveGroupQueryPlannersService::queryInterface(moveit_msgs::srv::QueryPlannerInterfaces::Request& req,
moveit_msgs::srv::QueryPlannerInterfaces::Response& res)
bool move_group::MoveGroupQueryPlannersService::queryInterface(std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Request> req,
std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Response> res)
{
const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
if (planner_interface)
Expand All @@ -64,13 +81,14 @@ bool move_group::MoveGroupQueryPlannersService::queryInterface(moveit_msgs::srv:
moveit_msgs::msg::PlannerInterfaceDescription pi_desc;
pi_desc.name = planner_interface->getDescription();
planner_interface->getPlanningAlgorithms(pi_desc.planner_ids);
res.planner_interfaces.push_back(pi_desc);
res->planner_interfaces.push_back(pi_desc);
}
return true;
}

bool move_group::MoveGroupQueryPlannersService::getParams(moveit_msgs::srv::GetPlannerParams::Request& req,
moveit_msgs::srv::GetPlannerParams::Response& res)
bool move_group::MoveGroupQueryPlannersService::getParams(std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Request> req,
std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Response> res)
{
const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
if (planner_interface)
Expand All @@ -80,45 +98,46 @@ bool move_group::MoveGroupQueryPlannersService::getParams(moveit_msgs::srv::GetP
const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations();

planning_interface::PlannerConfigurationMap::const_iterator it =
configs.find(req.planner_config); // fetch default params first
configs.find(req->planner_config); // fetch default params first
if (it != configs.end())
config.insert(it->second.config.begin(), it->second.config.end());

if (!req.group.empty())
if (!req->group.empty())
{ // merge in group-specific params
it = configs.find(req.group + "[" + req.planner_config + "]");
it = configs.find(req->group + "[" + req->planner_config + "]");
if (it != configs.end())
config.insert(it->second.config.begin(), it->second.config.end());
}

for (std::map<std::string, std::string>::const_iterator it = config.begin(), end = config.end(); it != end; ++it)
{
res.params.keys.push_back(it->first);
res.params.values.push_back(it->second);
res->params.keys.push_back(it->first);
res->params.values.push_back(it->second);
}
}
return true;
}

bool move_group::MoveGroupQueryPlannersService::setParams(moveit_msgs::srv::SetPlannerParams::Request& req,
moveit_msgs::srv::SetPlannerParams::Response& res)
bool move_group::MoveGroupQueryPlannersService::setParams(std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Request> req,
std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Response> res)
{
const planning_interface::PlannerManagerPtr& planner_interface = context_->planning_pipeline_->getPlannerManager();
if (req.params.keys.size() != req.params.values.size())
if (req->params.keys.size() != req->params.values.size())
return false;

if (planner_interface)
{
planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations();
std::string config_name = req.group.empty() ? req.planner_config : req.group + "[" + req.planner_config + "]";
std::string config_name = req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]";

planning_interface::PlannerConfigurationSettings& config = configs[config_name];
config.group = req.group;
config.group = req->group;
config.name = config_name;
if (req.replace)
if (req->replace)
config.config.clear();
for (unsigned int i = 0, end = req.params.keys.size(); i < end; ++i)
config.config[req.params.keys[i]] = req.params.values[i];
for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i)
config.config[req->params.keys[i]] = req->params.values[i];

planner_interface->setPlannerConfigurations(configs);
}
Expand Down
26 changes: 16 additions & 10 deletions moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,9 @@
#define MOVEIT_MOVE_GROUP_QUERY_PLANNERS_SERVICE_CAPABILITY_

#include <moveit/move_group/move_group_capability.h>
#include <moveit_msgs/QueryPlannerInterfaces.h>
#include <moveit_msgs/GetPlannerParams.h>
#include <moveit_msgs/SetPlannerParams.h>
#include <moveit_msgs/srv/query_planner_interfaces.hpp>
#include <moveit_msgs/srv/get_planner_params.hpp>
#include <moveit_msgs/srv/set_planner_params.hpp>

namespace move_group
{
Expand All @@ -52,15 +52,21 @@ class MoveGroupQueryPlannersService : public MoveGroupCapability
void initialize() override;

private:
bool queryInterface(moveit_msgs::srv::QueryPlannerInterfaces::Request& req,
moveit_msgs::srv::QueryPlannerInterfaces::Response& res);
bool queryInterface(std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Request> req,
std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Response> res);

bool getParams(moveit_msgs::srv::GetPlannerParams::Request& req, moveit_msgs::srv::GetPlannerParams::Response& res);
bool setParams(moveit_msgs::srv::SetPlannerParams::Request& req, moveit_msgs::srv::SetPlannerParams::Response& res);
bool getParams(std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Request> req,
std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Response> res);

ros::ServiceServer query_service_;
ros::ServiceServer get_service_;
ros::ServiceServer set_service_;
bool setParams(std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Request> req,
std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Response> res);

rclcpp::Service<moveit_msgs::srv::QueryPlannerInterfaces>::SharedPtr query_service_;
rclcpp::Service<moveit_msgs::srv::GetPlannerParams>::SharedPtr get_service_;
rclcpp::Service<moveit_msgs::srv::SetPlannerParams>::SharedPtr set_service_;
};
}

Expand Down

0 comments on commit bb4448a

Please sign in to comment.