move_group.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include <stdexcept>
#include <moveit/warehouse/constraints_storage.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group/capability_names.h>
#include <moveit/move_group_pick_place_capability/capability_names.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_monitor/current_state_monitor.h>
#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
#include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/MoveGroupAction.h>
#include <moveit_msgs/PickupAction.h>
#include <moveit_msgs/PlaceAction.h>
#include <moveit_msgs/ExecuteKnownTrajectory.h>
#include <moveit_msgs/QueryPlannerInterfaces.h>
#include <moveit_msgs/GetCartesianPath.h>

#include <actionlib/client/simple_action_client.h>
#include <eigen_conversions/eigen_msg.h>
#include <std_msgs/String.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>
#include <ros/console.h>
#include <ros/ros.h>

namespace moveit
{
namespace planning_interface
{

const std::string MoveGroup::ROBOT_DESCRIPTION = "robot_description";    // name of the robot description (a param name, so it can be changed externally)

namespace
{

enum ActiveTargetType
{
  JOINT, POSE, POSITION, ORIENTATION
};

}

class MoveGroup::MoveGroupImpl
{
public:

  MoveGroupImpl(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server) : opt_(opt), tf_(tf)
  {
    robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_);
    if (!getRobotModel())
    {
      std::string error = "Unable to construct robot model. Please make sure all needed information is on the parameter server.";
      ROS_FATAL_STREAM(error);
      throw std::runtime_error(error);
    }

    if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
    {
      std::string error = "Group '" + opt.group_name_ + "' was not found.";
      ROS_FATAL_STREAM(error);
      throw std::runtime_error(error);
    }

    joint_state_target_.reset(new robot_state::RobotState(getRobotModel()));
    joint_state_target_->setToDefaultValues();
    active_target_ = JOINT;
    can_look_ = false;
    can_replan_ = false;
    replan_delay_ = 2.0;
    goal_joint_tolerance_ = 1e-4;
    goal_position_tolerance_ = 1e-4; // 0.1 mm
    goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
    planning_time_ = 5.0;
    initializing_constraints_ = false;

    const robot_model::JointModelGroup *joint_model_group = getRobotModel()->getJointModelGroup(opt.group_name_);
    if (joint_model_group)
    {
      if (joint_model_group->isChain())
        end_effector_link_ = joint_model_group->getLinkModelNames().back();
      pose_reference_frame_ = getRobotModel()->getModelFrame();

      trajectory_event_publisher_ = node_handle_.advertise<std_msgs::String>(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false);

      current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_);

      move_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>(move_group::MOVE_ACTION, false));
      waitForAction(move_action_client_, wait_for_server, move_group::MOVE_ACTION);

      pick_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::PickupAction>(move_group::PICKUP_ACTION, false));
      waitForAction(pick_action_client_, wait_for_server, move_group::PICKUP_ACTION);

      place_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::PlaceAction>(move_group::PLACE_ACTION, false));
      waitForAction(place_action_client_, wait_for_server, move_group::PLACE_ACTION);

      execute_service_ = node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
      query_service_ = node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
      cartesian_path_service_ = node_handle_.serviceClient<moveit_msgs::GetCartesianPath>(move_group::CARTESIAN_PATH_SERVICE_NAME);

      ROS_INFO_STREAM("Ready to take MoveGroup commands for group " << opt.group_name_ << ".");
    }
    else
      ROS_ERROR("Unable to initialize MoveGroup interface.");
  }

  template<typename T>
  void waitForAction(const T &action, const ros::Duration &wait_for_server, const std::string &name)
  {
    ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());

    // in case ROS time is published, wait for the time data to arrive
    ros::Time start_time = ros::Time::now();
    while (start_time == ros::Time::now())
    {
      ros::WallDuration(0.01).sleep();
      ros::spinOnce();
    }

    // wait for the server (and spin as needed)
    if (wait_for_server == ros::Duration(0, 0))
    {
      while (node_handle_.ok() && !action->isServerConnected())
      {
        ros::WallDuration(0.02).sleep();
        ros::spinOnce();
      }
    }
    else
    {
      ros::Time final_time = ros::Time::now() + wait_for_server;
      while (node_handle_.ok() && !action->isServerConnected() && final_time > ros::Time::now())
      {
        ros::WallDuration(0.02).sleep();
        ros::spinOnce();
      }
    }

    if (!action->isServerConnected())
      throw std::runtime_error("Unable to connect to action server within allotted time");
    else
      ROS_DEBUG("Connected to '%s'", name.c_str());
  }

  ~MoveGroupImpl()
  {
    if (constraints_init_thread_)
      constraints_init_thread_->join();
  }

  const boost::shared_ptr<tf::Transformer>& getTF() const
  {
    return tf_;
  }

  const Options& getOptions() const
  {
    return opt_;
  }

  const robot_model::RobotModelConstPtr& getRobotModel() const
  {
    return robot_model_;
  }

  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
  {
    moveit_msgs::QueryPlannerInterfaces::Request req;
    moveit_msgs::QueryPlannerInterfaces::Response res;
    if (query_service_.call(req, res))
      if (!res.planner_interfaces.empty())
      {
        desc = res.planner_interfaces.front();
        return true;
      }
    return false;
  }

  void setPlannerId(const std::string &planner_id)
  {
    planner_id_ = planner_id;
  }

  robot_state::JointStateGroup* getJointStateTarget()
  {
    return joint_state_target_->getJointStateGroup(opt_.group_name_);
  }

  void setStartState(const robot_state::RobotState &start_state)
  {
    considered_start_state_.reset(new robot_state::RobotState(start_state));
  }

  void setStartStateToCurrentState()
  {
    considered_start_state_.reset();
  }

  void setEndEffectorLink(const std::string &end_effector)
  {
    end_effector_link_ = end_effector;
  }

  void clearPoseTarget(const std::string &end_effector_link)
  {
    pose_targets_.erase(end_effector_link);
  }

  void clearPoseTargets()
  {
    pose_targets_.clear();
  }

  const std::string &getEndEffectorLink() const
  {
    return end_effector_link_;
  }

  const std::string& getEndEffector() const
  {
    if (!end_effector_link_.empty())
    {
      const std::vector<std::string> &possible_eefs = getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
      for (std::size_t i = 0 ; i < possible_eefs.size() ; ++i)
        if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
          return possible_eefs[i];
    }
    static std::string empty;
    return empty;
  }

  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped> &poses, const std::string &end_effector_link)
  {
    const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
    if (eef.empty())
    {
      ROS_ERROR("No end-effector to set the pose for");
      return false;
    }
    else
    {
      pose_targets_[eef] = poses;
      // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
      std::vector<geometry_msgs::PoseStamped> &stored_poses = pose_targets_[eef];
      for (std::size_t i = 0 ; i < stored_poses.size() ; ++i)
        stored_poses[i].header.stamp = ros::Time(0);
    }
    return true;
  }

  bool hasPoseTarget(const std::string &end_effector_link) const
  {
    const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
    return pose_targets_.find(eef) != pose_targets_.end();
  }

  const geometry_msgs::PoseStamped& getPoseTarget(const std::string &end_effector_link) const
  {
    const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;

    // if multiple pose targets are set, return the first one
    std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
    if (jt != pose_targets_.end())
      if (!jt->second.empty())
        return jt->second.at(0);

    // or return an error
    static const geometry_msgs::PoseStamped unknown;
    ROS_ERROR("Pose for end effector '%s' not known.", eef.c_str());
    return unknown;
  }

  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string &end_effector_link) const
  {
    const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;

    std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
    if (jt != pose_targets_.end())
      if (!jt->second.empty())
        return jt->second;

    // or return an error
    static const std::vector<geometry_msgs::PoseStamped> empty;
    ROS_ERROR("Poses for end effector '%s' are not known.", eef.c_str());
    return empty;
  }

  void setPoseReferenceFrame(const std::string &pose_reference_frame)
  {
    pose_reference_frame_ = pose_reference_frame;
  }

  void setSupportSurfaceName(const std::string &support_surface)
  {
    support_surface_ = support_surface;
  }

  const std::string& getPoseReferenceFrame() const
  {
    return pose_reference_frame_;
  }

  void setTargetType(ActiveTargetType type)
  {
    active_target_ = type;
  }

  ActiveTargetType getTargetType() const
  {
    return active_target_;
  }

  bool getCurrentState(robot_state::RobotStatePtr &current_state)
  {
    if (!current_state_monitor_)
    {
      ROS_ERROR("Unable to get current robot state");
      return false;
    }

    // if needed, start the monitor and wait up to 1 second for a full robot state
    if (!current_state_monitor_->isActive())
      current_state_monitor_->startStateMonitor();

    if (!current_state_monitor_->waitForCurrentState(opt_.group_name_, 1.0))
      ROS_WARN("Joint values for monitored state are requested but the full state is not known");

    current_state = current_state_monitor_->getCurrentState();
    return true;
  }

  bool place(const std::string &object, const std::vector<geometry_msgs::PoseStamped> &poses)
  {
    std::vector<manipulation_msgs::PlaceLocation> locations;
    for (std::size_t i = 0; i < poses.size(); ++i)
    {
      manipulation_msgs::PlaceLocation location;
      location.approach.direction.vector.z = -1.0;
      location.retreat.direction.vector.x = -1.0;
      location.approach.direction.header.frame_id = getRobotModel()->getModelFrame();
      location.retreat.direction.header.frame_id = end_effector_link_;

      location.approach.min_distance = 0.1;
      location.approach.desired_distance = 0.2;
      location.retreat.min_distance = 0.0;
      location.retreat.desired_distance = 0.2;
      // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody

      location.place_pose = poses[i];
      locations.push_back(location);
    }
    ROS_DEBUG("Move group interface has %u place locations", (unsigned int) locations.size());
    return place(object, locations);
  }

  bool place(const std::string &object, const std::vector<manipulation_msgs::PlaceLocation> &locations)
  {
    if (!place_action_client_)
    {
      ROS_ERROR_STREAM("Place action client not found");
      return false;
    }
    if (!place_action_client_->isServerConnected())
    {
      ROS_ERROR_STREAM("Place action server not connected");
      return false;
    }
    moveit_msgs::PlaceGoal goal;
    constructGoal(goal, object);
    goal.place_locations = locations;
    goal.planning_options.plan_only = false;
    place_action_client_->sendGoal(goal);
    ROS_DEBUG("Sent place goal with %d locations", (int) goal.place_locations.size());
    if (!place_action_client_->waitForResult())
    {
      ROS_INFO_STREAM("Place action returned early");
    }
    if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      return true;
    }
    else
    {
      ROS_WARN_STREAM("Fail: " << place_action_client_->getState().toString() << ": " << place_action_client_->getState().getText());
      return false;
    }
  }

  bool pick(const std::string &object, const std::vector<manipulation_msgs::Grasp> &grasps)
  {
    if (!pick_action_client_)
    {
      ROS_ERROR_STREAM("Pick action client not found");
      return false;
    }
    if (!pick_action_client_->isServerConnected())
    {
      ROS_ERROR_STREAM("Pick action server not connected");
      return false;
    }
    moveit_msgs::PickupGoal goal;
    constructGoal(goal, object);
    goal.possible_grasps = grasps;
    goal.planning_options.plan_only = false;
    pick_action_client_->sendGoal(goal);
    if (!pick_action_client_->waitForResult())
    {
      ROS_INFO_STREAM("Pickup action returned early");
    }
    if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      return true;
    }
    else
    {
      ROS_WARN_STREAM("Fail: " << pick_action_client_->getState().toString() << ": " << pick_action_client_->getState().getText());
      return false;
    }
  }

  bool plan(Plan &plan)
  {
    if (!move_action_client_)
      return false;
    if (!move_action_client_->isServerConnected())
      return false;

    moveit_msgs::MoveGroupGoal goal;
    constructGoal(goal);
    goal.planning_options.plan_only = true;
    goal.planning_options.look_around = false;
    goal.planning_options.replan = false;
    move_action_client_->sendGoal(goal);
    if (!move_action_client_->waitForResult())
    {
      ROS_INFO_STREAM("MoveGroup action returned early");
    }
    if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
      plan.start_state_ = move_action_client_->getResult()->trajectory_start;
      return true;
    }
    else
    {
      ROS_WARN_STREAM("Fail: " << move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText());
      return false;
    }
  }

  bool move(bool wait)
  {
    if (!move_action_client_)
      return false;
    if (!move_action_client_->isServerConnected())
      return false;

    moveit_msgs::MoveGroupGoal goal;
    constructGoal(goal);
    goal.planning_options.plan_only = false;
    goal.planning_options.look_around = can_look_;
    goal.planning_options.replan = can_replan_;
    goal.planning_options.replan_delay = replan_delay_;

    move_action_client_->sendGoal(goal);
    if (!wait)
      return true;

    if (!move_action_client_->waitForResult())
    {
      ROS_INFO_STREAM("MoveGroup action returned early");
    }

    if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
      return true;
    else
    {
      ROS_INFO_STREAM(move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText());
      return false;
    }
  }

  bool execute(const Plan &plan, bool wait)
  {
    moveit_msgs::ExecuteKnownTrajectory::Request req;
    moveit_msgs::ExecuteKnownTrajectory::Response res;
    req.trajectory = plan.trajectory_;
    req.wait_for_execution = wait;
    if (execute_service_.call(req, res))
      return res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
    else
      return false;
  }

  double computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double step, double jump_threshold,
                  moveit_msgs::RobotTrajectory &msg, bool avoid_collisions)
  {
    moveit_msgs::GetCartesianPath::Request req;
    moveit_msgs::GetCartesianPath::Response res;

    if (considered_start_state_)
      robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
    req.group_name = opt_.group_name_;
    req.header.frame_id = getPoseReferenceFrame();
    req.header.stamp = ros::Time::now();
    req.waypoints = waypoints;
    req.max_step = step;
    req.jump_threshold = jump_threshold;
    req.avoid_collisions = avoid_collisions;

    if (cartesian_path_service_.call(req, res))
    {
      if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
      {
    msg = res.solution;
    return res.fraction;
      }
      else
    return -1.0;
    }
    else
      return -1.0;
  }

  void stop()
  {
    if (trajectory_event_publisher_)
    {
      std_msgs::String event;
      event.data = "stop";
      trajectory_event_publisher_.publish(event);
    }
  }

  double getGoalPositionTolerance() const
  {
    return goal_position_tolerance_;
  }

  double getGoalOrientationTolerance() const
  {
    return goal_orientation_tolerance_;
  }

  double getGoalJointTolerance() const
  {
    return goal_joint_tolerance_;
  }

  void setGoalJointTolerance(double tolerance)
  {
    goal_joint_tolerance_ = tolerance;
  }

  void setGoalPositionTolerance(double tolerance)
  {
    goal_position_tolerance_ = tolerance;
  }

  void setGoalOrientationTolerance(double tolerance)
  {
    goal_orientation_tolerance_ = tolerance;
  }

  void setPlanningTime(double seconds)
  {
    if (seconds > 0.0)
      planning_time_ = seconds;
  }

  double getPlanningTime() const
  {
    return planning_time_;
  }

  void allowLooking(bool flag)
  {
    can_look_ = flag;
    ROS_INFO("Looking around: %s", can_look_ ? "yes" : "no");
  }

  void allowReplanning(bool flag)
  {
    can_replan_ = flag;
    ROS_INFO("Replanning: %s", can_replan_ ? "yes" : "no");
  }

  void setReplanningDelay(double delay)
  {
    if (delay >= 0.0)
      replan_delay_ = delay;
  }

  double getReplanningDelay() const
  {
    return replan_delay_;
  }

  void constructGoal(moveit_msgs::MoveGroupGoal &goal_out)
  {
    moveit_msgs::MoveGroupGoal goal;
    goal.request.group_name = opt_.group_name_;
    goal.request.num_planning_attempts = 1;
    goal.request.allowed_planning_time = planning_time_;
    goal.request.planner_id = planner_id_;
    goal.request.workspace_parameters = workspace_parameters_;

    if (considered_start_state_)
      robot_state::robotStateToRobotStateMsg(*considered_start_state_, goal.request.start_state);

    if (active_target_ == JOINT)
    {
      goal.request.goal_constraints.resize(1);
      goal.request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(getJointStateTarget(), goal_joint_tolerance_);
    }
    else
      if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
      {
        // find out how many goals are specified
        std::size_t goal_count = 0;
        for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin() ;
             it != pose_targets_.end() ; ++it)
          goal_count = std::max(goal_count, it->second.size());

        // start filling the goals;
        // each end effector has a number of possible poses (K) as valid goals
        // but there could be multiple end effectors specified, so we want each end effector
        // to reach the goal that corresponds to the goals of the other end effectors
        goal.request.goal_constraints.resize(goal_count);

        for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin() ;
             it != pose_targets_.end() ; ++it)
        {
          for (std::size_t i = 0 ; i < it->second.size() ; ++i)
          {
            moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
            if (active_target_ == ORIENTATION)
              c.position_constraints.clear();
            if (active_target_ == POSITION)
              c.orientation_constraints.clear();
            goal.request.goal_constraints[i] = kinematic_constraints::mergeConstraints(goal.request.goal_constraints[i], c);
          }
        }
      }
      else
        ROS_ERROR("Unable to construct goal representation");

    if (path_constraints_)
      goal.request.path_constraints = *path_constraints_;
    goal_out = goal;
  }

  void constructGoal(moveit_msgs::PickupGoal &goal_out, const std::string &object)
  {
    moveit_msgs::PickupGoal goal;
    goal.target_name = object;
    goal.group_name = opt_.group_name_;
    goal.end_effector = getEndEffector();
    goal.allowed_planning_time = planning_time_;
    goal.support_surface_name = support_surface_;
    goal.planner_id = planner_id_;

    if (path_constraints_)
      goal.path_constraints = *path_constraints_;

    goal_out = goal;
  }

  void constructGoal(moveit_msgs::PlaceGoal &goal_out, const std::string &object)
  {
    moveit_msgs::PlaceGoal goal;
    goal.attached_object_name = object;
    goal.group_name = opt_.group_name_;
    goal.allowed_planning_time = planning_time_;
    goal.support_surface_name = support_surface_;
    goal.planner_id = planner_id_;

    if (path_constraints_)
      goal.path_constraints = *path_constraints_;

    goal_out = goal;
  }

  void setPathConstraints(const moveit_msgs::Constraints &constraint)
  {
    path_constraints_.reset(new moveit_msgs::Constraints(constraint));
  }

  bool setPathConstraints(const std::string &constraint)
  {
    if (constraints_storage_)
    {
      moveit_warehouse::ConstraintsWithMetadata msg_m;
      if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
      {
        path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
    return true;
      }
      else
        return false;
    }
    else
      return false;
  }

  void clearPathConstraints()
  {
    path_constraints_.reset();
  }

  std::vector<std::string> getKnownConstraints() const
  {
    while (initializing_constraints_)
    {
      static ros::WallDuration d(0.01);
      d.sleep();
    }

    std::vector<std::string> c;
    if (constraints_storage_)
      constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);

    return c;
  }

  void initializeConstraintsStorage(const std::string &host, unsigned int port)
  {
    initializing_constraints_ = true;
    if (constraints_init_thread_)
      constraints_init_thread_->join();
    constraints_init_thread_.reset(new boost::thread(boost::bind(&MoveGroupImpl::initializeConstraintsStorageThread, this, host, port)));
  }

  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
  {
    workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
    workspace_parameters_.header.stamp = ros::Time::now();
    workspace_parameters_.min_corner.x = minx;
    workspace_parameters_.min_corner.y = miny;
    workspace_parameters_.min_corner.z = minz;
    workspace_parameters_.max_corner.x = maxx;
    workspace_parameters_.max_corner.y = maxy;
    workspace_parameters_.max_corner.z = maxz;
  }

private:

  void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
  {
    try
    {
      constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(host, port));
      ROS_DEBUG("Connected to constraints database");
    }
    catch(std::runtime_error &ex)
    {
      ROS_DEBUG("%s", ex.what());
    }
    initializing_constraints_ = false;
  }

  Options opt_;
  ros::NodeHandle node_handle_;
  boost::shared_ptr<tf::Transformer> tf_;
  robot_model::RobotModelConstPtr robot_model_;
  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
  boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
  boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
  boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;

  // general planning params
  robot_state::RobotStatePtr considered_start_state_;
  moveit_msgs::WorkspaceParameters workspace_parameters_;
  double planning_time_;
  std::string planner_id_;
  double goal_joint_tolerance_;
  double goal_position_tolerance_;
  double goal_orientation_tolerance_;
  bool can_look_;
  bool can_replan_;
  double replan_delay_;

  // joint state goal
  robot_state::RobotStatePtr joint_state_target_;

  // pose goal;
  // for each link we have a set of possible goal locations;
  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;

  // common properties for goals
  ActiveTargetType active_target_;
  boost::scoped_ptr<moveit_msgs::Constraints> path_constraints_;
  std::string end_effector_link_;
  std::string pose_reference_frame_;
  std::string support_surface_;

  // ROS communication
  ros::Publisher trajectory_event_publisher_;
  ros::ServiceClient execute_service_;
  ros::ServiceClient query_service_;
  ros::ServiceClient cartesian_path_service_;
  boost::scoped_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
  boost::scoped_ptr<boost::thread> constraints_init_thread_;
  bool initializing_constraints_;
};

MoveGroup::MoveGroup(const std::string &group_name, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
{
  if (!ros::ok())
    throw std::runtime_error("ROS does not seem to be running");
  impl_ = new MoveGroupImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_server);
}

MoveGroup::MoveGroup(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
{
  impl_ = new MoveGroupImpl(opt, tf ? tf : getSharedTF(), wait_for_server);
}

MoveGroup::~MoveGroup()
{
  delete impl_;
}

const std::string& MoveGroup::getName() const
{
  return impl_->getOptions().group_name_;
}

bool MoveGroup::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
{
  return impl_->getInterfaceDescription(desc);
}

void MoveGroup::setPlannerId(const std::string &planner_id)
{
  impl_->setPlannerId(planner_id);
}

bool MoveGroup::asyncMove()
{
  return impl_->move(false);
}

bool MoveGroup::move()
{
  return impl_->move(true);
}

bool MoveGroup::asyncExecute(const Plan &plan)
{
  return impl_->execute(plan, false);
}

bool MoveGroup::execute(const Plan &plan)
{
  return impl_->execute(plan, true);
}

bool MoveGroup::plan(Plan &plan)
{
  return impl_->plan(plan);
}

bool MoveGroup::pick(const std::string &object)
{
  return impl_->pick(object, std::vector<manipulation_msgs::Grasp>());
}

bool MoveGroup::pick(const std::string &object, const manipulation_msgs::Grasp &grasp)
{
  return impl_->pick(object, std::vector<manipulation_msgs::Grasp>(1, grasp));
}

bool MoveGroup::pick(const std::string &object, const std::vector<manipulation_msgs::Grasp> &grasps)
{
  return impl_->pick(object, grasps);
}

bool MoveGroup::place(const std::string &object)
{
  return impl_->place(object, std::vector<manipulation_msgs::PlaceLocation>());
}

bool MoveGroup::place(const std::string &object, const std::vector<manipulation_msgs::PlaceLocation> &locations)
{
  return impl_->place(object, locations);
}

bool MoveGroup::place(const std::string &object, const std::vector<geometry_msgs::PoseStamped> &poses)
{
  return impl_->place(object, poses);
}

bool MoveGroup::place(const std::string &object, const geometry_msgs::PoseStamped &pose)
{
  return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose));
}

double MoveGroup::computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double eef_step, double jump_threshold,
                       moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions)
{
  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, avoid_collisions);
}

void MoveGroup::stop()
{
  impl_->stop();
}

void MoveGroup::setStartState(const robot_state::RobotState &start_state)
{
  impl_->setStartState(start_state);
}

void MoveGroup::setStartStateToCurrentState()
{
  impl_->setStartStateToCurrentState();
}

void MoveGroup::setRandomTarget()
{
  impl_->getJointStateTarget()->setToRandomValues();
  impl_->setTargetType(JOINT);
}

bool MoveGroup::setNamedTarget(const std::string &name)
{
  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
  if (it != remembered_joint_values_.end())
  {
    return setJointValueTarget(it->second);
  }
  else
  {
    if (impl_->getJointStateTarget()->setToDefaultState(name))
    {
      impl_->setTargetType(JOINT);
      return true;
    }
    return false;
  }
}

bool MoveGroup::setJointValueTarget(const std::vector<double> &joint_values)
{
  impl_->setTargetType(JOINT);
  if (impl_->getJointStateTarget()->setVariableValues(joint_values))
    return impl_->getJointStateTarget()->satisfiesBounds(impl_->getGoalJointTolerance());
  else
    return false;
}

bool MoveGroup::setJointValueTarget(const std::map<std::string, double> &joint_values)
{
  impl_->setTargetType(JOINT);
  impl_->getJointStateTarget()->setVariableValues(joint_values);
  return impl_->getJointStateTarget()->satisfiesBounds(impl_->getGoalJointTolerance());
}

bool MoveGroup::setJointValueTarget(const robot_state::RobotState &kinematic_state)
{
  return setJointValueTarget(*kinematic_state.getJointStateGroup(getName()));
}

bool MoveGroup::setJointValueTarget(const robot_state::JointStateGroup &joint_state_group)
{
  std::map<std::string, double> variable_values;
  joint_state_group.getVariableValues(variable_values);
  return setJointValueTarget(variable_values);
}

bool MoveGroup::setJointValueTarget(const robot_state::JointState &joint_state)
{
  return setJointValueTarget(joint_state.getName(), joint_state.getVariableValues());
}

bool MoveGroup::setJointValueTarget(const std::string &joint_name, double value)
{
  std::vector<double> values(1, value);
  return setJointValueTarget(joint_name, values);
}

bool MoveGroup::setJointValueTarget(const std::string &joint_name, const std::vector<double> &values)
{
  impl_->setTargetType(JOINT);
  robot_state::JointState *joint_state = impl_->getJointStateTarget()->getJointState(joint_name);
  if (joint_state)
    if (joint_state->setVariableValues(values))
      return true;
  return false;
}

bool MoveGroup::setJointValueTarget(const sensor_msgs::JointState &state)
{
  impl_->setTargetType(JOINT);
  impl_->getJointStateTarget()->setVariableValues(state);
  return impl_->getJointStateTarget()->satisfiesBounds(impl_->getGoalJointTolerance());
}

const robot_state::JointStateGroup& MoveGroup::getJointValueTarget() const
{
  return *impl_->getJointStateTarget();
}

const std::string& MoveGroup::getEndEffectorLink() const
{
  return impl_->getEndEffectorLink();
}

const std::string& MoveGroup::getEndEffector() const
{
  return impl_->getEndEffector();
}

bool MoveGroup::setEndEffectorLink(const std::string &link_name)
{
  if (impl_->getEndEffectorLink().empty() || link_name.empty())
    return false;
  impl_->setEndEffectorLink(link_name);
  impl_->setTargetType(POSE);
  return true;
}

bool MoveGroup::setEndEffector(const std::string &eef_name)
{
  const robot_model::JointModelGroup *jmg = impl_->getRobotModel()->getEndEffector(eef_name);
  if (jmg)
    return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
  return false;
}

void MoveGroup::clearPoseTarget(const std::string &end_effector_link)
{
  impl_->clearPoseTarget(end_effector_link);
}

void MoveGroup::clearPoseTargets()
{
  impl_->clearPoseTargets();
}

bool MoveGroup::setPoseTarget(const Eigen::Affine3d &pose, const std::string &end_effector_link)
{
  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
  tf::poseEigenToMsg(pose, pose_msg[0].pose);
  pose_msg[0].header.frame_id = getPoseReferenceFrame();
  pose_msg[0].header.stamp = ros::Time::now();
  return setPoseTargets(pose_msg, end_effector_link);
}

bool MoveGroup::setPoseTarget(const geometry_msgs::Pose &target, const std::string &end_effector_link)
{
  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
  pose_msg[0].pose = target;
  pose_msg[0].header.frame_id = getPoseReferenceFrame();
  pose_msg[0].header.stamp = ros::Time::now();
  return setPoseTargets(pose_msg, end_effector_link);
}

bool MoveGroup::setPoseTarget(const geometry_msgs::PoseStamped &target, const std::string &end_effector_link)
{
  std::vector<geometry_msgs::PoseStamped> targets(1, target);
  return setPoseTargets(targets, end_effector_link);
}

bool MoveGroup::setPoseTargets(const EigenSTL::vector_Affine3d &target, const std::string &end_effector_link)
{
  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
  ros::Time tm = ros::Time::now();
  const std::string &frame_id = getPoseReferenceFrame();
  for (std::size_t i = 0 ; i < target.size() ; ++i)
  {
    tf::poseEigenToMsg(target[i], pose_out[i].pose);
    pose_out[i].header.stamp = tm;
    pose_out[i].header.frame_id = frame_id;
  }
  return setPoseTargets(pose_out, end_effector_link);
}

bool MoveGroup::setPoseTargets(const std::vector<geometry_msgs::Pose> &target, const std::string &end_effector_link)
{
  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
  ros::Time tm = ros::Time::now();
  const std::string &frame_id = getPoseReferenceFrame();
  for (std::size_t i = 0 ; i < target.size() ; ++i)
  {
    target_stamped[i].pose = target[i];
    target_stamped[i].header.stamp = tm;
    target_stamped[i].header.frame_id = frame_id;
  }
  return setPoseTargets(target_stamped, end_effector_link);
}

bool MoveGroup::setPoseTargets(const std::vector<geometry_msgs::PoseStamped> &target, const std::string &end_effector_link)
{
  if (target.empty())
  {
    ROS_ERROR("No pose specified as goal target");
    return false;
  }
  else
  {
    impl_->setTargetType(POSE);
    return impl_->setPoseTargets(target, end_effector_link);
  }
}

const geometry_msgs::PoseStamped& MoveGroup::getPoseTarget(const std::string &end_effector_link) const
{
  return impl_->getPoseTarget(end_effector_link);
}

const std::vector<geometry_msgs::PoseStamped>& MoveGroup::getPoseTargets(const std::string &end_effector_link) const
{
  return impl_->getPoseTargets(end_effector_link);
}

namespace
{
inline void transformPose(const tf::Transformer& tf, const std::string &desired_frame, geometry_msgs::PoseStamped &target)
{
  if (desired_frame != target.header.frame_id)
  {
    tf::Pose pose;
    tf::poseMsgToTF(target.pose, pose);
    tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
    tf::Stamped<tf::Pose> stamped_target_out;
    tf.transformPose(desired_frame, stamped_target, stamped_target_out);
    target.header.frame_id = stamped_target_out.frame_id_;
    //    target.header.stamp = stamped_target_out.stamp_; // we leave the stamp to ros::Time(0) on purpose
    tf::poseTFToMsg(stamped_target_out, target.pose);
  }
}
}

bool MoveGroup::setPositionTarget(double x, double y, double z, const std::string &end_effector_link)
{
  geometry_msgs::PoseStamped target;
  if (impl_->hasPoseTarget(end_effector_link))
  {
    target = getPoseTarget(end_effector_link);
    transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
  }
  else
  {
    target.pose.orientation.x = 0.0;
    target.pose.orientation.y = 0.0;
    target.pose.orientation.z = 0.0;
    target.pose.orientation.w = 1.0;
    target.header.frame_id = impl_->getPoseReferenceFrame();
  }

  target.pose.position.x = x;
  target.pose.position.y = y;
  target.pose.position.z = z;
  bool result = setPoseTarget(target, end_effector_link);
  impl_->setTargetType(POSITION);
  return result;
}

bool MoveGroup::setRPYTarget(double r, double p, double y, const std::string &end_effector_link)
{
  geometry_msgs::PoseStamped target;
  if (impl_->hasPoseTarget(end_effector_link))
  {
    target = getPoseTarget(end_effector_link);
    transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
  }
  else
  {
    target.pose.position.x = 0.0;
    target.pose.position.y = 0.0;
    target.pose.position.z = 0.0;
    target.header.frame_id = impl_->getPoseReferenceFrame();
  }

  tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
  bool result = setPoseTarget(target, end_effector_link);
  impl_->setTargetType(ORIENTATION);
  return result;
}

bool MoveGroup::setOrientationTarget(double x, double y, double z, double w, const std::string &end_effector_link)
{
  geometry_msgs::PoseStamped target;
  if (impl_->hasPoseTarget(end_effector_link))
  {
    target = getPoseTarget(end_effector_link);
    transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
  }
  else
  {
    target.pose.position.x = 0.0;
    target.pose.position.y = 0.0;
    target.pose.position.z = 0.0;
    target.header.frame_id = impl_->getPoseReferenceFrame();
  }

  target.pose.orientation.x = x;
  target.pose.orientation.y = y;
  target.pose.orientation.z = z;
  target.pose.orientation.w = w;
  bool result = setPoseTarget(target, end_effector_link);
  impl_->setTargetType(ORIENTATION);
  return result;
}

void MoveGroup::setPoseReferenceFrame(const std::string &pose_reference_frame)
{
  impl_->setPoseReferenceFrame(pose_reference_frame);
}

const std::string& MoveGroup::getPoseReferenceFrame() const
{
  return impl_->getPoseReferenceFrame();
}

double MoveGroup::getGoalJointTolerance() const
{
  return impl_->getGoalJointTolerance();
}

double MoveGroup::getGoalPositionTolerance() const
{
  return impl_->getGoalPositionTolerance();
}

double MoveGroup::getGoalOrientationTolerance() const
{
  return impl_->getGoalOrientationTolerance();
}

void MoveGroup::setGoalTolerance(double tolerance)
{
  setGoalJointTolerance(tolerance);
  setGoalPositionTolerance(tolerance);
  setGoalOrientationTolerance(tolerance);
}

void MoveGroup::setGoalJointTolerance(double tolerance)
{
  impl_->setGoalJointTolerance(tolerance);
}

void MoveGroup::setGoalPositionTolerance(double tolerance)
{
  impl_->setGoalPositionTolerance(tolerance);
}

void MoveGroup::setGoalOrientationTolerance(double tolerance)
{
  impl_->setGoalOrientationTolerance(tolerance);
}

void MoveGroup::rememberJointValues(const std::string &name)
{
  rememberJointValues(name, getCurrentJointValues());
}

std::vector<double> MoveGroup::getCurrentJointValues()
{
  robot_state::RobotStatePtr current_state;
  std::vector<double> values;
  if (impl_->getCurrentState(current_state))
    current_state->getJointStateGroup(getName())->getVariableValues(values);
  return values;
}

std::vector<double> MoveGroup::getRandomJointValues()
{
  std::vector<double> backup;
  impl_->getJointStateTarget()->getVariableValues(backup);

  impl_->getJointStateTarget()->setToRandomValues();
  std::vector<double> r;
  impl_->getJointStateTarget()->getVariableValues(r);

  impl_->getJointStateTarget()->setVariableValues(backup);
  return r;
}

geometry_msgs::PoseStamped MoveGroup::getRandomPose(const std::string &end_effector_link)
{
  const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
  Eigen::Affine3d pose;
  pose.setIdentity();
  if (eef.empty())
    ROS_ERROR("No end-effector specified");
  else
  {
    robot_state::RobotStatePtr current_state;
    if (impl_->getCurrentState(current_state))
    {
      current_state->getJointStateGroup(getName())->setToRandomValues();
      const robot_state::LinkState *ls = current_state->getLinkState(eef);
      if (ls)
        pose = ls->getGlobalLinkTransform();
    }
  }
  geometry_msgs::PoseStamped pose_msg;
  pose_msg.header.stamp = ros::Time::now();
  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
  tf::poseEigenToMsg(pose, pose_msg.pose);
  return pose_msg;
}

geometry_msgs::PoseStamped MoveGroup::getCurrentPose(const std::string &end_effector_link)
{
  const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
  Eigen::Affine3d pose;
  pose.setIdentity();
  if (eef.empty())
    ROS_ERROR("No end-effector specified");
  else
  {
    robot_state::RobotStatePtr current_state;
    if (impl_->getCurrentState(current_state))
    {
      const robot_state::LinkState *ls = current_state->getLinkState(eef);
      if (ls)
        pose = ls->getGlobalLinkTransform();
    }
  }
  geometry_msgs::PoseStamped pose_msg;
  pose_msg.header.stamp = ros::Time::now();
  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
  tf::poseEigenToMsg(pose, pose_msg.pose);
  return pose_msg;
}

std::vector<double> MoveGroup::getCurrentRPY(const std::string &end_effector_link)
{
  std::vector<double> result;
  const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
  if (eef.empty())
    ROS_ERROR("No end-effector specified");
  else
  {
    robot_state::RobotStatePtr current_state;
    if (impl_->getCurrentState(current_state))
    {
      const robot_state::LinkState *ls = current_state->getLinkState(eef);
      if (ls)
      {
    result.resize(3);
    tf::Matrix3x3 ptf;
    tf::matrixEigenToTF(ls->getGlobalLinkTransform().rotation(), ptf);
    tfScalar pitch, roll, yaw;
    ptf.getRPY(roll, pitch, yaw);
    result[0] = roll;
    result[1] = pitch;
    result[2] = yaw;
      }
    }
  }
  return result;
}

const std::vector<std::string>& MoveGroup::getJoints() const
{
  return impl_->getJointStateTarget()->getJointModelGroup()->getJointModelNames();
}

unsigned int MoveGroup::getVariableCount() const
{
  return impl_->getJointStateTarget()->getJointModelGroup()->getVariableCount();
}

robot_state::RobotStatePtr MoveGroup::getCurrentState()
{
  robot_state::RobotStatePtr current_state;
  impl_->getCurrentState(current_state);
  return current_state;
}

void MoveGroup::rememberJointValues(const std::string &name, const std::vector<double> &values)
{
  remembered_joint_values_[name] = values;
}

void MoveGroup::forgetJointValues(const std::string &name)
{
  remembered_joint_values_.erase(name);
}

void MoveGroup::allowLooking(bool flag)
{
  impl_->allowLooking(flag);
}

void MoveGroup::allowReplanning(bool flag)
{
  impl_->allowReplanning(flag);
}

std::vector<std::string> MoveGroup::getKnownConstraints() const
{
  return impl_->getKnownConstraints();
}

bool MoveGroup::setPathConstraints(const std::string &constraint)
{
  return impl_->setPathConstraints(constraint);
}

void MoveGroup::setPathConstraints(const moveit_msgs::Constraints &constraint)
{
  impl_->setPathConstraints(constraint);
}

void MoveGroup::clearPathConstraints()
{
  impl_->clearPathConstraints();
}

void MoveGroup::setConstraintsDatabase(const std::string &host, unsigned int port)
{
  impl_->initializeConstraintsStorage(host, port);
}

void MoveGroup::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
{
  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
}

void MoveGroup::setPlanningTime(double seconds)
{
  impl_->setPlanningTime(seconds);
}

double MoveGroup::getPlanningTime() const
{
  return impl_->getPlanningTime();
}

void MoveGroup::setSupportSurfaceName(const std::string &name)
{
  impl_->setSupportSurfaceName(name);
}

const std::string& MoveGroup::getPlanningFrame() const
{
  return impl_->getRobotModel()->getModelFrame();
}

}
}