move_group.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, SRI International
00005  *  Copyright (c) 2013, Ioan A. Sucan
00006  *  Copyright (c) 2012, Willow Garage, Inc.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *********************************************************************/
00036 
00037 /* Author: Ioan Sucan, Sachin Chitta */
00038 
00039 #include <stdexcept>
00040 #include <sstream>
00041 #include <moveit/warehouse/constraints_storage.h>
00042 #include <moveit/kinematic_constraints/utils.h>
00043 #include <moveit/move_group/capability_names.h>
00044 #include <moveit/move_group_pick_place_capability/capability_names.h>
00045 #include <moveit/move_group_interface/move_group.h>
00046 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00047 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00048 #include <moveit/planning_scene_interface/planning_scene_interface.h>
00049 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
00050 #include <moveit/common_planning_interface_objects/common_objects.h>
00051 #include <moveit/robot_state/conversions.h>
00052 #include <moveit_msgs/MoveGroupAction.h>
00053 #include <moveit_msgs/PickupAction.h>
00054 #include <moveit_msgs/ExecuteTrajectoryAction.h>
00055 #include <moveit_msgs/PlaceAction.h>
00056 #include <moveit_msgs/ExecuteKnownTrajectory.h>
00057 #include <moveit_msgs/QueryPlannerInterfaces.h>
00058 #include <moveit_msgs/GetCartesianPath.h>
00059 #include <moveit_msgs/GraspPlanning.h>
00060 
00061 #include <actionlib/client/simple_action_client.h>
00062 #include <eigen_conversions/eigen_msg.h>
00063 #include <std_msgs/String.h>
00064 #include <tf/transform_listener.h>
00065 #include <tf/transform_datatypes.h>
00066 #include <tf_conversions/tf_eigen.h>
00067 #include <ros/console.h>
00068 #include <ros/ros.h>
00069 
00070 namespace moveit
00071 {
00072 namespace planning_interface
00073 {
00074 const std::string MoveGroup::ROBOT_DESCRIPTION =
00075     "robot_description";  // name of the robot description (a param name, so it can be changed externally)
00076 
00077 const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps";  // name of the service that can be used to plan grasps
00078 
00079 namespace
00080 {
00081 enum ActiveTargetType
00082 {
00083   JOINT,
00084   POSE,
00085   POSITION,
00086   ORIENTATION
00087 };
00088 }
00089 
00090 class MoveGroup::MoveGroupImpl
00091 {
00092 public:
00093   MoveGroupImpl(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf,
00094                 const ros::WallDuration& wait_for_servers)
00095     : opt_(opt), node_handle_(opt.node_handle_), tf_(tf)
00096   {
00097     robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_);
00098     if (!getRobotModel())
00099     {
00100       std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
00101                           "parameter server.";
00102       ROS_FATAL_STREAM_NAMED("move_group_interface", error);
00103       throw std::runtime_error(error);
00104     }
00105 
00106     if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
00107     {
00108       std::string error = "Group '" + opt.group_name_ + "' was not found.";
00109       ROS_FATAL_STREAM_NAMED("move_group_interface", error);
00110       throw std::runtime_error(error);
00111     }
00112 
00113     joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
00114 
00115     joint_state_target_.reset(new robot_state::RobotState(getRobotModel()));
00116     joint_state_target_->setToDefaultValues();
00117     active_target_ = JOINT;
00118     can_look_ = false;
00119     can_replan_ = false;
00120     replan_delay_ = 2.0;
00121     goal_joint_tolerance_ = 1e-4;
00122     goal_position_tolerance_ = 1e-4;     // 0.1 mm
00123     goal_orientation_tolerance_ = 1e-3;  // ~0.1 deg
00124     planning_time_ = 5.0;
00125     num_planning_attempts_ = 1;
00126     max_velocity_scaling_factor_ = 1.0;
00127     max_acceleration_scaling_factor_ = 1.0;
00128     initializing_constraints_ = false;
00129 
00130     if (joint_model_group_->isChain())
00131       end_effector_link_ = joint_model_group_->getLinkModelNames().back();
00132     pose_reference_frame_ = getRobotModel()->getModelFrame();
00133 
00134     setPlannerId(getDefaultPlannerId(opt.group_name_));
00135 
00136     trajectory_event_publisher_ = node_handle_.advertise<std_msgs::String>(
00137         trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false);
00138     attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(
00139         planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false);
00140 
00141     current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_, node_handle_);
00142 
00143     ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers;
00144     if (wait_for_servers == ros::WallDuration())
00145       timeout_for_servers = ros::WallTime();  // wait for ever
00146     double allotted_time = wait_for_servers.toSec();
00147 
00148     move_action_client_.reset(
00149         new actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>(node_handle_, move_group::MOVE_ACTION, false));
00150     waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time);
00151 
00152     pick_action_client_.reset(
00153         new actionlib::SimpleActionClient<moveit_msgs::PickupAction>(node_handle_, move_group::PICKUP_ACTION, false));
00154     waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time);
00155 
00156     place_action_client_.reset(
00157         new actionlib::SimpleActionClient<moveit_msgs::PlaceAction>(node_handle_, move_group::PLACE_ACTION, false));
00158     waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time);
00159 
00160     execute_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction>(
00161         node_handle_, move_group::EXECUTE_ACTION_NAME, false));
00162     // In Indigo, we maintain backwards compatibility
00163     // silently falling back to old service when new action is not available
00164     waitForExecuteActionOrService(timeout_for_servers);
00165 
00166     query_service_ =
00167         node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
00168     cartesian_path_service_ =
00169         node_handle_.serviceClient<moveit_msgs::GetCartesianPath>(move_group::CARTESIAN_PATH_SERVICE_NAME);
00170     plan_grasps_service_ = node_handle_.serviceClient<moveit_msgs::GraspPlanning>(GRASP_PLANNING_SERVICE_NAME);
00171 
00172     ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take MoveGroup commands for group " << opt.group_name_
00173                                                                                                 << ".");
00174   }
00175 
00176   template <typename T>
00177   void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time)
00178   {
00179     ROS_DEBUG_NAMED("move_group_interface", "Waiting for MoveGroup action server (%s)...", name.c_str());
00180 
00181     // wait for the server (and spin as needed)
00182     if (timeout == ros::WallTime())  // wait forever
00183     {
00184       while (node_handle_.ok() && !action->isServerConnected())
00185       {
00186         ros::WallDuration(0.001).sleep();
00187         // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
00188         ros::CallbackQueue* queue = dynamic_cast<ros::CallbackQueue*>(node_handle_.getCallbackQueue());
00189         if (queue)
00190         {
00191           queue->callAvailable();
00192         }
00193         else  // in case of nodelets and specific callback queue implementations
00194         {
00195           ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
00196                                                       "handling.");
00197         }
00198       }
00199     }
00200     else  // wait with timeout
00201     {
00202       while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
00203       {
00204         ros::WallDuration(0.001).sleep();
00205         // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
00206         ros::CallbackQueue* queue = dynamic_cast<ros::CallbackQueue*>(node_handle_.getCallbackQueue());
00207         if (queue)
00208         {
00209           queue->callAvailable();
00210         }
00211         else  // in case of nodelets and specific callback queue implementations
00212         {
00213           ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
00214                                                       "handling.");
00215         }
00216       }
00217     }
00218 
00219     if (!action->isServerConnected())
00220     {
00221       std::stringstream error;
00222       error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
00223             << "s)";
00224       throw std::runtime_error(error.str());
00225     }
00226     else
00227     {
00228       ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str());
00229     }
00230   }
00231 
00232   void waitForExecuteActionOrService(ros::WallTime timeout)
00233   {
00234     ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...",
00235                     move_group::EXECUTE_ACTION_NAME.c_str());
00236 
00237     // Deprecated service
00238     execute_service_ =
00239         node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
00240 
00241     // wait for either of action or service
00242     if (timeout == ros::WallTime())  // wait forever
00243     {
00244       while (!execute_action_client_->isServerConnected() && !execute_service_.exists())
00245       {
00246         ros::WallDuration(0.001).sleep();
00247         // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
00248         ros::CallbackQueue* queue = dynamic_cast<ros::CallbackQueue*>(node_handle_.getCallbackQueue());
00249         if (queue)
00250         {
00251           queue->callAvailable();
00252         }
00253         else  // in case of nodelets and specific callback queue implementations
00254         {
00255           ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
00256                                                       "handling.");
00257         }
00258       }
00259     }
00260     else  // wait with timeout
00261     {
00262       while (!execute_action_client_->isServerConnected() && !execute_service_.exists() &&
00263              timeout > ros::WallTime::now())
00264       {
00265         ros::WallDuration(0.001).sleep();
00266         // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
00267         ros::CallbackQueue* queue = dynamic_cast<ros::CallbackQueue*>(node_handle_.getCallbackQueue());
00268         if (queue)
00269         {
00270           queue->callAvailable();
00271         }
00272         else  // in case of nodelets and specific callback queue implementations
00273         {
00274           ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
00275                                                       "handling.");
00276         }
00277       }
00278     }
00279 
00280     std::string version = "new action";
00281     if (!execute_action_client_->isServerConnected())
00282     {
00283       if (execute_service_.exists())
00284         version = "old service";
00285       else
00286       {
00287         ROS_ERROR_STREAM_NAMED("move_group_interface",
00288                                "Unable to find execution action on topic: "
00289                                    << node_handle_.getNamespace() + move_group::EXECUTE_ACTION_NAME << " or service: "
00290                                    << node_handle_.getNamespace() + move_group::EXECUTE_SERVICE_NAME);
00291         throw std::runtime_error("No Trajectory execution capability available.");
00292       }
00293       execute_action_client_.reset();
00294     }
00295     ROS_INFO("TrajectoryExecution will use %s capability.", version.c_str());
00296   }
00297 
00298   ~MoveGroupImpl()
00299   {
00300     if (constraints_init_thread_)
00301       constraints_init_thread_->join();
00302   }
00303 
00304   const boost::shared_ptr<tf::Transformer>& getTF() const
00305   {
00306     return tf_;
00307   }
00308 
00309   const Options& getOptions() const
00310   {
00311     return opt_;
00312   }
00313 
00314   const robot_model::RobotModelConstPtr& getRobotModel() const
00315   {
00316     return robot_model_;
00317   }
00318 
00319   const robot_model::JointModelGroup* getJointModelGroup() const
00320   {
00321     return joint_model_group_;
00322   }
00323 
00324   bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
00325   {
00326     moveit_msgs::QueryPlannerInterfaces::Request req;
00327     moveit_msgs::QueryPlannerInterfaces::Response res;
00328     if (query_service_.call(req, res))
00329       if (!res.planner_interfaces.empty())
00330       {
00331         desc = res.planner_interfaces.front();
00332         return true;
00333       }
00334     return false;
00335   }
00336 
00337   std::string getDefaultPlannerId(const std::string& group) const
00338   {
00339     std::stringstream param_name;
00340     param_name << "move_group";
00341     if (!group.empty())
00342       param_name << "/" << group;
00343     param_name << "/default_planner_config";
00344 
00345     std::string default_planner_config;
00346     node_handle_.getParam(param_name.str(), default_planner_config);
00347     return default_planner_config;
00348   }
00349 
00350   void setPlannerId(const std::string& planner_id)
00351   {
00352     planner_id_ = planner_id;
00353   }
00354 
00355   void setNumPlanningAttempts(unsigned int num_planning_attempts)
00356   {
00357     num_planning_attempts_ = num_planning_attempts;
00358   }
00359 
00360   void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
00361   {
00362     max_velocity_scaling_factor_ = max_velocity_scaling_factor;
00363   }
00364 
00365   void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
00366   {
00367     max_acceleration_scaling_factor_ = max_acceleration_scaling_factor;
00368   }
00369 
00370   robot_state::RobotState& getJointStateTarget()
00371   {
00372     return *joint_state_target_;
00373   }
00374 
00375   void setStartState(const robot_state::RobotState& start_state)
00376   {
00377     considered_start_state_.reset(new robot_state::RobotState(start_state));
00378   }
00379 
00380   void setStartStateToCurrentState()
00381   {
00382     considered_start_state_.reset();
00383   }
00384 
00385   robot_state::RobotStatePtr getStartState()
00386   {
00387     if (considered_start_state_)
00388       return considered_start_state_;
00389     else
00390     {
00391       robot_state::RobotStatePtr s;
00392       getCurrentState(s);
00393       return s;
00394     }
00395   }
00396 
00397   bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
00398                            const std::string& frame, bool approx)
00399   {
00400     const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
00401     // this only works if we have an end-effector
00402     if (!eef.empty())
00403     {
00404       // first we set the goal to be the same as the start state
00405       moveit::core::RobotStatePtr c = getStartState();
00406       if (c)
00407       {
00408         setTargetType(JOINT);
00409         c->enforceBounds();
00410         getJointStateTarget() = *c;
00411         if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
00412           return false;
00413       }
00414       else
00415         return false;
00416 
00417       // we may need to do approximate IK
00418       kinematics::KinematicsQueryOptions o;
00419       o.return_approximate_solution = approx;
00420 
00421       // if no frame transforms are needed, call IK directly
00422       if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
00423         return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0, 0.0,
00424                                                moveit::core::GroupStateValidityCallbackFn(), o);
00425       else
00426       {
00427         if (c->knowsFrameTransform(frame))
00428         {
00429           // transform the pose first if possible, then do IK
00430           const Eigen::Affine3d& t = getJointStateTarget().getFrameTransform(frame);
00431           Eigen::Affine3d p;
00432           tf::poseMsgToEigen(eef_pose, p);
00433           return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0, 0.0,
00434                                                  moveit::core::GroupStateValidityCallbackFn(), o);
00435         }
00436         else
00437         {
00438           logError("Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
00439                    getRobotModel()->getModelFrame().c_str());
00440           return false;
00441         }
00442       }
00443     }
00444     else
00445       return false;
00446   }
00447 
00448   void setEndEffectorLink(const std::string& end_effector)
00449   {
00450     end_effector_link_ = end_effector;
00451   }
00452 
00453   void clearPoseTarget(const std::string& end_effector_link)
00454   {
00455     pose_targets_.erase(end_effector_link);
00456   }
00457 
00458   void clearPoseTargets()
00459   {
00460     pose_targets_.clear();
00461   }
00462 
00463   const std::string& getEndEffectorLink() const
00464   {
00465     return end_effector_link_;
00466   }
00467 
00468   const std::string& getEndEffector() const
00469   {
00470     if (!end_effector_link_.empty())
00471     {
00472       const std::vector<std::string>& possible_eefs =
00473           getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
00474       for (std::size_t i = 0; i < possible_eefs.size(); ++i)
00475         if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
00476           return possible_eefs[i];
00477     }
00478     static std::string empty;
00479     return empty;
00480   }
00481 
00482   bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
00483   {
00484     const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00485     if (eef.empty())
00486     {
00487       ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for");
00488       return false;
00489     }
00490     else
00491     {
00492       pose_targets_[eef] = poses;
00493       // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
00494       std::vector<geometry_msgs::PoseStamped>& stored_poses = pose_targets_[eef];
00495       for (std::size_t i = 0; i < stored_poses.size(); ++i)
00496         stored_poses[i].header.stamp = ros::Time(0);
00497     }
00498     return true;
00499   }
00500 
00501   bool hasPoseTarget(const std::string& end_effector_link) const
00502   {
00503     const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00504     return pose_targets_.find(eef) != pose_targets_.end();
00505   }
00506 
00507   const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
00508   {
00509     const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00510 
00511     // if multiple pose targets are set, return the first one
00512     std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
00513     if (jt != pose_targets_.end())
00514       if (!jt->second.empty())
00515         return jt->second.at(0);
00516 
00517     // or return an error
00518     static const geometry_msgs::PoseStamped unknown;
00519     ROS_ERROR_NAMED("move_group_interface", "Pose for end effector '%s' not known.", eef.c_str());
00520     return unknown;
00521   }
00522 
00523   const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
00524   {
00525     const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00526 
00527     std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
00528     if (jt != pose_targets_.end())
00529       if (!jt->second.empty())
00530         return jt->second;
00531 
00532     // or return an error
00533     static const std::vector<geometry_msgs::PoseStamped> empty;
00534     ROS_ERROR_NAMED("move_group_interface", "Poses for end effector '%s' are not known.", eef.c_str());
00535     return empty;
00536   }
00537 
00538   void setPoseReferenceFrame(const std::string& pose_reference_frame)
00539   {
00540     pose_reference_frame_ = pose_reference_frame;
00541   }
00542 
00543   void setSupportSurfaceName(const std::string& support_surface)
00544   {
00545     support_surface_ = support_surface;
00546   }
00547 
00548   const std::string& getPoseReferenceFrame() const
00549   {
00550     return pose_reference_frame_;
00551   }
00552 
00553   void setTargetType(ActiveTargetType type)
00554   {
00555     active_target_ = type;
00556   }
00557 
00558   ActiveTargetType getTargetType() const
00559   {
00560     return active_target_;
00561   }
00562 
00563   bool startStateMonitor(double wait)
00564   {
00565     if (!current_state_monitor_)
00566     {
00567       ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state");
00568       return false;
00569     }
00570 
00571     // if needed, start the monitor and wait up to 1 second for a full robot state
00572     if (!current_state_monitor_->isActive())
00573       current_state_monitor_->startStateMonitor();
00574 
00575     current_state_monitor_->waitForCurrentState(opt_.group_name_, wait);
00576     return true;
00577   }
00578 
00579   bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0)
00580   {
00581     if (!current_state_monitor_)
00582     {
00583       ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state");
00584       return false;
00585     }
00586 
00587     // if needed, start the monitor and wait up to 1 second for a full robot state
00588     if (!current_state_monitor_->isActive())
00589       current_state_monitor_->startStateMonitor();
00590 
00591     if (!current_state_monitor_->waitForCurrentState(opt_.group_name_, wait_seconds))
00592       ROS_WARN_NAMED("move_group_interface", "Joint values for monitored state are requested but the full state is not "
00593                                              "known");
00594 
00595     current_state = current_state_monitor_->getCurrentState();
00596     return true;
00597   }
00598 
00600   MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
00601   {
00602     std::vector<moveit_msgs::PlaceLocation> locations;
00603     for (std::size_t i = 0; i < poses.size(); ++i)
00604     {
00605       moveit_msgs::PlaceLocation location;
00606       location.pre_place_approach.direction.vector.z = -1.0;
00607       location.post_place_retreat.direction.vector.x = -1.0;
00608       location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
00609       location.post_place_retreat.direction.header.frame_id = end_effector_link_;
00610 
00611       location.pre_place_approach.min_distance = 0.1;
00612       location.pre_place_approach.desired_distance = 0.2;
00613       location.post_place_retreat.min_distance = 0.0;
00614       location.post_place_retreat.desired_distance = 0.2;
00615       // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
00616 
00617       location.place_pose = poses[i];
00618       locations.push_back(location);
00619     }
00620     ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
00621                     (unsigned int)locations.size());
00622     return place(object, locations);
00623   }
00624 
00625   MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
00626   {
00627     if (!place_action_client_)
00628     {
00629       ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
00630       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00631     }
00632     if (!place_action_client_->isServerConnected())
00633     {
00634       ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
00635       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00636     }
00637     moveit_msgs::PlaceGoal goal;
00638     constructGoal(goal, object);
00639     goal.place_locations = locations;
00640     goal.planning_options.plan_only = false;
00641     goal.planning_options.look_around = can_look_;
00642     goal.planning_options.replan = can_replan_;
00643     goal.planning_options.replan_delay = replan_delay_;
00644     goal.planning_options.planning_scene_diff.is_diff = true;
00645     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00646 
00647     place_action_client_->sendGoal(goal);
00648     ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
00649     if (!place_action_client_->waitForResult())
00650     {
00651       ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
00652     }
00653     if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00654     {
00655       return MoveItErrorCode(place_action_client_->getResult()->error_code);
00656     }
00657     else
00658     {
00659       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
00660                                                              << place_action_client_->getState().getText());
00661       return MoveItErrorCode(place_action_client_->getResult()->error_code);
00662     }
00663   }
00664 
00665   MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
00666   {
00667     if (!pick_action_client_)
00668     {
00669       ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
00670       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00671     }
00672     if (!pick_action_client_->isServerConnected())
00673     {
00674       ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
00675       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00676     }
00677     moveit_msgs::PickupGoal goal;
00678     constructGoal(goal, object);
00679     goal.possible_grasps = grasps;
00680     goal.planning_options.plan_only = false;
00681     goal.planning_options.look_around = can_look_;
00682     goal.planning_options.replan = can_replan_;
00683     goal.planning_options.replan_delay = replan_delay_;
00684     goal.planning_options.planning_scene_diff.is_diff = true;
00685     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00686 
00687     pick_action_client_->sendGoal(goal);
00688     if (!pick_action_client_->waitForResult())
00689     {
00690       ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
00691     }
00692     if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00693     {
00694       return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00695     }
00696     else
00697     {
00698       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
00699                                                              << pick_action_client_->getState().getText());
00700       return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00701     }
00702   }
00703 
00704   MoveItErrorCode planGraspsAndPick(const std::string& object)
00705   {
00706     if (object.empty())
00707     {
00708       return planGraspsAndPick(moveit_msgs::CollisionObject());
00709     }
00710     moveit::planning_interface::PlanningSceneInterface psi;
00711 
00712     std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
00713 
00714     if (objects.size() < 1)
00715     {
00716       ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '"
00717                                                          << object << "', but the object could not be found");
00718       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
00719     }
00720 
00721     return planGraspsAndPick(objects[object]);
00722   }
00723 
00724   MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object)
00725   {
00726     if (!plan_grasps_service_)
00727     {
00728       ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
00729                                                          << GRASP_PLANNING_SERVICE_NAME
00730                                                          << "' is not available."
00731                                                             " This has to be implemented and started separately.");
00732       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00733     }
00734 
00735     moveit_msgs::GraspPlanning::Request request;
00736     moveit_msgs::GraspPlanning::Response response;
00737 
00738     request.group_name = opt_.group_name_;
00739     request.target = object;
00740     request.support_surfaces.push_back(support_surface_);
00741 
00742     ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
00743     if (!plan_grasps_service_.call(request, response) ||
00744         response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
00745     {
00746       ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
00747       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00748     }
00749 
00750     return pick(object.id, response.grasps);
00751   }
00752 
00753   MoveItErrorCode plan(Plan& plan)
00754   {
00755     if (!move_action_client_)
00756     {
00757       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00758     }
00759     if (!move_action_client_->isServerConnected())
00760     {
00761       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00762     }
00763 
00764     moveit_msgs::MoveGroupGoal goal;
00765     constructGoal(goal);
00766     goal.planning_options.plan_only = true;
00767     goal.planning_options.look_around = false;
00768     goal.planning_options.replan = false;
00769     goal.planning_options.planning_scene_diff.is_diff = true;
00770     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00771 
00772     move_action_client_->sendGoal(goal);
00773     if (!move_action_client_->waitForResult())
00774     {
00775       ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
00776     }
00777     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00778     {
00779       plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
00780       plan.start_state_ = move_action_client_->getResult()->trajectory_start;
00781       plan.planning_time_ = move_action_client_->getResult()->planning_time;
00782       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00783     }
00784     else
00785     {
00786       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
00787                                                              << move_action_client_->getState().getText());
00788       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00789     }
00790   }
00791 
00792   MoveItErrorCode move(bool wait)
00793   {
00794     if (!move_action_client_)
00795     {
00796       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00797     }
00798     if (!move_action_client_->isServerConnected())
00799     {
00800       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00801     }
00802 
00803     moveit_msgs::MoveGroupGoal goal;
00804     constructGoal(goal);
00805     goal.planning_options.plan_only = false;
00806     goal.planning_options.look_around = can_look_;
00807     goal.planning_options.replan = can_replan_;
00808     goal.planning_options.replan_delay = replan_delay_;
00809     goal.planning_options.planning_scene_diff.is_diff = true;
00810     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00811 
00812     move_action_client_->sendGoal(goal);
00813     if (!wait)
00814     {
00815       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
00816     }
00817 
00818     if (!move_action_client_->waitForResult())
00819     {
00820       ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
00821     }
00822 
00823     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00824     {
00825       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00826     }
00827     else
00828     {
00829       ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
00830                                                         << ": " << move_action_client_->getState().getText());
00831       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00832     }
00833   }
00834 
00835   MoveItErrorCode execute(const Plan& plan, bool wait)
00836   {
00837     if (!execute_action_client_)
00838     {
00839       // TODO: Remove this backwards compatibility code in L-turtle
00840       moveit_msgs::ExecuteKnownTrajectory::Request req;
00841       moveit_msgs::ExecuteKnownTrajectory::Response res;
00842       req.trajectory = plan.trajectory_;
00843       req.wait_for_execution = wait;
00844       if (execute_service_.call(req, res))
00845       {
00846         return MoveItErrorCode(res.error_code);
00847       }
00848       else
00849       {
00850         return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00851       }
00852     }
00853 
00854     if (!execute_action_client_->isServerConnected())
00855     {
00856       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00857     }
00858 
00859     moveit_msgs::ExecuteTrajectoryGoal goal;
00860     goal.trajectory = plan.trajectory_;
00861 
00862     execute_action_client_->sendGoal(goal);
00863     if (!wait)
00864     {
00865       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
00866     }
00867 
00868     if (!execute_action_client_->waitForResult())
00869     {
00870       ROS_INFO_STREAM("ExecuteTrajectory action returned early");
00871     }
00872 
00873     if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00874     {
00875       return MoveItErrorCode(execute_action_client_->getResult()->error_code);
00876     }
00877     else
00878     {
00879       ROS_INFO_STREAM(execute_action_client_->getState().toString() << ": "
00880                                                                     << execute_action_client_->getState().getText());
00881       return MoveItErrorCode(execute_action_client_->getResult()->error_code);
00882     }
00883   }
00884 
00885   double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
00886                               moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
00887                               bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
00888   {
00889     moveit_msgs::GetCartesianPath::Request req;
00890     moveit_msgs::GetCartesianPath::Response res;
00891 
00892     if (considered_start_state_)
00893       robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
00894     else
00895       req.start_state.is_diff = true;
00896 
00897     req.group_name = opt_.group_name_;
00898     req.header.frame_id = getPoseReferenceFrame();
00899     req.header.stamp = ros::Time::now();
00900     req.waypoints = waypoints;
00901     req.max_step = step;
00902     req.jump_threshold = jump_threshold;
00903     req.path_constraints = path_constraints;
00904     req.avoid_collisions = avoid_collisions;
00905     req.link_name = getEndEffectorLink();
00906 
00907     if (cartesian_path_service_.call(req, res))
00908     {
00909       error_code = res.error_code;
00910       if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00911       {
00912         msg = res.solution;
00913         return res.fraction;
00914       }
00915       else
00916         return -1.0;
00917     }
00918     else
00919     {
00920       error_code.val = error_code.FAILURE;
00921       return -1.0;
00922     }
00923   }
00924 
00925   void stop()
00926   {
00927     if (trajectory_event_publisher_)
00928     {
00929       std_msgs::String event;
00930       event.data = "stop";
00931       trajectory_event_publisher_.publish(event);
00932     }
00933   }
00934 
00935   bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
00936   {
00937     std::string l = link.empty() ? getEndEffectorLink() : link;
00938     if (l.empty())
00939     {
00940       const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
00941       if (!links.empty())
00942         l = links[0];
00943     }
00944     if (l.empty())
00945     {
00946       ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
00947       return false;
00948     }
00949     moveit_msgs::AttachedCollisionObject aco;
00950     aco.object.id = object;
00951     aco.link_name.swap(l);
00952     if (touch_links.empty())
00953       aco.touch_links.push_back(aco.link_name);
00954     else
00955       aco.touch_links = touch_links;
00956     aco.object.operation = moveit_msgs::CollisionObject::ADD;
00957     attached_object_publisher_.publish(aco);
00958     return true;
00959   }
00960 
00961   bool detachObject(const std::string& name)
00962   {
00963     moveit_msgs::AttachedCollisionObject aco;
00964     // if name is a link
00965     if (!name.empty() && joint_model_group_->hasLinkModel(name))
00966       aco.link_name = name;
00967     else
00968       aco.object.id = name;
00969     aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
00970     if (aco.link_name.empty() && aco.object.id.empty())
00971     {
00972       // we only want to detach objects for this group
00973       const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
00974       for (std::size_t i = 0; i < lnames.size(); ++i)
00975       {
00976         aco.link_name = lnames[i];
00977         attached_object_publisher_.publish(aco);
00978       }
00979     }
00980     else
00981       attached_object_publisher_.publish(aco);
00982     return true;
00983   }
00984 
00985   double getGoalPositionTolerance() const
00986   {
00987     return goal_position_tolerance_;
00988   }
00989 
00990   double getGoalOrientationTolerance() const
00991   {
00992     return goal_orientation_tolerance_;
00993   }
00994 
00995   double getGoalJointTolerance() const
00996   {
00997     return goal_joint_tolerance_;
00998   }
00999 
01000   void setGoalJointTolerance(double tolerance)
01001   {
01002     goal_joint_tolerance_ = tolerance;
01003   }
01004 
01005   void setGoalPositionTolerance(double tolerance)
01006   {
01007     goal_position_tolerance_ = tolerance;
01008   }
01009 
01010   void setGoalOrientationTolerance(double tolerance)
01011   {
01012     goal_orientation_tolerance_ = tolerance;
01013   }
01014 
01015   void setPlanningTime(double seconds)
01016   {
01017     if (seconds > 0.0)
01018       planning_time_ = seconds;
01019   }
01020 
01021   double getPlanningTime() const
01022   {
01023     return planning_time_;
01024   }
01025 
01026   void allowLooking(bool flag)
01027   {
01028     can_look_ = flag;
01029     ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
01030   }
01031 
01032   void allowReplanning(bool flag)
01033   {
01034     can_replan_ = flag;
01035     ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
01036   }
01037 
01038   void setReplanningDelay(double delay)
01039   {
01040     if (delay >= 0.0)
01041       replan_delay_ = delay;
01042   }
01043 
01044   double getReplanningDelay() const
01045   {
01046     return replan_delay_;
01047   }
01048 
01049   void constructGoal(moveit_msgs::MoveGroupGoal& goal_out)
01050   {
01051     moveit_msgs::MoveGroupGoal goal;
01052     goal.request.group_name = opt_.group_name_;
01053     goal.request.num_planning_attempts = num_planning_attempts_;
01054     goal.request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
01055     goal.request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
01056     goal.request.allowed_planning_time = planning_time_;
01057     goal.request.planner_id = planner_id_;
01058     goal.request.workspace_parameters = workspace_parameters_;
01059 
01060     if (considered_start_state_)
01061       robot_state::robotStateToRobotStateMsg(*considered_start_state_, goal.request.start_state);
01062     else
01063       goal.request.start_state.is_diff = true;
01064 
01065     if (active_target_ == JOINT)
01066     {
01067       goal.request.goal_constraints.resize(1);
01068       goal.request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
01069           getJointStateTarget(), joint_model_group_, goal_joint_tolerance_);
01070     }
01071     else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
01072     {
01073       // find out how many goals are specified
01074       std::size_t goal_count = 0;
01075       for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
01076            it != pose_targets_.end(); ++it)
01077         goal_count = std::max(goal_count, it->second.size());
01078 
01079       // start filling the goals;
01080       // each end effector has a number of possible poses (K) as valid goals
01081       // but there could be multiple end effectors specified, so we want each end effector
01082       // to reach the goal that corresponds to the goals of the other end effectors
01083       goal.request.goal_constraints.resize(goal_count);
01084 
01085       for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
01086            it != pose_targets_.end(); ++it)
01087       {
01088         for (std::size_t i = 0; i < it->second.size(); ++i)
01089         {
01090           moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
01091               it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
01092           if (active_target_ == ORIENTATION)
01093             c.position_constraints.clear();
01094           if (active_target_ == POSITION)
01095             c.orientation_constraints.clear();
01096           goal.request.goal_constraints[i] =
01097               kinematic_constraints::mergeConstraints(goal.request.goal_constraints[i], c);
01098         }
01099       }
01100     }
01101     else
01102       ROS_ERROR_NAMED("move_group_interface", "Unable to construct goal representation");
01103 
01104     if (path_constraints_)
01105       goal.request.path_constraints = *path_constraints_;
01106     goal_out = goal;
01107   }
01108 
01109   void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object)
01110   {
01111     moveit_msgs::PickupGoal goal;
01112     goal.target_name = object;
01113     goal.group_name = opt_.group_name_;
01114     goal.end_effector = getEndEffector();
01115     goal.allowed_planning_time = planning_time_;
01116     goal.support_surface_name = support_surface_;
01117     goal.planner_id = planner_id_;
01118     if (!support_surface_.empty())
01119       goal.allow_gripper_support_collision = true;
01120 
01121     if (path_constraints_)
01122       goal.path_constraints = *path_constraints_;
01123 
01124     goal_out = goal;
01125   }
01126 
01127   void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object)
01128   {
01129     moveit_msgs::PlaceGoal goal;
01130     goal.attached_object_name = object;
01131     goal.group_name = opt_.group_name_;
01132     goal.allowed_planning_time = planning_time_;
01133     goal.support_surface_name = support_surface_;
01134     goal.planner_id = planner_id_;
01135     if (!support_surface_.empty())
01136       goal.allow_gripper_support_collision = true;
01137 
01138     if (path_constraints_)
01139       goal.path_constraints = *path_constraints_;
01140 
01141     goal_out = goal;
01142   }
01143 
01144   void setPathConstraints(const moveit_msgs::Constraints& constraint)
01145   {
01146     path_constraints_.reset(new moveit_msgs::Constraints(constraint));
01147   }
01148 
01149   bool setPathConstraints(const std::string& constraint)
01150   {
01151     if (constraints_storage_)
01152     {
01153       moveit_warehouse::ConstraintsWithMetadata msg_m;
01154       if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
01155       {
01156         path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
01157         return true;
01158       }
01159       else
01160         return false;
01161     }
01162     else
01163       return false;
01164   }
01165 
01166   void clearPathConstraints()
01167   {
01168     path_constraints_.reset();
01169   }
01170 
01171   std::vector<std::string> getKnownConstraints() const
01172   {
01173     while (initializing_constraints_)
01174     {
01175       static ros::WallDuration d(0.01);
01176       d.sleep();
01177     }
01178 
01179     std::vector<std::string> c;
01180     if (constraints_storage_)
01181       constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
01182 
01183     return c;
01184   }
01185 
01186   moveit_msgs::Constraints getPathConstraints() const
01187   {
01188     if (path_constraints_)
01189       return *path_constraints_;
01190     else
01191       return moveit_msgs::Constraints();
01192   }
01193 
01194   void initializeConstraintsStorage(const std::string& host, unsigned int port)
01195   {
01196     initializing_constraints_ = true;
01197     if (constraints_init_thread_)
01198       constraints_init_thread_->join();
01199     constraints_init_thread_.reset(
01200         new boost::thread(boost::bind(&MoveGroupImpl::initializeConstraintsStorageThread, this, host, port)));
01201   }
01202 
01203   void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
01204   {
01205     workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
01206     workspace_parameters_.header.stamp = ros::Time::now();
01207     workspace_parameters_.min_corner.x = minx;
01208     workspace_parameters_.min_corner.y = miny;
01209     workspace_parameters_.min_corner.z = minz;
01210     workspace_parameters_.max_corner.x = maxx;
01211     workspace_parameters_.max_corner.y = maxy;
01212     workspace_parameters_.max_corner.z = maxz;
01213   }
01214 
01215 private:
01216   void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
01217   {
01218     try
01219     {
01220       constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(host, port));
01221       ROS_DEBUG_NAMED("move_group_interface", "Connected to constraints database");
01222     }
01223     catch (std::runtime_error& ex)
01224     {
01225       ROS_DEBUG_NAMED("move_group_interface", "%s", ex.what());
01226     }
01227     initializing_constraints_ = false;
01228   }
01229 
01230   Options opt_;
01231   ros::NodeHandle node_handle_;
01232   boost::shared_ptr<tf::Transformer> tf_;
01233   robot_model::RobotModelConstPtr robot_model_;
01234   planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
01235   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
01236   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
01237   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
01238   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
01239 
01240   // general planning params
01241   robot_state::RobotStatePtr considered_start_state_;
01242   moveit_msgs::WorkspaceParameters workspace_parameters_;
01243   double planning_time_;
01244   std::string planner_id_;
01245   unsigned int num_planning_attempts_;
01246   double max_velocity_scaling_factor_;
01247   double max_acceleration_scaling_factor_;
01248   double goal_joint_tolerance_;
01249   double goal_position_tolerance_;
01250   double goal_orientation_tolerance_;
01251   bool can_look_;
01252   bool can_replan_;
01253   double replan_delay_;
01254 
01255   // joint state goal
01256   robot_state::RobotStatePtr joint_state_target_;
01257   const robot_model::JointModelGroup* joint_model_group_;
01258 
01259   // pose goal;
01260   // for each link we have a set of possible goal locations;
01261   std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
01262 
01263   // common properties for goals
01264   ActiveTargetType active_target_;
01265   boost::scoped_ptr<moveit_msgs::Constraints> path_constraints_;
01266   std::string end_effector_link_;
01267   std::string pose_reference_frame_;
01268   std::string support_surface_;
01269 
01270   // ROS communication
01271   ros::Publisher trajectory_event_publisher_;
01272   ros::Publisher attached_object_publisher_;
01273   ros::ServiceClient execute_service_;
01274   ros::ServiceClient query_service_;
01275   ros::ServiceClient cartesian_path_service_;
01276   ros::ServiceClient plan_grasps_service_;
01277   boost::scoped_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
01278   boost::scoped_ptr<boost::thread> constraints_init_thread_;
01279   bool initializing_constraints_;
01280 };
01281 }
01282 }
01283 
01284 moveit::planning_interface::MoveGroup::MoveGroup(const std::string& group_name,
01285                                                  const boost::shared_ptr<tf::Transformer>& tf,
01286                                                  const ros::WallDuration& wait_for_servers)
01287 {
01288   if (!ros::ok())
01289     throw std::runtime_error("ROS does not seem to be running");
01290   impl_ = new MoveGroupImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_servers);
01291 }
01292 
01293 moveit::planning_interface::MoveGroup::MoveGroup(const std::string& group, const boost::shared_ptr<tf::Transformer>& tf,
01294                                                  const ros::Duration& wait_for_servers)
01295   : MoveGroup(group, tf, ros::WallDuration(wait_for_servers.toSec()))
01296 {
01297 }
01298 
01299 moveit::planning_interface::MoveGroup::MoveGroup(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf,
01300                                                  const ros::WallDuration& wait_for_servers)
01301 {
01302   impl_ = new MoveGroupImpl(opt, tf ? tf : getSharedTF(), wait_for_servers);
01303 }
01304 
01305 moveit::planning_interface::MoveGroup::MoveGroup(const moveit::planning_interface::MoveGroup::Options& opt,
01306                                                  const boost::shared_ptr<tf::Transformer>& tf,
01307                                                  const ros::Duration& wait_for_servers)
01308   : MoveGroup(opt, tf, ros::WallDuration(wait_for_servers.toSec()))
01309 {
01310 }
01311 
01312 moveit::planning_interface::MoveGroup::~MoveGroup()
01313 {
01314   delete impl_;
01315 }
01316 
01317 const std::string& moveit::planning_interface::MoveGroup::getName() const
01318 {
01319   return impl_->getOptions().group_name_;
01320 }
01321 
01322 const std::vector<std::string> moveit::planning_interface::MoveGroup::getNamedTargets()
01323 {
01324   const robot_model::RobotModelConstPtr& robot = getRobotModel();
01325   const std::string& group = getName();
01326   const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
01327 
01328   if (joint_group)
01329   {
01330     return joint_group->getDefaultStateNames();
01331   }
01332 
01333   std::vector<std::string> empty;
01334   return empty;
01335 }
01336 
01337 robot_model::RobotModelConstPtr moveit::planning_interface::MoveGroup::getRobotModel() const
01338 {
01339   return impl_->getRobotModel();
01340 }
01341 
01342 const ros::NodeHandle& moveit::planning_interface::MoveGroup::getNodeHandle() const
01343 {
01344   return impl_->getOptions().node_handle_;
01345 }
01346 
01347 bool moveit::planning_interface::MoveGroup::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
01348 {
01349   return impl_->getInterfaceDescription(desc);
01350 }
01351 
01352 std::string moveit::planning_interface::MoveGroup::getDefaultPlannerId(const std::string& group) const
01353 {
01354   return impl_->getDefaultPlannerId(group);
01355 }
01356 
01357 void moveit::planning_interface::MoveGroup::setPlannerId(const std::string& planner_id)
01358 {
01359   impl_->setPlannerId(planner_id);
01360 }
01361 
01362 void moveit::planning_interface::MoveGroup::setNumPlanningAttempts(unsigned int num_planning_attempts)
01363 {
01364   impl_->setNumPlanningAttempts(num_planning_attempts);
01365 }
01366 
01367 void moveit::planning_interface::MoveGroup::setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
01368 {
01369   impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
01370 }
01371 
01372 void moveit::planning_interface::MoveGroup::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
01373 {
01374   impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
01375 }
01376 
01377 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncMove()
01378 {
01379   return impl_->move(false);
01380 }
01381 
01382 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::move()
01383 {
01384   return impl_->move(true);
01385 }
01386 
01387 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncExecute(const Plan& plan)
01388 {
01389   return impl_->execute(plan, false);
01390 }
01391 
01392 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::execute(const Plan& plan)
01393 {
01394   return impl_->execute(plan, true);
01395 }
01396 
01397 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::plan(Plan& plan)
01398 {
01399   return impl_->plan(plan);
01400 }
01401 
01402 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string& object)
01403 {
01404   return impl_->pick(object, std::vector<moveit_msgs::Grasp>());
01405 }
01406 
01407 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string& object,
01408                                                                                         const moveit_msgs::Grasp& grasp)
01409 {
01410   return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp));
01411 }
01412 
01413 moveit::planning_interface::MoveItErrorCode
01414 moveit::planning_interface::MoveGroup::pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
01415 {
01416   return impl_->pick(object, grasps);
01417 }
01418 
01419 moveit::planning_interface::MoveItErrorCode
01420 moveit::planning_interface::MoveGroup::planGraspsAndPick(const std::string& object)
01421 {
01422   return impl_->planGraspsAndPick(object);
01423 }
01424 
01425 moveit::planning_interface::MoveItErrorCode
01426 moveit::planning_interface::MoveGroup::planGraspsAndPick(const moveit_msgs::CollisionObject& object)
01427 {
01428   return impl_->planGraspsAndPick(object);
01429 }
01430 
01431 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string& object)
01432 {
01433   return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>());
01434 }
01435 
01436 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(
01437     const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
01438 {
01439   return impl_->place(object, locations);
01440 }
01441 
01442 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(
01443     const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
01444 {
01445   return impl_->place(object, poses);
01446 }
01447 
01448 moveit::planning_interface::MoveItErrorCode
01449 moveit::planning_interface::MoveGroup::place(const std::string& object, const geometry_msgs::PoseStamped& pose)
01450 {
01451   return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose));
01452 }
01453 
01454 double moveit::planning_interface::MoveGroup::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints,
01455                                                                    double eef_step, double jump_threshold,
01456                                                                    moveit_msgs::RobotTrajectory& trajectory,
01457                                                                    bool avoid_collisions,
01458                                                                    moveit_msgs::MoveItErrorCodes* error_code)
01459 {
01460   moveit_msgs::Constraints path_constraints_tmp;
01461   return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
01462                               error_code);
01463 }
01464 
01465 double moveit::planning_interface::MoveGroup::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints,
01466                                                                    double eef_step, double jump_threshold,
01467                                                                    moveit_msgs::RobotTrajectory& trajectory,
01468                                                                    const moveit_msgs::Constraints& path_constraints,
01469                                                                    bool avoid_collisions,
01470                                                                    moveit_msgs::MoveItErrorCodes* error_code)
01471 {
01472   if (error_code)
01473   {
01474     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
01475                                        avoid_collisions, *error_code);
01476   }
01477   else
01478   {
01479     moveit_msgs::MoveItErrorCodes error_code_tmp;
01480     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
01481                                        avoid_collisions, error_code_tmp);
01482   }
01483 }
01484 
01485 void moveit::planning_interface::MoveGroup::stop()
01486 {
01487   impl_->stop();
01488 }
01489 
01490 void moveit::planning_interface::MoveGroup::setStartState(const moveit_msgs::RobotState& start_state)
01491 {
01492   robot_state::RobotStatePtr rs;
01493   impl_->getCurrentState(rs);
01494   robot_state::robotStateMsgToRobotState(start_state, *rs);
01495   setStartState(*rs);
01496 }
01497 
01498 void moveit::planning_interface::MoveGroup::setStartState(const robot_state::RobotState& start_state)
01499 {
01500   impl_->setStartState(start_state);
01501 }
01502 
01503 void moveit::planning_interface::MoveGroup::setStartStateToCurrentState()
01504 {
01505   impl_->setStartStateToCurrentState();
01506 }
01507 
01508 void moveit::planning_interface::MoveGroup::setRandomTarget()
01509 {
01510   impl_->getJointStateTarget().setToRandomPositions();
01511   impl_->setTargetType(JOINT);
01512 }
01513 
01514 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getJointNames()
01515 {
01516   return impl_->getJointModelGroup()->getVariableNames();
01517 }
01518 
01519 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getLinkNames()
01520 {
01521   return impl_->getJointModelGroup()->getLinkModelNames();
01522 }
01523 
01524 std::map<std::string, double> moveit::planning_interface::MoveGroup::getNamedTargetValues(const std::string& name)
01525 {
01526   std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
01527   std::map<std::string, double> positions;
01528 
01529   if (it != remembered_joint_values_.end())
01530   {
01531     std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
01532     for (size_t x = 0; x < names.size(); ++x)
01533     {
01534       positions[names[x]] = it->second[x];
01535     }
01536   }
01537   else
01538   {
01539     impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
01540   }
01541   return positions;
01542 }
01543 
01544 bool moveit::planning_interface::MoveGroup::setNamedTarget(const std::string& name)
01545 {
01546   std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
01547   if (it != remembered_joint_values_.end())
01548   {
01549     return setJointValueTarget(it->second);
01550   }
01551   else
01552   {
01553     if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
01554     {
01555       impl_->setTargetType(JOINT);
01556       return true;
01557     }
01558     ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
01559     return false;
01560   }
01561 }
01562 
01563 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::vector<double>& joint_values)
01564 {
01565   if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
01566     return false;
01567   impl_->setTargetType(JOINT);
01568   impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
01569   return impl_->getJointStateTarget().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance());
01570 }
01571 
01572 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::map<std::string, double>& joint_values)
01573 {
01574   impl_->setTargetType(JOINT);
01575   impl_->getJointStateTarget().setVariablePositions(joint_values);
01576   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01577 }
01578 
01579 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const robot_state::RobotState& rstate)
01580 {
01581   impl_->setTargetType(JOINT);
01582   impl_->getJointStateTarget() = rstate;
01583   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01584 }
01585 
01586 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string& joint_name, double value)
01587 {
01588   std::vector<double> values(1, value);
01589   return setJointValueTarget(joint_name, values);
01590 }
01591 
01592 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string& joint_name,
01593                                                                 const std::vector<double>& values)
01594 {
01595   impl_->setTargetType(JOINT);
01596   const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
01597   if (jm && jm->getVariableCount() == values.size())
01598   {
01599     impl_->getJointStateTarget().setJointPositions(jm, values);
01600     return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
01601   }
01602   return false;
01603 }
01604 
01605 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const sensor_msgs::JointState& state)
01606 {
01607   impl_->setTargetType(JOINT);
01608   impl_->getJointStateTarget().setVariableValues(state);
01609   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01610 }
01611 
01612 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::Pose& eef_pose,
01613                                                                 const std::string& end_effector_link)
01614 {
01615   return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
01616 }
01617 
01618 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
01619                                                                 const std::string& end_effector_link)
01620 {
01621   return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
01622 }
01623 
01624 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const Eigen::Affine3d& eef_pose,
01625                                                                 const std::string& end_effector_link)
01626 {
01627   geometry_msgs::Pose msg;
01628   tf::poseEigenToMsg(eef_pose, msg);
01629   return setJointValueTarget(msg, end_effector_link);
01630 }
01631 
01632 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose,
01633                                                                            const std::string& end_effector_link)
01634 {
01635   return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
01636 }
01637 
01638 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
01639                                                                            const std::string& end_effector_link)
01640 {
01641   return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
01642 }
01643 
01644 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const Eigen::Affine3d& eef_pose,
01645                                                                            const std::string& end_effector_link)
01646 {
01647   geometry_msgs::Pose msg;
01648   tf::poseEigenToMsg(eef_pose, msg);
01649   return setApproximateJointValueTarget(msg, end_effector_link);
01650 }
01651 
01652 const robot_state::RobotState& moveit::planning_interface::MoveGroup::getJointValueTarget() const
01653 {
01654   return impl_->getJointStateTarget();
01655 }
01656 
01657 const std::string& moveit::planning_interface::MoveGroup::getEndEffectorLink() const
01658 {
01659   return impl_->getEndEffectorLink();
01660 }
01661 
01662 const std::string& moveit::planning_interface::MoveGroup::getEndEffector() const
01663 {
01664   return impl_->getEndEffector();
01665 }
01666 
01667 bool moveit::planning_interface::MoveGroup::setEndEffectorLink(const std::string& link_name)
01668 {
01669   if (impl_->getEndEffectorLink().empty() || link_name.empty())
01670     return false;
01671   impl_->setEndEffectorLink(link_name);
01672   impl_->setTargetType(POSE);
01673   return true;
01674 }
01675 
01676 bool moveit::planning_interface::MoveGroup::setEndEffector(const std::string& eef_name)
01677 {
01678   const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
01679   if (jmg)
01680     return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
01681   return false;
01682 }
01683 
01684 void moveit::planning_interface::MoveGroup::clearPoseTarget(const std::string& end_effector_link)
01685 {
01686   impl_->clearPoseTarget(end_effector_link);
01687 }
01688 
01689 void moveit::planning_interface::MoveGroup::clearPoseTargets()
01690 {
01691   impl_->clearPoseTargets();
01692 }
01693 
01694 bool moveit::planning_interface::MoveGroup::setPoseTarget(const Eigen::Affine3d& pose,
01695                                                           const std::string& end_effector_link)
01696 {
01697   std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01698   tf::poseEigenToMsg(pose, pose_msg[0].pose);
01699   pose_msg[0].header.frame_id = getPoseReferenceFrame();
01700   pose_msg[0].header.stamp = ros::Time::now();
01701   return setPoseTargets(pose_msg, end_effector_link);
01702 }
01703 
01704 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::Pose& target,
01705                                                           const std::string& end_effector_link)
01706 {
01707   std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01708   pose_msg[0].pose = target;
01709   pose_msg[0].header.frame_id = getPoseReferenceFrame();
01710   pose_msg[0].header.stamp = ros::Time::now();
01711   return setPoseTargets(pose_msg, end_effector_link);
01712 }
01713 
01714 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::PoseStamped& target,
01715                                                           const std::string& end_effector_link)
01716 {
01717   std::vector<geometry_msgs::PoseStamped> targets(1, target);
01718   return setPoseTargets(targets, end_effector_link);
01719 }
01720 
01721 bool moveit::planning_interface::MoveGroup::setPoseTargets(const EigenSTL::vector_Affine3d& target,
01722                                                            const std::string& end_effector_link)
01723 {
01724   std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
01725   ros::Time tm = ros::Time::now();
01726   const std::string& frame_id = getPoseReferenceFrame();
01727   for (std::size_t i = 0; i < target.size(); ++i)
01728   {
01729     tf::poseEigenToMsg(target[i], pose_out[i].pose);
01730     pose_out[i].header.stamp = tm;
01731     pose_out[i].header.frame_id = frame_id;
01732   }
01733   return setPoseTargets(pose_out, end_effector_link);
01734 }
01735 
01736 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
01737                                                            const std::string& end_effector_link)
01738 {
01739   std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
01740   ros::Time tm = ros::Time::now();
01741   const std::string& frame_id = getPoseReferenceFrame();
01742   for (std::size_t i = 0; i < target.size(); ++i)
01743   {
01744     target_stamped[i].pose = target[i];
01745     target_stamped[i].header.stamp = tm;
01746     target_stamped[i].header.frame_id = frame_id;
01747   }
01748   return setPoseTargets(target_stamped, end_effector_link);
01749 }
01750 
01751 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target,
01752                                                            const std::string& end_effector_link)
01753 {
01754   if (target.empty())
01755   {
01756     ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
01757     return false;
01758   }
01759   else
01760   {
01761     impl_->setTargetType(POSE);
01762     return impl_->setPoseTargets(target, end_effector_link);
01763   }
01764 }
01765 
01766 const geometry_msgs::PoseStamped&
01767 moveit::planning_interface::MoveGroup::getPoseTarget(const std::string& end_effector_link) const
01768 {
01769   return impl_->getPoseTarget(end_effector_link);
01770 }
01771 
01772 const std::vector<geometry_msgs::PoseStamped>&
01773 moveit::planning_interface::MoveGroup::getPoseTargets(const std::string& end_effector_link) const
01774 {
01775   return impl_->getPoseTargets(end_effector_link);
01776 }
01777 
01778 namespace
01779 {
01780 inline void transformPose(const tf::Transformer& tf, const std::string& desired_frame,
01781                           geometry_msgs::PoseStamped& target)
01782 {
01783   if (desired_frame != target.header.frame_id)
01784   {
01785     tf::Pose pose;
01786     tf::poseMsgToTF(target.pose, pose);
01787     tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
01788     tf::Stamped<tf::Pose> stamped_target_out;
01789     tf.transformPose(desired_frame, stamped_target, stamped_target_out);
01790     target.header.frame_id = stamped_target_out.frame_id_;
01791     //    target.header.stamp = stamped_target_out.stamp_; // we leave the stamp to ros::Time(0) on purpose
01792     tf::poseTFToMsg(stamped_target_out, target.pose);
01793   }
01794 }
01795 }
01796 
01797 bool moveit::planning_interface::MoveGroup::setPositionTarget(double x, double y, double z,
01798                                                               const std::string& end_effector_link)
01799 {
01800   geometry_msgs::PoseStamped target;
01801   if (impl_->hasPoseTarget(end_effector_link))
01802   {
01803     target = getPoseTarget(end_effector_link);
01804     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01805   }
01806   else
01807   {
01808     target.pose.orientation.x = 0.0;
01809     target.pose.orientation.y = 0.0;
01810     target.pose.orientation.z = 0.0;
01811     target.pose.orientation.w = 1.0;
01812     target.header.frame_id = impl_->getPoseReferenceFrame();
01813   }
01814 
01815   target.pose.position.x = x;
01816   target.pose.position.y = y;
01817   target.pose.position.z = z;
01818   bool result = setPoseTarget(target, end_effector_link);
01819   impl_->setTargetType(POSITION);
01820   return result;
01821 }
01822 
01823 bool moveit::planning_interface::MoveGroup::setRPYTarget(double r, double p, double y,
01824                                                          const std::string& end_effector_link)
01825 {
01826   geometry_msgs::PoseStamped target;
01827   if (impl_->hasPoseTarget(end_effector_link))
01828   {
01829     target = getPoseTarget(end_effector_link);
01830     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01831   }
01832   else
01833   {
01834     target.pose.position.x = 0.0;
01835     target.pose.position.y = 0.0;
01836     target.pose.position.z = 0.0;
01837     target.header.frame_id = impl_->getPoseReferenceFrame();
01838   }
01839 
01840   tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
01841   bool result = setPoseTarget(target, end_effector_link);
01842   impl_->setTargetType(ORIENTATION);
01843   return result;
01844 }
01845 
01846 bool moveit::planning_interface::MoveGroup::setOrientationTarget(double x, double y, double z, double w,
01847                                                                  const std::string& end_effector_link)
01848 {
01849   geometry_msgs::PoseStamped target;
01850   if (impl_->hasPoseTarget(end_effector_link))
01851   {
01852     target = getPoseTarget(end_effector_link);
01853     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01854   }
01855   else
01856   {
01857     target.pose.position.x = 0.0;
01858     target.pose.position.y = 0.0;
01859     target.pose.position.z = 0.0;
01860     target.header.frame_id = impl_->getPoseReferenceFrame();
01861   }
01862 
01863   target.pose.orientation.x = x;
01864   target.pose.orientation.y = y;
01865   target.pose.orientation.z = z;
01866   target.pose.orientation.w = w;
01867   bool result = setPoseTarget(target, end_effector_link);
01868   impl_->setTargetType(ORIENTATION);
01869   return result;
01870 }
01871 
01872 void moveit::planning_interface::MoveGroup::setPoseReferenceFrame(const std::string& pose_reference_frame)
01873 {
01874   impl_->setPoseReferenceFrame(pose_reference_frame);
01875 }
01876 
01877 const std::string& moveit::planning_interface::MoveGroup::getPoseReferenceFrame() const
01878 {
01879   return impl_->getPoseReferenceFrame();
01880 }
01881 
01882 double moveit::planning_interface::MoveGroup::getGoalJointTolerance() const
01883 {
01884   return impl_->getGoalJointTolerance();
01885 }
01886 
01887 double moveit::planning_interface::MoveGroup::getGoalPositionTolerance() const
01888 {
01889   return impl_->getGoalPositionTolerance();
01890 }
01891 
01892 double moveit::planning_interface::MoveGroup::getGoalOrientationTolerance() const
01893 {
01894   return impl_->getGoalOrientationTolerance();
01895 }
01896 
01897 void moveit::planning_interface::MoveGroup::setGoalTolerance(double tolerance)
01898 {
01899   setGoalJointTolerance(tolerance);
01900   setGoalPositionTolerance(tolerance);
01901   setGoalOrientationTolerance(tolerance);
01902 }
01903 
01904 void moveit::planning_interface::MoveGroup::setGoalJointTolerance(double tolerance)
01905 {
01906   impl_->setGoalJointTolerance(tolerance);
01907 }
01908 
01909 void moveit::planning_interface::MoveGroup::setGoalPositionTolerance(double tolerance)
01910 {
01911   impl_->setGoalPositionTolerance(tolerance);
01912 }
01913 
01914 void moveit::planning_interface::MoveGroup::setGoalOrientationTolerance(double tolerance)
01915 {
01916   impl_->setGoalOrientationTolerance(tolerance);
01917 }
01918 
01919 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string& name)
01920 {
01921   rememberJointValues(name, getCurrentJointValues());
01922 }
01923 
01924 bool moveit::planning_interface::MoveGroup::startStateMonitor(double wait)
01925 {
01926   return impl_->startStateMonitor(wait);
01927 }
01928 
01929 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentJointValues()
01930 {
01931   robot_state::RobotStatePtr current_state;
01932   std::vector<double> values;
01933   if (impl_->getCurrentState(current_state))
01934     current_state->copyJointGroupPositions(getName(), values);
01935   return values;
01936 }
01937 
01938 std::vector<double> moveit::planning_interface::MoveGroup::getRandomJointValues()
01939 {
01940   std::vector<double> r;
01941   impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
01942   return r;
01943 }
01944 
01945 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getRandomPose(const std::string& end_effector_link)
01946 {
01947   const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01948   Eigen::Affine3d pose;
01949   pose.setIdentity();
01950   if (eef.empty())
01951     ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
01952   else
01953   {
01954     robot_state::RobotStatePtr current_state;
01955     if (impl_->getCurrentState(current_state))
01956     {
01957       current_state->setToRandomPositions(impl_->getJointModelGroup());
01958       const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
01959       if (lm)
01960         pose = current_state->getGlobalLinkTransform(lm);
01961     }
01962   }
01963   geometry_msgs::PoseStamped pose_msg;
01964   pose_msg.header.stamp = ros::Time::now();
01965   pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01966   tf::poseEigenToMsg(pose, pose_msg.pose);
01967   return pose_msg;
01968 }
01969 
01970 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getCurrentPose(const std::string& end_effector_link)
01971 {
01972   const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01973   Eigen::Affine3d pose;
01974   pose.setIdentity();
01975   if (eef.empty())
01976     ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
01977   else
01978   {
01979     robot_state::RobotStatePtr current_state;
01980     if (impl_->getCurrentState(current_state))
01981     {
01982       const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
01983       if (lm)
01984         pose = current_state->getGlobalLinkTransform(lm);
01985     }
01986   }
01987   geometry_msgs::PoseStamped pose_msg;
01988   pose_msg.header.stamp = ros::Time::now();
01989   pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01990   tf::poseEigenToMsg(pose, pose_msg.pose);
01991   return pose_msg;
01992 }
01993 
01994 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentRPY(const std::string& end_effector_link)
01995 {
01996   std::vector<double> result;
01997   const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01998   if (eef.empty())
01999     ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
02000   else
02001   {
02002     robot_state::RobotStatePtr current_state;
02003     if (impl_->getCurrentState(current_state))
02004     {
02005       const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
02006       if (lm)
02007       {
02008         result.resize(3);
02009         tf::Matrix3x3 ptf;
02010         tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
02011         tfScalar pitch, roll, yaw;
02012         ptf.getRPY(roll, pitch, yaw);
02013         result[0] = roll;
02014         result[1] = pitch;
02015         result[2] = yaw;
02016       }
02017     }
02018   }
02019   return result;
02020 }
02021 
02022 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getActiveJoints() const
02023 {
02024   return impl_->getJointModelGroup()->getActiveJointModelNames();
02025 }
02026 
02027 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getJoints() const
02028 {
02029   return impl_->getJointModelGroup()->getJointModelNames();
02030 }
02031 
02032 unsigned int moveit::planning_interface::MoveGroup::getVariableCount() const
02033 {
02034   return impl_->getJointModelGroup()->getVariableCount();
02035 }
02036 
02037 robot_state::RobotStatePtr moveit::planning_interface::MoveGroup::getCurrentState()
02038 {
02039   robot_state::RobotStatePtr current_state;
02040   impl_->getCurrentState(current_state);
02041   return current_state;
02042 }
02043 
02044 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string& name,
02045                                                                 const std::vector<double>& values)
02046 {
02047   remembered_joint_values_[name] = values;
02048 }
02049 
02050 void moveit::planning_interface::MoveGroup::forgetJointValues(const std::string& name)
02051 {
02052   remembered_joint_values_.erase(name);
02053 }
02054 
02055 void moveit::planning_interface::MoveGroup::allowLooking(bool flag)
02056 {
02057   impl_->allowLooking(flag);
02058 }
02059 
02060 void moveit::planning_interface::MoveGroup::allowReplanning(bool flag)
02061 {
02062   impl_->allowReplanning(flag);
02063 }
02064 
02065 std::vector<std::string> moveit::planning_interface::MoveGroup::getKnownConstraints() const
02066 {
02067   return impl_->getKnownConstraints();
02068 }
02069 
02070 moveit_msgs::Constraints moveit::planning_interface::MoveGroup::getPathConstraints() const
02071 {
02072   return impl_->getPathConstraints();
02073 }
02074 
02075 bool moveit::planning_interface::MoveGroup::setPathConstraints(const std::string& constraint)
02076 {
02077   return impl_->setPathConstraints(constraint);
02078 }
02079 
02080 void moveit::planning_interface::MoveGroup::setPathConstraints(const moveit_msgs::Constraints& constraint)
02081 {
02082   impl_->setPathConstraints(constraint);
02083 }
02084 
02085 void moveit::planning_interface::MoveGroup::clearPathConstraints()
02086 {
02087   impl_->clearPathConstraints();
02088 }
02089 
02090 void moveit::planning_interface::MoveGroup::setConstraintsDatabase(const std::string& host, unsigned int port)
02091 {
02092   impl_->initializeConstraintsStorage(host, port);
02093 }
02094 
02095 void moveit::planning_interface::MoveGroup::setWorkspace(double minx, double miny, double minz, double maxx,
02096                                                          double maxy, double maxz)
02097 {
02098   impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
02099 }
02100 
02101 void moveit::planning_interface::MoveGroup::setPlanningTime(double seconds)
02102 {
02103   impl_->setPlanningTime(seconds);
02104 }
02105 
02106 double moveit::planning_interface::MoveGroup::getPlanningTime() const
02107 {
02108   return impl_->getPlanningTime();
02109 }
02110 
02111 void moveit::planning_interface::MoveGroup::setSupportSurfaceName(const std::string& name)
02112 {
02113   impl_->setSupportSurfaceName(name);
02114 }
02115 
02116 const std::string& moveit::planning_interface::MoveGroup::getPlanningFrame() const
02117 {
02118   return impl_->getRobotModel()->getModelFrame();
02119 }
02120 
02121 bool moveit::planning_interface::MoveGroup::attachObject(const std::string& object, const std::string& link)
02122 {
02123   return attachObject(object, link, std::vector<std::string>());
02124 }
02125 
02126 bool moveit::planning_interface::MoveGroup::attachObject(const std::string& object, const std::string& link,
02127                                                          const std::vector<std::string>& touch_links)
02128 {
02129   return impl_->attachObject(object, link, touch_links);
02130 }
02131 
02132 bool moveit::planning_interface::MoveGroup::detachObject(const std::string& name)
02133 {
02134   return impl_->detachObject(name);
02135 }


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Jan 17 2018 03:32:49