constrained_ik_planner_plugin.cpp

Go to the documentation of this file.

#include <constrained_ik/moveit_interface/constrained_ik_planner_plugin.h>
#include <class_loader/class_loader.h>

const std::string JOINT_INTERP_PLANNER = "JointInterpolation"; 
const std::string CARTESIAN_PLANNER = "Cartesian"; 
namespace constrained_ik
{
  bool CLIKPlannerManager::initialize(const robot_model::RobotModelConstPtr &model, const std::string &ns)
  {
    if (!ns.empty())
      nh_ = ros::NodeHandle(ns);

    for (auto & group_name : model->getJointModelGroupNames())
    {
      //Create a cartesian planner for group
      if (model->getJointModelGroup(group_name)->isChain())
      {
        planners_.insert(std::make_pair(std::make_pair(CARTESIAN_PLANNER, group_name), CLIKPlanningContextPtr(new CartesianPlanner(CARTESIAN_PLANNER, group_name))));

        CartesianDynReconfigServerPtr drs(new CartesianDynReconfigServer(mutex_, ros::NodeHandle(nh_, "constrained_ik_solver/" + group_name)));
        drs->setCallback(boost::bind(&CLIKPlannerManager::cartesianDynamicReconfigureCallback, this, _1, _2, group_name));
        cartesian_dynamic_reconfigure_server_.insert(std::make_pair(group_name, drs));

        //Create a joint interpolated planner for group
        planners_.insert(std::make_pair(std::make_pair(JOINT_INTERP_PLANNER, group_name), CLIKPlanningContextPtr(new JointInterpolationPlanner(JOINT_INTERP_PLANNER, group_name))));
      }
    }

    dynamic_reconfigure_server_.reset(new ManagerDynReconfigServer(mutex_, ros::NodeHandle(nh_, "constrained_ik_planner")));
    dynamic_reconfigure_server_->setCallback(boost::bind(&CLIKPlannerManager::managerDynamicReconfigureCallback, this, _1, _2));

    return true;
  }

  void CLIKPlannerManager::getPlanningAlgorithms(std::vector<std::string> &algs) const
  {
    algs.clear();
    algs.push_back(JOINT_INTERP_PLANNER);
    algs.push_back(CARTESIAN_PLANNER);
  }

  void CLIKPlannerManager::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pcs)
  {
    std::cout << "Entered setPlannerConfigurations" << std::endl;
  }

  planning_interface::PlanningContextPtr CLIKPlannerManager::getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, moveit_msgs::MoveItErrorCodes &error_code) const
  {
    constrained_ik::CLIKPlanningContextPtr planner;

    if (req.group_name.empty())
    {
      logError("No group specified to plan for");
      error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
      return planning_interface::PlanningContextPtr();
    }

    error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;

    if (!planning_scene)
    {
      logError("No planning scene supplied as input");
      return planning_interface::PlanningContextPtr();
    }

    // Get planner
    std::map<std::pair<std::string, std::string>, constrained_ik::CLIKPlanningContextPtr>::const_iterator it;
    if (req.planner_id.empty())
      it = planners_.find(std::make_pair(JOINT_INTERP_PLANNER, req.group_name));
    else
      it = planners_.find(std::make_pair(req.planner_id, req.group_name));

    if (it == planners_.end())
    {
      logError("No planner for specified group");
      error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
      return planning_interface::PlanningContextPtr();
    }
    planner = it->second;

    // Setup Planner
    planner->clear();
    planner->setPlanningScene(planning_scene);
    planner->setMotionPlanRequest(req);

    // Return Planner
    return planner;
  }

  void CLIKPlannerManager::managerDynamicReconfigureCallback(CLIKPlannerDynamicConfig &config, uint32_t level)
  {
    typedef std::map<std::pair<std::string, std::string>, constrained_ik::CLIKPlanningContextPtr>::const_iterator it_type;

    config_ = config;
    for (it_type it = planners_.begin(); it != planners_.end(); it++)
    {
      it->second->setPlannerConfiguration(config_);
    }

  }

  void CLIKPlannerManager::cartesianDynamicReconfigureCallback(CLIKDynamicConfig &config, uint32_t level, std::string group_name)
  {
    // process configuration parameters and fix errors/limits
    validateConstrainedIKConfiguration<CLIKDynamicConfig>(config);

    typedef std::map<std::pair<std::string, std::string>, constrained_ik::CLIKPlanningContextPtr>::const_iterator it_type;
    it_type it = planners_.find(std::make_pair(CARTESIAN_PLANNER, group_name));
    if (it != planners_.end())
    {
      ConstrainedIKConfiguration new_config = convertToConstrainedIKConfiguration(config);
      boost::shared_ptr<CartesianPlanner> planner = boost::static_pointer_cast<CartesianPlanner>(it->second);
      planner->setSolverConfiguration(new_config);
    }
  }

} //namespace constrained_ik
CLASS_LOADER_REGISTER_CLASS(constrained_ik::CLIKPlannerManager, planning_interface::PlannerManager)