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(ros::Time::now(), wait_seconds))
00592     {
00593       ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state");
00594       return false;
00595     }
00596 
00597     current_state = current_state_monitor_->getCurrentState();
00598     return true;
00599   }
00600 
00602   MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
00603   {
00604     std::vector<moveit_msgs::PlaceLocation> locations;
00605     for (std::size_t i = 0; i < poses.size(); ++i)
00606     {
00607       moveit_msgs::PlaceLocation location;
00608       location.pre_place_approach.direction.vector.z = -1.0;
00609       location.post_place_retreat.direction.vector.x = -1.0;
00610       location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
00611       location.post_place_retreat.direction.header.frame_id = end_effector_link_;
00612 
00613       location.pre_place_approach.min_distance = 0.1;
00614       location.pre_place_approach.desired_distance = 0.2;
00615       location.post_place_retreat.min_distance = 0.0;
00616       location.post_place_retreat.desired_distance = 0.2;
00617       // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
00618 
00619       location.place_pose = poses[i];
00620       locations.push_back(location);
00621     }
00622     ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
00623                     (unsigned int)locations.size());
00624     return place(object, locations);
00625   }
00626 
00627   MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
00628   {
00629     if (!place_action_client_)
00630     {
00631       ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
00632       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00633     }
00634     if (!place_action_client_->isServerConnected())
00635     {
00636       ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
00637       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00638     }
00639     moveit_msgs::PlaceGoal goal;
00640     constructGoal(goal, object);
00641     goal.place_locations = locations;
00642     goal.planning_options.plan_only = false;
00643     goal.planning_options.look_around = can_look_;
00644     goal.planning_options.replan = can_replan_;
00645     goal.planning_options.replan_delay = replan_delay_;
00646     goal.planning_options.planning_scene_diff.is_diff = true;
00647     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00648 
00649     place_action_client_->sendGoal(goal);
00650     ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
00651     if (!place_action_client_->waitForResult())
00652     {
00653       ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
00654     }
00655     if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00656     {
00657       return MoveItErrorCode(place_action_client_->getResult()->error_code);
00658     }
00659     else
00660     {
00661       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
00662                                                              << place_action_client_->getState().getText());
00663       return MoveItErrorCode(place_action_client_->getResult()->error_code);
00664     }
00665   }
00666 
00667   MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
00668   {
00669     if (!pick_action_client_)
00670     {
00671       ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
00672       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00673     }
00674     if (!pick_action_client_->isServerConnected())
00675     {
00676       ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
00677       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00678     }
00679     moveit_msgs::PickupGoal goal;
00680     constructGoal(goal, object);
00681     goal.possible_grasps = grasps;
00682     goal.planning_options.plan_only = false;
00683     goal.planning_options.look_around = can_look_;
00684     goal.planning_options.replan = can_replan_;
00685     goal.planning_options.replan_delay = replan_delay_;
00686     goal.planning_options.planning_scene_diff.is_diff = true;
00687     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00688 
00689     pick_action_client_->sendGoal(goal);
00690     if (!pick_action_client_->waitForResult())
00691     {
00692       ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
00693     }
00694     if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00695     {
00696       return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00697     }
00698     else
00699     {
00700       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
00701                                                              << pick_action_client_->getState().getText());
00702       return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00703     }
00704   }
00705 
00706   MoveItErrorCode planGraspsAndPick(const std::string& object)
00707   {
00708     if (object.empty())
00709     {
00710       return planGraspsAndPick(moveit_msgs::CollisionObject());
00711     }
00712     moveit::planning_interface::PlanningSceneInterface psi;
00713 
00714     std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
00715 
00716     if (objects.size() < 1)
00717     {
00718       ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '"
00719                                                          << object << "', but the object could not be found");
00720       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
00721     }
00722 
00723     return planGraspsAndPick(objects[object]);
00724   }
00725 
00726   MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object)
00727   {
00728     if (!plan_grasps_service_)
00729     {
00730       ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
00731                                                          << GRASP_PLANNING_SERVICE_NAME
00732                                                          << "' is not available."
00733                                                             " This has to be implemented and started separately.");
00734       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00735     }
00736 
00737     moveit_msgs::GraspPlanning::Request request;
00738     moveit_msgs::GraspPlanning::Response response;
00739 
00740     request.group_name = opt_.group_name_;
00741     request.target = object;
00742     request.support_surfaces.push_back(support_surface_);
00743 
00744     ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
00745     if (!plan_grasps_service_.call(request, response) ||
00746         response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
00747     {
00748       ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
00749       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00750     }
00751 
00752     return pick(object.id, response.grasps);
00753   }
00754 
00755   MoveItErrorCode plan(Plan& plan)
00756   {
00757     if (!move_action_client_)
00758     {
00759       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00760     }
00761     if (!move_action_client_->isServerConnected())
00762     {
00763       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00764     }
00765 
00766     moveit_msgs::MoveGroupGoal goal;
00767     constructGoal(goal);
00768     goal.planning_options.plan_only = true;
00769     goal.planning_options.look_around = false;
00770     goal.planning_options.replan = false;
00771     goal.planning_options.planning_scene_diff.is_diff = true;
00772     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00773 
00774     move_action_client_->sendGoal(goal);
00775     if (!move_action_client_->waitForResult())
00776     {
00777       ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
00778     }
00779     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00780     {
00781       plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
00782       plan.start_state_ = move_action_client_->getResult()->trajectory_start;
00783       plan.planning_time_ = move_action_client_->getResult()->planning_time;
00784       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00785     }
00786     else
00787     {
00788       ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
00789                                                              << move_action_client_->getState().getText());
00790       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00791     }
00792   }
00793 
00794   MoveItErrorCode move(bool wait)
00795   {
00796     if (!move_action_client_)
00797     {
00798       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00799     }
00800     if (!move_action_client_->isServerConnected())
00801     {
00802       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00803     }
00804 
00805     moveit_msgs::MoveGroupGoal goal;
00806     constructGoal(goal);
00807     goal.planning_options.plan_only = false;
00808     goal.planning_options.look_around = can_look_;
00809     goal.planning_options.replan = can_replan_;
00810     goal.planning_options.replan_delay = replan_delay_;
00811     goal.planning_options.planning_scene_diff.is_diff = true;
00812     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00813 
00814     move_action_client_->sendGoal(goal);
00815     if (!wait)
00816     {
00817       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
00818     }
00819 
00820     if (!move_action_client_->waitForResult())
00821     {
00822       ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
00823     }
00824 
00825     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00826     {
00827       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00828     }
00829     else
00830     {
00831       ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
00832                                                         << ": " << move_action_client_->getState().getText());
00833       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00834     }
00835   }
00836 
00837   MoveItErrorCode execute(const Plan& plan, bool wait)
00838   {
00839     if (!execute_action_client_)
00840     {
00841       // TODO: Remove this backwards compatibility code in L-turtle
00842       moveit_msgs::ExecuteKnownTrajectory::Request req;
00843       moveit_msgs::ExecuteKnownTrajectory::Response res;
00844       req.trajectory = plan.trajectory_;
00845       req.wait_for_execution = wait;
00846       if (execute_service_.call(req, res))
00847       {
00848         return MoveItErrorCode(res.error_code);
00849       }
00850       else
00851       {
00852         return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00853       }
00854     }
00855 
00856     if (!execute_action_client_->isServerConnected())
00857     {
00858       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00859     }
00860 
00861     moveit_msgs::ExecuteTrajectoryGoal goal;
00862     goal.trajectory = plan.trajectory_;
00863 
00864     execute_action_client_->sendGoal(goal);
00865     if (!wait)
00866     {
00867       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
00868     }
00869 
00870     if (!execute_action_client_->waitForResult())
00871     {
00872       ROS_INFO_STREAM("ExecuteTrajectory action returned early");
00873     }
00874 
00875     if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00876     {
00877       return MoveItErrorCode(execute_action_client_->getResult()->error_code);
00878     }
00879     else
00880     {
00881       ROS_INFO_STREAM(execute_action_client_->getState().toString() << ": "
00882                                                                     << execute_action_client_->getState().getText());
00883       return MoveItErrorCode(execute_action_client_->getResult()->error_code);
00884     }
00885   }
00886 
00887   double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
00888                               moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
00889                               bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
00890   {
00891     moveit_msgs::GetCartesianPath::Request req;
00892     moveit_msgs::GetCartesianPath::Response res;
00893 
00894     if (considered_start_state_)
00895       robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
00896     else
00897       req.start_state.is_diff = true;
00898 
00899     req.group_name = opt_.group_name_;
00900     req.header.frame_id = getPoseReferenceFrame();
00901     req.header.stamp = ros::Time::now();
00902     req.waypoints = waypoints;
00903     req.max_step = step;
00904     req.jump_threshold = jump_threshold;
00905     req.path_constraints = path_constraints;
00906     req.avoid_collisions = avoid_collisions;
00907     req.link_name = getEndEffectorLink();
00908 
00909     if (cartesian_path_service_.call(req, res))
00910     {
00911       error_code = res.error_code;
00912       if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00913       {
00914         msg = res.solution;
00915         return res.fraction;
00916       }
00917       else
00918         return -1.0;
00919     }
00920     else
00921     {
00922       error_code.val = error_code.FAILURE;
00923       return -1.0;
00924     }
00925   }
00926 
00927   void stop()
00928   {
00929     if (trajectory_event_publisher_)
00930     {
00931       std_msgs::String event;
00932       event.data = "stop";
00933       trajectory_event_publisher_.publish(event);
00934     }
00935   }
00936 
00937   bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
00938   {
00939     std::string l = link.empty() ? getEndEffectorLink() : link;
00940     if (l.empty())
00941     {
00942       const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
00943       if (!links.empty())
00944         l = links[0];
00945     }
00946     if (l.empty())
00947     {
00948       ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
00949       return false;
00950     }
00951     moveit_msgs::AttachedCollisionObject aco;
00952     aco.object.id = object;
00953     aco.link_name.swap(l);
00954     if (touch_links.empty())
00955       aco.touch_links.push_back(aco.link_name);
00956     else
00957       aco.touch_links = touch_links;
00958     aco.object.operation = moveit_msgs::CollisionObject::ADD;
00959     attached_object_publisher_.publish(aco);
00960     return true;
00961   }
00962 
00963   bool detachObject(const std::string& name)
00964   {
00965     moveit_msgs::AttachedCollisionObject aco;
00966     // if name is a link
00967     if (!name.empty() && joint_model_group_->hasLinkModel(name))
00968       aco.link_name = name;
00969     else
00970       aco.object.id = name;
00971     aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
00972     if (aco.link_name.empty() && aco.object.id.empty())
00973     {
00974       // we only want to detach objects for this group
00975       const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
00976       for (std::size_t i = 0; i < lnames.size(); ++i)
00977       {
00978         aco.link_name = lnames[i];
00979         attached_object_publisher_.publish(aco);
00980       }
00981     }
00982     else
00983       attached_object_publisher_.publish(aco);
00984     return true;
00985   }
00986 
00987   double getGoalPositionTolerance() const
00988   {
00989     return goal_position_tolerance_;
00990   }
00991 
00992   double getGoalOrientationTolerance() const
00993   {
00994     return goal_orientation_tolerance_;
00995   }
00996 
00997   double getGoalJointTolerance() const
00998   {
00999     return goal_joint_tolerance_;
01000   }
01001 
01002   void setGoalJointTolerance(double tolerance)
01003   {
01004     goal_joint_tolerance_ = tolerance;
01005   }
01006 
01007   void setGoalPositionTolerance(double tolerance)
01008   {
01009     goal_position_tolerance_ = tolerance;
01010   }
01011 
01012   void setGoalOrientationTolerance(double tolerance)
01013   {
01014     goal_orientation_tolerance_ = tolerance;
01015   }
01016 
01017   void setPlanningTime(double seconds)
01018   {
01019     if (seconds > 0.0)
01020       planning_time_ = seconds;
01021   }
01022 
01023   double getPlanningTime() const
01024   {
01025     return planning_time_;
01026   }
01027 
01028   void allowLooking(bool flag)
01029   {
01030     can_look_ = flag;
01031     ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
01032   }
01033 
01034   void allowReplanning(bool flag)
01035   {
01036     can_replan_ = flag;
01037     ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
01038   }
01039 
01040   void setReplanningDelay(double delay)
01041   {
01042     if (delay >= 0.0)
01043       replan_delay_ = delay;
01044   }
01045 
01046   double getReplanningDelay() const
01047   {
01048     return replan_delay_;
01049   }
01050 
01051   void constructGoal(moveit_msgs::MoveGroupGoal& goal_out)
01052   {
01053     moveit_msgs::MoveGroupGoal goal;
01054     goal.request.group_name = opt_.group_name_;
01055     goal.request.num_planning_attempts = num_planning_attempts_;
01056     goal.request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
01057     goal.request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
01058     goal.request.allowed_planning_time = planning_time_;
01059     goal.request.planner_id = planner_id_;
01060     goal.request.workspace_parameters = workspace_parameters_;
01061 
01062     if (considered_start_state_)
01063       robot_state::robotStateToRobotStateMsg(*considered_start_state_, goal.request.start_state);
01064     else
01065       goal.request.start_state.is_diff = true;
01066 
01067     if (active_target_ == JOINT)
01068     {
01069       goal.request.goal_constraints.resize(1);
01070       goal.request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
01071           getJointStateTarget(), joint_model_group_, goal_joint_tolerance_);
01072     }
01073     else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
01074     {
01075       // find out how many goals are specified
01076       std::size_t goal_count = 0;
01077       for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
01078            it != pose_targets_.end(); ++it)
01079         goal_count = std::max(goal_count, it->second.size());
01080 
01081       // start filling the goals;
01082       // each end effector has a number of possible poses (K) as valid goals
01083       // but there could be multiple end effectors specified, so we want each end effector
01084       // to reach the goal that corresponds to the goals of the other end effectors
01085       goal.request.goal_constraints.resize(goal_count);
01086 
01087       for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
01088            it != pose_targets_.end(); ++it)
01089       {
01090         for (std::size_t i = 0; i < it->second.size(); ++i)
01091         {
01092           moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
01093               it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
01094           if (active_target_ == ORIENTATION)
01095             c.position_constraints.clear();
01096           if (active_target_ == POSITION)
01097             c.orientation_constraints.clear();
01098           goal.request.goal_constraints[i] =
01099               kinematic_constraints::mergeConstraints(goal.request.goal_constraints[i], c);
01100         }
01101       }
01102     }
01103     else
01104       ROS_ERROR_NAMED("move_group_interface", "Unable to construct goal representation");
01105 
01106     if (path_constraints_)
01107       goal.request.path_constraints = *path_constraints_;
01108     goal_out = goal;
01109   }
01110 
01111   void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object)
01112   {
01113     moveit_msgs::PickupGoal goal;
01114     goal.target_name = object;
01115     goal.group_name = opt_.group_name_;
01116     goal.end_effector = getEndEffector();
01117     goal.allowed_planning_time = planning_time_;
01118     goal.support_surface_name = support_surface_;
01119     goal.planner_id = planner_id_;
01120     if (!support_surface_.empty())
01121       goal.allow_gripper_support_collision = true;
01122 
01123     if (path_constraints_)
01124       goal.path_constraints = *path_constraints_;
01125 
01126     goal_out = goal;
01127   }
01128 
01129   void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object)
01130   {
01131     moveit_msgs::PlaceGoal goal;
01132     goal.attached_object_name = object;
01133     goal.group_name = opt_.group_name_;
01134     goal.allowed_planning_time = planning_time_;
01135     goal.support_surface_name = support_surface_;
01136     goal.planner_id = planner_id_;
01137     if (!support_surface_.empty())
01138       goal.allow_gripper_support_collision = true;
01139 
01140     if (path_constraints_)
01141       goal.path_constraints = *path_constraints_;
01142 
01143     goal_out = goal;
01144   }
01145 
01146   void setPathConstraints(const moveit_msgs::Constraints& constraint)
01147   {
01148     path_constraints_.reset(new moveit_msgs::Constraints(constraint));
01149   }
01150 
01151   bool setPathConstraints(const std::string& constraint)
01152   {
01153     if (constraints_storage_)
01154     {
01155       moveit_warehouse::ConstraintsWithMetadata msg_m;
01156       if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
01157       {
01158         path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
01159         return true;
01160       }
01161       else
01162         return false;
01163     }
01164     else
01165       return false;
01166   }
01167 
01168   void clearPathConstraints()
01169   {
01170     path_constraints_.reset();
01171   }
01172 
01173   std::vector<std::string> getKnownConstraints() const
01174   {
01175     while (initializing_constraints_)
01176     {
01177       static ros::WallDuration d(0.01);
01178       d.sleep();
01179     }
01180 
01181     std::vector<std::string> c;
01182     if (constraints_storage_)
01183       constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
01184 
01185     return c;
01186   }
01187 
01188   moveit_msgs::Constraints getPathConstraints() const
01189   {
01190     if (path_constraints_)
01191       return *path_constraints_;
01192     else
01193       return moveit_msgs::Constraints();
01194   }
01195 
01196   void initializeConstraintsStorage(const std::string& host, unsigned int port)
01197   {
01198     initializing_constraints_ = true;
01199     if (constraints_init_thread_)
01200       constraints_init_thread_->join();
01201     constraints_init_thread_.reset(
01202         new boost::thread(boost::bind(&MoveGroupImpl::initializeConstraintsStorageThread, this, host, port)));
01203   }
01204 
01205   void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
01206   {
01207     workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
01208     workspace_parameters_.header.stamp = ros::Time::now();
01209     workspace_parameters_.min_corner.x = minx;
01210     workspace_parameters_.min_corner.y = miny;
01211     workspace_parameters_.min_corner.z = minz;
01212     workspace_parameters_.max_corner.x = maxx;
01213     workspace_parameters_.max_corner.y = maxy;
01214     workspace_parameters_.max_corner.z = maxz;
01215   }
01216 
01217 private:
01218   void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
01219   {
01220     try
01221     {
01222       constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(host, port));
01223       ROS_DEBUG_NAMED("move_group_interface", "Connected to constraints database");
01224     }
01225     catch (std::runtime_error& ex)
01226     {
01227       ROS_DEBUG_NAMED("move_group_interface", "%s", ex.what());
01228     }
01229     initializing_constraints_ = false;
01230   }
01231 
01232   Options opt_;
01233   ros::NodeHandle node_handle_;
01234   boost::shared_ptr<tf::Transformer> tf_;
01235   robot_model::RobotModelConstPtr robot_model_;
01236   planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
01237   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
01238   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
01239   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
01240   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
01241 
01242   // general planning params
01243   robot_state::RobotStatePtr considered_start_state_;
01244   moveit_msgs::WorkspaceParameters workspace_parameters_;
01245   double planning_time_;
01246   std::string planner_id_;
01247   unsigned int num_planning_attempts_;
01248   double max_velocity_scaling_factor_;
01249   double max_acceleration_scaling_factor_;
01250   double goal_joint_tolerance_;
01251   double goal_position_tolerance_;
01252   double goal_orientation_tolerance_;
01253   bool can_look_;
01254   bool can_replan_;
01255   double replan_delay_;
01256 
01257   // joint state goal
01258   robot_state::RobotStatePtr joint_state_target_;
01259   const robot_model::JointModelGroup* joint_model_group_;
01260 
01261   // pose goal;
01262   // for each link we have a set of possible goal locations;
01263   std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
01264 
01265   // common properties for goals
01266   ActiveTargetType active_target_;
01267   boost::scoped_ptr<moveit_msgs::Constraints> path_constraints_;
01268   std::string end_effector_link_;
01269   std::string pose_reference_frame_;
01270   std::string support_surface_;
01271 
01272   // ROS communication
01273   ros::Publisher trajectory_event_publisher_;
01274   ros::Publisher attached_object_publisher_;
01275   ros::ServiceClient execute_service_;
01276   ros::ServiceClient query_service_;
01277   ros::ServiceClient cartesian_path_service_;
01278   ros::ServiceClient plan_grasps_service_;
01279   boost::scoped_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
01280   boost::scoped_ptr<boost::thread> constraints_init_thread_;
01281   bool initializing_constraints_;
01282 };
01283 }
01284 }
01285 
01286 moveit::planning_interface::MoveGroup::MoveGroup(const std::string& group_name,
01287                                                  const boost::shared_ptr<tf::Transformer>& tf,
01288                                                  const ros::WallDuration& wait_for_servers)
01289 {
01290   if (!ros::ok())
01291     throw std::runtime_error("ROS does not seem to be running");
01292   impl_ = new MoveGroupImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_servers);
01293 }
01294 
01295 moveit::planning_interface::MoveGroup::MoveGroup(const std::string& group, const boost::shared_ptr<tf::Transformer>& tf,
01296                                                  const ros::Duration& wait_for_servers)
01297   : MoveGroup(group, tf, ros::WallDuration(wait_for_servers.toSec()))
01298 {
01299 }
01300 
01301 moveit::planning_interface::MoveGroup::MoveGroup(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf,
01302                                                  const ros::WallDuration& wait_for_servers)
01303 {
01304   impl_ = new MoveGroupImpl(opt, tf ? tf : getSharedTF(), wait_for_servers);
01305 }
01306 
01307 moveit::planning_interface::MoveGroup::MoveGroup(const moveit::planning_interface::MoveGroup::Options& opt,
01308                                                  const boost::shared_ptr<tf::Transformer>& tf,
01309                                                  const ros::Duration& wait_for_servers)
01310   : MoveGroup(opt, tf, ros::WallDuration(wait_for_servers.toSec()))
01311 {
01312 }
01313 
01314 moveit::planning_interface::MoveGroup::~MoveGroup()
01315 {
01316   delete impl_;
01317 }
01318 
01319 const std::string& moveit::planning_interface::MoveGroup::getName() const
01320 {
01321   return impl_->getOptions().group_name_;
01322 }
01323 
01324 const std::vector<std::string> moveit::planning_interface::MoveGroup::getNamedTargets()
01325 {
01326   const robot_model::RobotModelConstPtr& robot = getRobotModel();
01327   const std::string& group = getName();
01328   const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
01329 
01330   if (joint_group)
01331   {
01332     return joint_group->getDefaultStateNames();
01333   }
01334 
01335   std::vector<std::string> empty;
01336   return empty;
01337 }
01338 
01339 robot_model::RobotModelConstPtr moveit::planning_interface::MoveGroup::getRobotModel() const
01340 {
01341   return impl_->getRobotModel();
01342 }
01343 
01344 const ros::NodeHandle& moveit::planning_interface::MoveGroup::getNodeHandle() const
01345 {
01346   return impl_->getOptions().node_handle_;
01347 }
01348 
01349 bool moveit::planning_interface::MoveGroup::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
01350 {
01351   return impl_->getInterfaceDescription(desc);
01352 }
01353 
01354 std::string moveit::planning_interface::MoveGroup::getDefaultPlannerId(const std::string& group) const
01355 {
01356   return impl_->getDefaultPlannerId(group);
01357 }
01358 
01359 void moveit::planning_interface::MoveGroup::setPlannerId(const std::string& planner_id)
01360 {
01361   impl_->setPlannerId(planner_id);
01362 }
01363 
01364 void moveit::planning_interface::MoveGroup::setNumPlanningAttempts(unsigned int num_planning_attempts)
01365 {
01366   impl_->setNumPlanningAttempts(num_planning_attempts);
01367 }
01368 
01369 void moveit::planning_interface::MoveGroup::setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
01370 {
01371   impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
01372 }
01373 
01374 void moveit::planning_interface::MoveGroup::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
01375 {
01376   impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
01377 }
01378 
01379 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncMove()
01380 {
01381   return impl_->move(false);
01382 }
01383 
01384 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::move()
01385 {
01386   return impl_->move(true);
01387 }
01388 
01389 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncExecute(const Plan& plan)
01390 {
01391   return impl_->execute(plan, false);
01392 }
01393 
01394 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::execute(const Plan& plan)
01395 {
01396   return impl_->execute(plan, true);
01397 }
01398 
01399 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::plan(Plan& plan)
01400 {
01401   return impl_->plan(plan);
01402 }
01403 
01404 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string& object)
01405 {
01406   return impl_->pick(object, std::vector<moveit_msgs::Grasp>());
01407 }
01408 
01409 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string& object,
01410                                                                                         const moveit_msgs::Grasp& grasp)
01411 {
01412   return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp));
01413 }
01414 
01415 moveit::planning_interface::MoveItErrorCode
01416 moveit::planning_interface::MoveGroup::pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
01417 {
01418   return impl_->pick(object, grasps);
01419 }
01420 
01421 moveit::planning_interface::MoveItErrorCode
01422 moveit::planning_interface::MoveGroup::planGraspsAndPick(const std::string& object)
01423 {
01424   return impl_->planGraspsAndPick(object);
01425 }
01426 
01427 moveit::planning_interface::MoveItErrorCode
01428 moveit::planning_interface::MoveGroup::planGraspsAndPick(const moveit_msgs::CollisionObject& object)
01429 {
01430   return impl_->planGraspsAndPick(object);
01431 }
01432 
01433 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string& object)
01434 {
01435   return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>());
01436 }
01437 
01438 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(
01439     const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
01440 {
01441   return impl_->place(object, locations);
01442 }
01443 
01444 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(
01445     const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
01446 {
01447   return impl_->place(object, poses);
01448 }
01449 
01450 moveit::planning_interface::MoveItErrorCode
01451 moveit::planning_interface::MoveGroup::place(const std::string& object, const geometry_msgs::PoseStamped& pose)
01452 {
01453   return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose));
01454 }
01455 
01456 double moveit::planning_interface::MoveGroup::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints,
01457                                                                    double eef_step, double jump_threshold,
01458                                                                    moveit_msgs::RobotTrajectory& trajectory,
01459                                                                    bool avoid_collisions,
01460                                                                    moveit_msgs::MoveItErrorCodes* error_code)
01461 {
01462   moveit_msgs::Constraints path_constraints_tmp;
01463   return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
01464                               error_code);
01465 }
01466 
01467 double moveit::planning_interface::MoveGroup::computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints,
01468                                                                    double eef_step, double jump_threshold,
01469                                                                    moveit_msgs::RobotTrajectory& trajectory,
01470                                                                    const moveit_msgs::Constraints& path_constraints,
01471                                                                    bool avoid_collisions,
01472                                                                    moveit_msgs::MoveItErrorCodes* error_code)
01473 {
01474   if (error_code)
01475   {
01476     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
01477                                        avoid_collisions, *error_code);
01478   }
01479   else
01480   {
01481     moveit_msgs::MoveItErrorCodes error_code_tmp;
01482     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
01483                                        avoid_collisions, error_code_tmp);
01484   }
01485 }
01486 
01487 void moveit::planning_interface::MoveGroup::stop()
01488 {
01489   impl_->stop();
01490 }
01491 
01492 void moveit::planning_interface::MoveGroup::setStartState(const moveit_msgs::RobotState& start_state)
01493 {
01494   robot_state::RobotStatePtr rs;
01495   impl_->getCurrentState(rs);
01496   robot_state::robotStateMsgToRobotState(start_state, *rs);
01497   setStartState(*rs);
01498 }
01499 
01500 void moveit::planning_interface::MoveGroup::setStartState(const robot_state::RobotState& start_state)
01501 {
01502   impl_->setStartState(start_state);
01503 }
01504 
01505 void moveit::planning_interface::MoveGroup::setStartStateToCurrentState()
01506 {
01507   impl_->setStartStateToCurrentState();
01508 }
01509 
01510 void moveit::planning_interface::MoveGroup::setRandomTarget()
01511 {
01512   impl_->getJointStateTarget().setToRandomPositions();
01513   impl_->setTargetType(JOINT);
01514 }
01515 
01516 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getJointNames()
01517 {
01518   return impl_->getJointModelGroup()->getVariableNames();
01519 }
01520 
01521 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getLinkNames()
01522 {
01523   return impl_->getJointModelGroup()->getLinkModelNames();
01524 }
01525 
01526 std::map<std::string, double> moveit::planning_interface::MoveGroup::getNamedTargetValues(const std::string& name)
01527 {
01528   std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
01529   std::map<std::string, double> positions;
01530 
01531   if (it != remembered_joint_values_.end())
01532   {
01533     std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
01534     for (size_t x = 0; x < names.size(); ++x)
01535     {
01536       positions[names[x]] = it->second[x];
01537     }
01538   }
01539   else
01540   {
01541     impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
01542   }
01543   return positions;
01544 }
01545 
01546 bool moveit::planning_interface::MoveGroup::setNamedTarget(const std::string& name)
01547 {
01548   std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
01549   if (it != remembered_joint_values_.end())
01550   {
01551     return setJointValueTarget(it->second);
01552   }
01553   else
01554   {
01555     if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
01556     {
01557       impl_->setTargetType(JOINT);
01558       return true;
01559     }
01560     ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
01561     return false;
01562   }
01563 }
01564 
01565 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::vector<double>& joint_values)
01566 {
01567   if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
01568     return false;
01569   impl_->setTargetType(JOINT);
01570   impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
01571   return impl_->getJointStateTarget().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance());
01572 }
01573 
01574 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::map<std::string, double>& joint_values)
01575 {
01576   impl_->setTargetType(JOINT);
01577   impl_->getJointStateTarget().setVariablePositions(joint_values);
01578   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01579 }
01580 
01581 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const robot_state::RobotState& rstate)
01582 {
01583   impl_->setTargetType(JOINT);
01584   impl_->getJointStateTarget() = rstate;
01585   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01586 }
01587 
01588 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string& joint_name, double value)
01589 {
01590   std::vector<double> values(1, value);
01591   return setJointValueTarget(joint_name, values);
01592 }
01593 
01594 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string& joint_name,
01595                                                                 const std::vector<double>& values)
01596 {
01597   impl_->setTargetType(JOINT);
01598   const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
01599   if (jm && jm->getVariableCount() == values.size())
01600   {
01601     impl_->getJointStateTarget().setJointPositions(jm, values);
01602     return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
01603   }
01604   return false;
01605 }
01606 
01607 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const sensor_msgs::JointState& state)
01608 {
01609   impl_->setTargetType(JOINT);
01610   impl_->getJointStateTarget().setVariableValues(state);
01611   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01612 }
01613 
01614 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::Pose& eef_pose,
01615                                                                 const std::string& end_effector_link)
01616 {
01617   return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
01618 }
01619 
01620 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
01621                                                                 const std::string& end_effector_link)
01622 {
01623   return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
01624 }
01625 
01626 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const Eigen::Affine3d& eef_pose,
01627                                                                 const std::string& end_effector_link)
01628 {
01629   geometry_msgs::Pose msg;
01630   tf::poseEigenToMsg(eef_pose, msg);
01631   return setJointValueTarget(msg, end_effector_link);
01632 }
01633 
01634 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose,
01635                                                                            const std::string& end_effector_link)
01636 {
01637   return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
01638 }
01639 
01640 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
01641                                                                            const std::string& end_effector_link)
01642 {
01643   return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
01644 }
01645 
01646 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const Eigen::Affine3d& eef_pose,
01647                                                                            const std::string& end_effector_link)
01648 {
01649   geometry_msgs::Pose msg;
01650   tf::poseEigenToMsg(eef_pose, msg);
01651   return setApproximateJointValueTarget(msg, end_effector_link);
01652 }
01653 
01654 const robot_state::RobotState& moveit::planning_interface::MoveGroup::getJointValueTarget() const
01655 {
01656   return impl_->getJointStateTarget();
01657 }
01658 
01659 const std::string& moveit::planning_interface::MoveGroup::getEndEffectorLink() const
01660 {
01661   return impl_->getEndEffectorLink();
01662 }
01663 
01664 const std::string& moveit::planning_interface::MoveGroup::getEndEffector() const
01665 {
01666   return impl_->getEndEffector();
01667 }
01668 
01669 bool moveit::planning_interface::MoveGroup::setEndEffectorLink(const std::string& link_name)
01670 {
01671   if (impl_->getEndEffectorLink().empty() || link_name.empty())
01672     return false;
01673   impl_->setEndEffectorLink(link_name);
01674   impl_->setTargetType(POSE);
01675   return true;
01676 }
01677 
01678 bool moveit::planning_interface::MoveGroup::setEndEffector(const std::string& eef_name)
01679 {
01680   const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
01681   if (jmg)
01682     return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
01683   return false;
01684 }
01685 
01686 void moveit::planning_interface::MoveGroup::clearPoseTarget(const std::string& end_effector_link)
01687 {
01688   impl_->clearPoseTarget(end_effector_link);
01689 }
01690 
01691 void moveit::planning_interface::MoveGroup::clearPoseTargets()
01692 {
01693   impl_->clearPoseTargets();
01694 }
01695 
01696 bool moveit::planning_interface::MoveGroup::setPoseTarget(const Eigen::Affine3d& pose,
01697                                                           const std::string& end_effector_link)
01698 {
01699   std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01700   tf::poseEigenToMsg(pose, pose_msg[0].pose);
01701   pose_msg[0].header.frame_id = getPoseReferenceFrame();
01702   pose_msg[0].header.stamp = ros::Time::now();
01703   return setPoseTargets(pose_msg, end_effector_link);
01704 }
01705 
01706 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::Pose& target,
01707                                                           const std::string& end_effector_link)
01708 {
01709   std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01710   pose_msg[0].pose = target;
01711   pose_msg[0].header.frame_id = getPoseReferenceFrame();
01712   pose_msg[0].header.stamp = ros::Time::now();
01713   return setPoseTargets(pose_msg, end_effector_link);
01714 }
01715 
01716 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::PoseStamped& target,
01717                                                           const std::string& end_effector_link)
01718 {
01719   std::vector<geometry_msgs::PoseStamped> targets(1, target);
01720   return setPoseTargets(targets, end_effector_link);
01721 }
01722 
01723 bool moveit::planning_interface::MoveGroup::setPoseTargets(const EigenSTL::vector_Affine3d& target,
01724                                                            const std::string& end_effector_link)
01725 {
01726   std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
01727   ros::Time tm = ros::Time::now();
01728   const std::string& frame_id = getPoseReferenceFrame();
01729   for (std::size_t i = 0; i < target.size(); ++i)
01730   {
01731     tf::poseEigenToMsg(target[i], pose_out[i].pose);
01732     pose_out[i].header.stamp = tm;
01733     pose_out[i].header.frame_id = frame_id;
01734   }
01735   return setPoseTargets(pose_out, end_effector_link);
01736 }
01737 
01738 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
01739                                                            const std::string& end_effector_link)
01740 {
01741   std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
01742   ros::Time tm = ros::Time::now();
01743   const std::string& frame_id = getPoseReferenceFrame();
01744   for (std::size_t i = 0; i < target.size(); ++i)
01745   {
01746     target_stamped[i].pose = target[i];
01747     target_stamped[i].header.stamp = tm;
01748     target_stamped[i].header.frame_id = frame_id;
01749   }
01750   return setPoseTargets(target_stamped, end_effector_link);
01751 }
01752 
01753 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target,
01754                                                            const std::string& end_effector_link)
01755 {
01756   if (target.empty())
01757   {
01758     ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
01759     return false;
01760   }
01761   else
01762   {
01763     impl_->setTargetType(POSE);
01764     return impl_->setPoseTargets(target, end_effector_link);
01765   }
01766 }
01767 
01768 const geometry_msgs::PoseStamped&
01769 moveit::planning_interface::MoveGroup::getPoseTarget(const std::string& end_effector_link) const
01770 {
01771   return impl_->getPoseTarget(end_effector_link);
01772 }
01773 
01774 const std::vector<geometry_msgs::PoseStamped>&
01775 moveit::planning_interface::MoveGroup::getPoseTargets(const std::string& end_effector_link) const
01776 {
01777   return impl_->getPoseTargets(end_effector_link);
01778 }
01779 
01780 namespace
01781 {
01782 inline void transformPose(const tf::Transformer& tf, const std::string& desired_frame,
01783                           geometry_msgs::PoseStamped& target)
01784 {
01785   if (desired_frame != target.header.frame_id)
01786   {
01787     tf::Pose pose;
01788     tf::poseMsgToTF(target.pose, pose);
01789     tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
01790     tf::Stamped<tf::Pose> stamped_target_out;
01791     tf.transformPose(desired_frame, stamped_target, stamped_target_out);
01792     target.header.frame_id = stamped_target_out.frame_id_;
01793     //    target.header.stamp = stamped_target_out.stamp_; // we leave the stamp to ros::Time(0) on purpose
01794     tf::poseTFToMsg(stamped_target_out, target.pose);
01795   }
01796 }
01797 }
01798 
01799 bool moveit::planning_interface::MoveGroup::setPositionTarget(double x, double y, double z,
01800                                                               const std::string& end_effector_link)
01801 {
01802   geometry_msgs::PoseStamped target;
01803   if (impl_->hasPoseTarget(end_effector_link))
01804   {
01805     target = getPoseTarget(end_effector_link);
01806     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01807   }
01808   else
01809   {
01810     target.pose.orientation.x = 0.0;
01811     target.pose.orientation.y = 0.0;
01812     target.pose.orientation.z = 0.0;
01813     target.pose.orientation.w = 1.0;
01814     target.header.frame_id = impl_->getPoseReferenceFrame();
01815   }
01816 
01817   target.pose.position.x = x;
01818   target.pose.position.y = y;
01819   target.pose.position.z = z;
01820   bool result = setPoseTarget(target, end_effector_link);
01821   impl_->setTargetType(POSITION);
01822   return result;
01823 }
01824 
01825 bool moveit::planning_interface::MoveGroup::setRPYTarget(double r, double p, double y,
01826                                                          const std::string& end_effector_link)
01827 {
01828   geometry_msgs::PoseStamped target;
01829   if (impl_->hasPoseTarget(end_effector_link))
01830   {
01831     target = getPoseTarget(end_effector_link);
01832     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01833   }
01834   else
01835   {
01836     target.pose.position.x = 0.0;
01837     target.pose.position.y = 0.0;
01838     target.pose.position.z = 0.0;
01839     target.header.frame_id = impl_->getPoseReferenceFrame();
01840   }
01841 
01842   tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
01843   bool result = setPoseTarget(target, end_effector_link);
01844   impl_->setTargetType(ORIENTATION);
01845   return result;
01846 }
01847 
01848 bool moveit::planning_interface::MoveGroup::setOrientationTarget(double x, double y, double z, double w,
01849                                                                  const std::string& end_effector_link)
01850 {
01851   geometry_msgs::PoseStamped target;
01852   if (impl_->hasPoseTarget(end_effector_link))
01853   {
01854     target = getPoseTarget(end_effector_link);
01855     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01856   }
01857   else
01858   {
01859     target.pose.position.x = 0.0;
01860     target.pose.position.y = 0.0;
01861     target.pose.position.z = 0.0;
01862     target.header.frame_id = impl_->getPoseReferenceFrame();
01863   }
01864 
01865   target.pose.orientation.x = x;
01866   target.pose.orientation.y = y;
01867   target.pose.orientation.z = z;
01868   target.pose.orientation.w = w;
01869   bool result = setPoseTarget(target, end_effector_link);
01870   impl_->setTargetType(ORIENTATION);
01871   return result;
01872 }
01873 
01874 void moveit::planning_interface::MoveGroup::setPoseReferenceFrame(const std::string& pose_reference_frame)
01875 {
01876   impl_->setPoseReferenceFrame(pose_reference_frame);
01877 }
01878 
01879 const std::string& moveit::planning_interface::MoveGroup::getPoseReferenceFrame() const
01880 {
01881   return impl_->getPoseReferenceFrame();
01882 }
01883 
01884 double moveit::planning_interface::MoveGroup::getGoalJointTolerance() const
01885 {
01886   return impl_->getGoalJointTolerance();
01887 }
01888 
01889 double moveit::planning_interface::MoveGroup::getGoalPositionTolerance() const
01890 {
01891   return impl_->getGoalPositionTolerance();
01892 }
01893 
01894 double moveit::planning_interface::MoveGroup::getGoalOrientationTolerance() const
01895 {
01896   return impl_->getGoalOrientationTolerance();
01897 }
01898 
01899 void moveit::planning_interface::MoveGroup::setGoalTolerance(double tolerance)
01900 {
01901   setGoalJointTolerance(tolerance);
01902   setGoalPositionTolerance(tolerance);
01903   setGoalOrientationTolerance(tolerance);
01904 }
01905 
01906 void moveit::planning_interface::MoveGroup::setGoalJointTolerance(double tolerance)
01907 {
01908   impl_->setGoalJointTolerance(tolerance);
01909 }
01910 
01911 void moveit::planning_interface::MoveGroup::setGoalPositionTolerance(double tolerance)
01912 {
01913   impl_->setGoalPositionTolerance(tolerance);
01914 }
01915 
01916 void moveit::planning_interface::MoveGroup::setGoalOrientationTolerance(double tolerance)
01917 {
01918   impl_->setGoalOrientationTolerance(tolerance);
01919 }
01920 
01921 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string& name)
01922 {
01923   rememberJointValues(name, getCurrentJointValues());
01924 }
01925 
01926 bool moveit::planning_interface::MoveGroup::startStateMonitor(double wait)
01927 {
01928   return impl_->startStateMonitor(wait);
01929 }
01930 
01931 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentJointValues()
01932 {
01933   robot_state::RobotStatePtr current_state;
01934   std::vector<double> values;
01935   if (impl_->getCurrentState(current_state))
01936     current_state->copyJointGroupPositions(getName(), values);
01937   return values;
01938 }
01939 
01940 std::vector<double> moveit::planning_interface::MoveGroup::getRandomJointValues()
01941 {
01942   std::vector<double> r;
01943   impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
01944   return r;
01945 }
01946 
01947 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getRandomPose(const std::string& end_effector_link)
01948 {
01949   const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01950   Eigen::Affine3d pose;
01951   pose.setIdentity();
01952   if (eef.empty())
01953     ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
01954   else
01955   {
01956     robot_state::RobotStatePtr current_state;
01957     if (impl_->getCurrentState(current_state))
01958     {
01959       current_state->setToRandomPositions(impl_->getJointModelGroup());
01960       const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
01961       if (lm)
01962         pose = current_state->getGlobalLinkTransform(lm);
01963     }
01964   }
01965   geometry_msgs::PoseStamped pose_msg;
01966   pose_msg.header.stamp = ros::Time::now();
01967   pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01968   tf::poseEigenToMsg(pose, pose_msg.pose);
01969   return pose_msg;
01970 }
01971 
01972 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getCurrentPose(const std::string& end_effector_link)
01973 {
01974   const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01975   Eigen::Affine3d pose;
01976   pose.setIdentity();
01977   if (eef.empty())
01978     ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
01979   else
01980   {
01981     robot_state::RobotStatePtr current_state;
01982     if (impl_->getCurrentState(current_state))
01983     {
01984       const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
01985       if (lm)
01986         pose = current_state->getGlobalLinkTransform(lm);
01987     }
01988   }
01989   geometry_msgs::PoseStamped pose_msg;
01990   pose_msg.header.stamp = ros::Time::now();
01991   pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01992   tf::poseEigenToMsg(pose, pose_msg.pose);
01993   return pose_msg;
01994 }
01995 
01996 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentRPY(const std::string& end_effector_link)
01997 {
01998   std::vector<double> result;
01999   const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
02000   if (eef.empty())
02001     ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
02002   else
02003   {
02004     robot_state::RobotStatePtr current_state;
02005     if (impl_->getCurrentState(current_state))
02006     {
02007       const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
02008       if (lm)
02009       {
02010         result.resize(3);
02011         tf::Matrix3x3 ptf;
02012         tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
02013         tfScalar pitch, roll, yaw;
02014         ptf.getRPY(roll, pitch, yaw);
02015         result[0] = roll;
02016         result[1] = pitch;
02017         result[2] = yaw;
02018       }
02019     }
02020   }
02021   return result;
02022 }
02023 
02024 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getActiveJoints() const
02025 {
02026   return impl_->getJointModelGroup()->getActiveJointModelNames();
02027 }
02028 
02029 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getJoints() const
02030 {
02031   return impl_->getJointModelGroup()->getJointModelNames();
02032 }
02033 
02034 unsigned int moveit::planning_interface::MoveGroup::getVariableCount() const
02035 {
02036   return impl_->getJointModelGroup()->getVariableCount();
02037 }
02038 
02039 robot_state::RobotStatePtr moveit::planning_interface::MoveGroup::getCurrentState()
02040 {
02041   robot_state::RobotStatePtr current_state;
02042   impl_->getCurrentState(current_state);
02043   return current_state;
02044 }
02045 
02046 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string& name,
02047                                                                 const std::vector<double>& values)
02048 {
02049   remembered_joint_values_[name] = values;
02050 }
02051 
02052 void moveit::planning_interface::MoveGroup::forgetJointValues(const std::string& name)
02053 {
02054   remembered_joint_values_.erase(name);
02055 }
02056 
02057 void moveit::planning_interface::MoveGroup::allowLooking(bool flag)
02058 {
02059   impl_->allowLooking(flag);
02060 }
02061 
02062 void moveit::planning_interface::MoveGroup::allowReplanning(bool flag)
02063 {
02064   impl_->allowReplanning(flag);
02065 }
02066 
02067 std::vector<std::string> moveit::planning_interface::MoveGroup::getKnownConstraints() const
02068 {
02069   return impl_->getKnownConstraints();
02070 }
02071 
02072 moveit_msgs::Constraints moveit::planning_interface::MoveGroup::getPathConstraints() const
02073 {
02074   return impl_->getPathConstraints();
02075 }
02076 
02077 bool moveit::planning_interface::MoveGroup::setPathConstraints(const std::string& constraint)
02078 {
02079   return impl_->setPathConstraints(constraint);
02080 }
02081 
02082 void moveit::planning_interface::MoveGroup::setPathConstraints(const moveit_msgs::Constraints& constraint)
02083 {
02084   impl_->setPathConstraints(constraint);
02085 }
02086 
02087 void moveit::planning_interface::MoveGroup::clearPathConstraints()
02088 {
02089   impl_->clearPathConstraints();
02090 }
02091 
02092 void moveit::planning_interface::MoveGroup::setConstraintsDatabase(const std::string& host, unsigned int port)
02093 {
02094   impl_->initializeConstraintsStorage(host, port);
02095 }
02096 
02097 void moveit::planning_interface::MoveGroup::setWorkspace(double minx, double miny, double minz, double maxx,
02098                                                          double maxy, double maxz)
02099 {
02100   impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
02101 }
02102 
02103 void moveit::planning_interface::MoveGroup::setPlanningTime(double seconds)
02104 {
02105   impl_->setPlanningTime(seconds);
02106 }
02107 
02108 double moveit::planning_interface::MoveGroup::getPlanningTime() const
02109 {
02110   return impl_->getPlanningTime();
02111 }
02112 
02113 void moveit::planning_interface::MoveGroup::setSupportSurfaceName(const std::string& name)
02114 {
02115   impl_->setSupportSurfaceName(name);
02116 }
02117 
02118 const std::string& moveit::planning_interface::MoveGroup::getPlanningFrame() const
02119 {
02120   return impl_->getRobotModel()->getModelFrame();
02121 }
02122 
02123 bool moveit::planning_interface::MoveGroup::attachObject(const std::string& object, const std::string& link)
02124 {
02125   return attachObject(object, link, std::vector<std::string>());
02126 }
02127 
02128 bool moveit::planning_interface::MoveGroup::attachObject(const std::string& object, const std::string& link,
02129                                                          const std::vector<std::string>& touch_links)
02130 {
02131   return impl_->attachObject(object, link, touch_links);
02132 }
02133 
02134 bool moveit::planning_interface::MoveGroup::detachObject(const std::string& name)
02135 {
02136   return impl_->detachObject(name);
02137 }


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:53