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 <moveit/warehouse/constraints_storage.h>
00041 #include <moveit/kinematic_constraints/utils.h>
00042 #include <moveit/move_group/capability_names.h>
00043 #include <moveit/move_group_pick_place_capability/capability_names.h>
00044 #include <moveit/move_group_interface/move_group.h>
00045 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00046 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00047 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
00048 #include <moveit/common_planning_interface_objects/common_objects.h>
00049 #include <moveit/robot_state/conversions.h>
00050 #include <moveit_msgs/MoveGroupAction.h>
00051 #include <moveit_msgs/PickupAction.h>
00052 #include <moveit_msgs/PlaceAction.h>
00053 #include <moveit_msgs/ExecuteKnownTrajectory.h>
00054 #include <moveit_msgs/QueryPlannerInterfaces.h>
00055 #include <moveit_msgs/GetCartesianPath.h>
00056 
00057 #include <actionlib/client/simple_action_client.h>
00058 #include <eigen_conversions/eigen_msg.h>
00059 #include <std_msgs/String.h>
00060 #include <tf/transform_listener.h>
00061 #include <tf/transform_datatypes.h>
00062 #include <tf_conversions/tf_eigen.h>
00063 #include <ros/console.h>
00064 #include <ros/ros.h>
00065 
00066 namespace moveit
00067 {
00068 namespace planning_interface
00069 {
00070 
00071 const std::string MoveGroup::ROBOT_DESCRIPTION = "robot_description";    // name of the robot description (a param name, so it can be changed externally)
00072 
00073 namespace
00074 {
00075 
00076 enum ActiveTargetType
00077 {
00078   JOINT, POSE, POSITION, ORIENTATION
00079 };
00080 
00081 }
00082 
00083 class MoveGroup::MoveGroupImpl
00084 {
00085 public:
00086 
00087   MoveGroupImpl(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
00088     : opt_(opt),
00089       node_handle_(opt.node_handle_),
00090       tf_(tf)
00091   {
00092     robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_);
00093     if (!getRobotModel())
00094     {
00095       std::string error = "Unable to construct robot model. Please make sure all needed information is on the parameter server.";
00096       ROS_FATAL_STREAM(error);
00097       throw std::runtime_error(error);
00098     }
00099 
00100     if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
00101     {
00102       std::string error = "Group '" + opt.group_name_ + "' was not found.";
00103       ROS_FATAL_STREAM(error);
00104       throw std::runtime_error(error);
00105     }
00106     joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
00107     
00108     joint_state_target_.reset(new robot_state::RobotState(getRobotModel()));
00109     joint_state_target_->setToDefaultValues();
00110     active_target_ = JOINT;
00111     can_look_ = false;
00112     can_replan_ = false;
00113     replan_delay_ = 2.0;
00114     goal_joint_tolerance_ = 1e-4;
00115     goal_position_tolerance_ = 1e-4; // 0.1 mm
00116     goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
00117     planning_time_ = 5.0;
00118     num_planning_attempts_ = 1;
00119     initializing_constraints_ = false;
00120 
00121     if (joint_model_group_->isChain())
00122       end_effector_link_ = joint_model_group_->getLinkModelNames().back();
00123     pose_reference_frame_ = getRobotModel()->getModelFrame();
00124     
00125     trajectory_event_publisher_ = node_handle_.advertise<std_msgs::String>(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false);
00126     attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false);
00127     
00128     current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_);
00129     
00130     move_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>(node_handle_,
00131                                                                                               move_group::MOVE_ACTION,
00132                                                                                               false));
00133     waitForAction(move_action_client_, wait_for_server, move_group::MOVE_ACTION);
00134     
00135     pick_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::PickupAction>(node_handle_,
00136                                                                                            move_group::PICKUP_ACTION,
00137                                                                                            false));
00138     waitForAction(pick_action_client_, wait_for_server, move_group::PICKUP_ACTION);
00139     
00140     place_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::PlaceAction>(node_handle_,
00141                                                                                            move_group::PLACE_ACTION,
00142                                                                                            false));
00143     waitForAction(place_action_client_, wait_for_server, move_group::PLACE_ACTION);
00144     
00145     execute_service_ = node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
00146     query_service_ = node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
00147     cartesian_path_service_ = node_handle_.serviceClient<moveit_msgs::GetCartesianPath>(move_group::CARTESIAN_PATH_SERVICE_NAME);
00148     
00149     ROS_INFO_STREAM("Ready to take MoveGroup commands for group " << opt.group_name_ << ".");
00150   }
00151 
00152   template<typename T>
00153   void waitForAction(const T &action, const ros::Duration &wait_for_server, const std::string &name)
00154   {
00155     ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());
00156 
00157     // in case ROS time is published, wait for the time data to arrive
00158     ros::Time start_time = ros::Time::now();
00159     while (start_time == ros::Time::now())
00160     {
00161       ros::WallDuration(0.01).sleep();
00162       ros::spinOnce();
00163     }
00164 
00165     // wait for the server (and spin as needed)
00166     if (wait_for_server == ros::Duration(0, 0))
00167     {
00168       while (node_handle_.ok() && !action->isServerConnected())
00169       {
00170         ros::WallDuration(0.02).sleep();
00171         ros::spinOnce();
00172       }
00173     }
00174     else
00175     {
00176       ros::Time final_time = ros::Time::now() + wait_for_server;
00177       while (node_handle_.ok() && !action->isServerConnected() && final_time > ros::Time::now())
00178       {
00179         ros::WallDuration(0.02).sleep();
00180         ros::spinOnce();
00181       }
00182     }
00183 
00184     if (!action->isServerConnected())
00185       throw std::runtime_error("Unable to connect to move_group action server within allotted time (2)");
00186     else
00187       ROS_DEBUG("Connected to '%s'", name.c_str());
00188   }
00189 
00190   ~MoveGroupImpl()
00191   {
00192     if (constraints_init_thread_)
00193       constraints_init_thread_->join();
00194   }
00195 
00196   const boost::shared_ptr<tf::Transformer>& getTF() const
00197   {
00198     return tf_;
00199   }
00200 
00201   const Options& getOptions() const
00202   {
00203     return opt_;
00204   }
00205 
00206   const robot_model::RobotModelConstPtr& getRobotModel() const
00207   {
00208     return robot_model_;
00209   }
00210 
00211   const robot_model::JointModelGroup* getJointModelGroup() const
00212   {
00213     return joint_model_group_;
00214   }
00215   
00216   bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
00217   {
00218     moveit_msgs::QueryPlannerInterfaces::Request req;
00219     moveit_msgs::QueryPlannerInterfaces::Response res;
00220     if (query_service_.call(req, res))
00221       if (!res.planner_interfaces.empty())
00222       {
00223         desc = res.planner_interfaces.front();
00224         return true;
00225       }
00226     return false;
00227   }
00228 
00229   void setPlannerId(const std::string &planner_id)
00230   {
00231     planner_id_ = planner_id;
00232   }
00233 
00234   void setNumPlanningAttempts(unsigned int num_planning_attempts)
00235   {
00236     num_planning_attempts_ = num_planning_attempts;
00237   }
00238   
00239   robot_state::RobotState& getJointStateTarget()
00240   {
00241     return *joint_state_target_;
00242   }
00243 
00244   void setStartState(const robot_state::RobotState &start_state)
00245   {
00246     considered_start_state_.reset(new robot_state::RobotState(start_state));
00247   }
00248   
00249   void setStartStateToCurrentState()
00250   {
00251     considered_start_state_.reset();
00252   }
00253   
00254   robot_state::RobotStatePtr getStartState()
00255   {
00256     if (considered_start_state_)
00257       return considered_start_state_;
00258     else
00259     {
00260       robot_state::RobotStatePtr s;
00261       getCurrentState(s);
00262       return s;
00263     }
00264   }
00265   
00266   bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
00267   {
00268     const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
00269     // this only works if we have an end-effector
00270     if (!eef.empty())
00271     {
00272       // first we set the goal to be the same as the start state
00273       moveit::core::RobotStatePtr c = getStartState();
00274       if (c)
00275       {
00276         setTargetType(JOINT);
00277         c->enforceBounds();
00278         getJointStateTarget() = *c;
00279         if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
00280           return false;
00281       }
00282       else
00283         return false;
00284 
00285       // we may need to do approximate IK
00286       kinematics::KinematicsQueryOptions o;
00287       o.return_approximate_solution = approx;
00288 
00289       // if no frame transforms are needed, call IK directly
00290       if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
00291         return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0, 0.0, moveit::core::GroupStateValidityCallbackFn(), o);
00292       else
00293       {
00294         if (c->knowsFrameTransform(frame))
00295         {
00296           // transform the pose first if possible, then do IK
00297           const Eigen::Affine3d &t = getJointStateTarget().getFrameTransform(frame);
00298           Eigen::Affine3d p;
00299           tf::poseMsgToEigen(eef_pose, p);
00300           return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0, 0.0, moveit::core::GroupStateValidityCallbackFn(), o);
00301         }
00302         else
00303         {
00304           logError("Unable to transform from frame '%s' to frame '%s'", frame.c_str(), getRobotModel()->getModelFrame().c_str());
00305           return false;
00306         }
00307       }
00308     }
00309     else
00310       return false;
00311   }
00312     
00313   void setEndEffectorLink(const std::string &end_effector)
00314   {
00315     end_effector_link_ = end_effector;
00316   }
00317 
00318   void clearPoseTarget(const std::string &end_effector_link)
00319   {
00320     pose_targets_.erase(end_effector_link);
00321   }
00322 
00323   void clearPoseTargets()
00324   {
00325     pose_targets_.clear();
00326   }
00327 
00328   const std::string &getEndEffectorLink() const
00329   {
00330     return end_effector_link_;
00331   }
00332 
00333   const std::string& getEndEffector() const
00334   {
00335     if (!end_effector_link_.empty())
00336     {
00337       const std::vector<std::string> &possible_eefs = getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
00338       for (std::size_t i = 0 ; i < possible_eefs.size() ; ++i)
00339         if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
00340           return possible_eefs[i];
00341     }
00342     static std::string empty;
00343     return empty;
00344   }
00345 
00346   bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped> &poses, const std::string &end_effector_link)
00347   {
00348     const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00349     if (eef.empty())
00350     {
00351       ROS_ERROR("No end-effector to set the pose for");
00352       return false;
00353     }
00354     else
00355     {
00356       pose_targets_[eef] = poses;
00357       // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
00358       std::vector<geometry_msgs::PoseStamped> &stored_poses = pose_targets_[eef];
00359       for (std::size_t i = 0 ; i < stored_poses.size() ; ++i)
00360         stored_poses[i].header.stamp = ros::Time(0);
00361     }
00362     return true;
00363   }
00364 
00365   bool hasPoseTarget(const std::string &end_effector_link) const
00366   {
00367     const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00368     return pose_targets_.find(eef) != pose_targets_.end();
00369   }
00370 
00371   const geometry_msgs::PoseStamped& getPoseTarget(const std::string &end_effector_link) const
00372   {
00373     const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00374 
00375     // if multiple pose targets are set, return the first one
00376     std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
00377     if (jt != pose_targets_.end())
00378       if (!jt->second.empty())
00379         return jt->second.at(0);
00380 
00381     // or return an error
00382     static const geometry_msgs::PoseStamped unknown;
00383     ROS_ERROR("Pose for end effector '%s' not known.", eef.c_str());
00384     return unknown;
00385   }
00386 
00387   const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string &end_effector_link) const
00388   {
00389     const std::string &eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
00390 
00391     std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
00392     if (jt != pose_targets_.end())
00393       if (!jt->second.empty())
00394         return jt->second;
00395 
00396     // or return an error
00397     static const std::vector<geometry_msgs::PoseStamped> empty;
00398     ROS_ERROR("Poses for end effector '%s' are not known.", eef.c_str());
00399     return empty;
00400   }
00401 
00402   void setPoseReferenceFrame(const std::string &pose_reference_frame)
00403   {
00404     pose_reference_frame_ = pose_reference_frame;
00405   }
00406 
00407   void setSupportSurfaceName(const std::string &support_surface)
00408   {
00409     support_surface_ = support_surface;
00410   }
00411 
00412   const std::string& getPoseReferenceFrame() const
00413   {
00414     return pose_reference_frame_;
00415   }
00416 
00417   void setTargetType(ActiveTargetType type)
00418   {
00419     active_target_ = type;
00420   }
00421 
00422   ActiveTargetType getTargetType() const
00423   {
00424     return active_target_;
00425   }
00426 
00427   bool startStateMonitor(double wait)
00428   {
00429     if (!current_state_monitor_)
00430     {
00431       ROS_ERROR("Unable to monitor current robot state");
00432       return false;
00433     }
00434 
00435     // if needed, start the monitor and wait up to 1 second for a full robot state
00436     if (!current_state_monitor_->isActive())
00437       current_state_monitor_->startStateMonitor();
00438 
00439     current_state_monitor_->waitForCurrentState(opt_.group_name_, wait);
00440     return true;
00441   }
00442   
00443   bool getCurrentState(robot_state::RobotStatePtr &current_state, double wait_seconds = 1.0)
00444   {
00445     if (!current_state_monitor_)
00446     {
00447       ROS_ERROR("Unable to get current robot state");
00448       return false;
00449     }
00450 
00451     // if needed, start the monitor and wait up to 1 second for a full robot state
00452     if (!current_state_monitor_->isActive())
00453       current_state_monitor_->startStateMonitor();
00454 
00455     if (!current_state_monitor_->waitForCurrentState(opt_.group_name_, wait_seconds))
00456       ROS_WARN("Joint values for monitored state are requested but the full state is not known");
00457 
00458     current_state = current_state_monitor_->getCurrentState();
00459     return true;
00460   }
00461 
00463   MoveItErrorCode place(const std::string &object, const std::vector<geometry_msgs::PoseStamped> &poses)
00464   {
00465     std::vector<moveit_msgs::PlaceLocation> locations;
00466     for (std::size_t i = 0; i < poses.size(); ++i)
00467     {
00468       moveit_msgs::PlaceLocation location;
00469       location.pre_place_approach.direction.vector.z = -1.0;
00470       location.post_place_retreat.direction.vector.x = -1.0;
00471       location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
00472       location.post_place_retreat.direction.header.frame_id = end_effector_link_;
00473 
00474       location.pre_place_approach.min_distance = 0.1;
00475       location.pre_place_approach.desired_distance = 0.2;
00476       location.post_place_retreat.min_distance = 0.0;
00477       location.post_place_retreat.desired_distance = 0.2;
00478       // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
00479 
00480       location.place_pose = poses[i];
00481       locations.push_back(location);
00482     }
00483     ROS_DEBUG("Move group interface has %u place locations", (unsigned int) locations.size());
00484     return place(object, locations);
00485   }
00486 
00487   MoveItErrorCode place(const std::string &object, const std::vector<moveit_msgs::PlaceLocation> &locations)
00488   {
00489     if (!place_action_client_)
00490     {
00491       ROS_ERROR_STREAM("Place action client not found");
00492       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00493     }
00494     if (!place_action_client_->isServerConnected())
00495     {
00496       ROS_ERROR_STREAM("Place action server not connected");
00497       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00498     }
00499     moveit_msgs::PlaceGoal goal;
00500     constructGoal(goal, object);
00501     goal.place_locations = locations;
00502     goal.planning_options.plan_only = false;
00503     goal.planning_options.look_around = can_look_;
00504     goal.planning_options.replan = can_replan_;
00505     goal.planning_options.replan_delay = replan_delay_;
00506     goal.planning_options.planning_scene_diff.is_diff = true;
00507     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00508 
00509     place_action_client_->sendGoal(goal);
00510     ROS_DEBUG("Sent place goal with %d locations", (int) goal.place_locations.size());
00511     if (!place_action_client_->waitForResult())
00512     {
00513       ROS_INFO_STREAM("Place action returned early");
00514     }
00515     if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00516     {
00517       return MoveItErrorCode(place_action_client_->getResult()->error_code);
00518     }
00519     else
00520     {
00521       ROS_WARN_STREAM("Fail: " << place_action_client_->getState().toString() << ": " << place_action_client_->getState().getText());
00522       return MoveItErrorCode(place_action_client_->getResult()->error_code);
00523     }
00524   }
00525 
00526   MoveItErrorCode pick(const std::string &object, const std::vector<moveit_msgs::Grasp> &grasps)
00527   {
00528     if (!pick_action_client_)
00529     {
00530       ROS_ERROR_STREAM("Pick action client not found");
00531       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00532     }
00533     if (!pick_action_client_->isServerConnected())
00534     {
00535       ROS_ERROR_STREAM("Pick action server not connected");
00536       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00537     }
00538     moveit_msgs::PickupGoal goal;
00539     constructGoal(goal, object);
00540     goal.possible_grasps = grasps;
00541     goal.planning_options.plan_only = false;
00542     goal.planning_options.look_around = can_look_;
00543     goal.planning_options.replan = can_replan_;
00544     goal.planning_options.replan_delay = replan_delay_;
00545     goal.planning_options.planning_scene_diff.is_diff = true;
00546     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00547 
00548     pick_action_client_->sendGoal(goal);
00549     if (!pick_action_client_->waitForResult())
00550     {
00551       ROS_INFO_STREAM("Pickup action returned early");
00552     }
00553     if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00554     {
00555       return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00556     }
00557     else
00558     {
00559       ROS_WARN_STREAM("Fail: " << pick_action_client_->getState().toString() << ": " << pick_action_client_->getState().getText());
00560       return MoveItErrorCode(pick_action_client_->getResult()->error_code);
00561     }
00562   }
00563 
00564   MoveItErrorCode plan(Plan &plan)
00565   {
00566     if (!move_action_client_)
00567     {
00568       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00569     }
00570     if (!move_action_client_->isServerConnected())
00571     {
00572       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00573     }
00574 
00575     moveit_msgs::MoveGroupGoal goal;
00576     constructGoal(goal);
00577     goal.planning_options.plan_only = true;
00578     goal.planning_options.look_around = false;
00579     goal.planning_options.replan = false;
00580     goal.planning_options.planning_scene_diff.is_diff = true;
00581     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00582 
00583     move_action_client_->sendGoal(goal);
00584     if (!move_action_client_->waitForResult())
00585     {
00586       ROS_INFO_STREAM("MoveGroup action returned early");
00587     }
00588     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00589     {
00590       plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
00591       plan.start_state_ = move_action_client_->getResult()->trajectory_start;
00592       plan.planning_time_ = move_action_client_->getResult()->planning_time;
00593       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00594     }
00595     else
00596     {
00597       ROS_WARN_STREAM("Fail: " << move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText());
00598       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00599     }
00600   }
00601 
00602   MoveItErrorCode move(bool wait)
00603   {
00604     if (!move_action_client_)
00605     {
00606       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00607     }    
00608     if (!move_action_client_->isServerConnected())
00609     {
00610       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00611     }
00612 
00613     moveit_msgs::MoveGroupGoal goal;
00614     constructGoal(goal);
00615     goal.planning_options.plan_only = false;
00616     goal.planning_options.look_around = can_look_;
00617     goal.planning_options.replan = can_replan_;
00618     goal.planning_options.replan_delay = replan_delay_;
00619     goal.planning_options.planning_scene_diff.is_diff = true;
00620     goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00621 
00622     move_action_client_->sendGoal(goal);
00623     if (!wait)    
00624     {
00625       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
00626     }
00627 
00628     if (!move_action_client_->waitForResult())
00629     {
00630       ROS_INFO_STREAM("MoveGroup action returned early");
00631     }
00632 
00633     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00634     {
00635       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00636     }    
00637     else
00638     {
00639       ROS_INFO_STREAM(move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText());
00640       return MoveItErrorCode(move_action_client_->getResult()->error_code);
00641     }
00642   }
00643 
00644   MoveItErrorCode execute(const Plan &plan, bool wait)
00645   {
00646     moveit_msgs::ExecuteKnownTrajectory::Request req;
00647     moveit_msgs::ExecuteKnownTrajectory::Response res;
00648     req.trajectory = plan.trajectory_;
00649     req.wait_for_execution = wait;
00650     if (execute_service_.call(req, res))
00651     {      
00652       return MoveItErrorCode(res.error_code);
00653     }    
00654     else
00655     {
00656       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
00657     }    
00658   }
00659 
00660   double computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double step, double jump_threshold,
00661                               moveit_msgs::RobotTrajectory &msg, bool avoid_collisions, moveit_msgs::MoveItErrorCodes &error_code)
00662   {
00663     moveit_msgs::GetCartesianPath::Request req;
00664     moveit_msgs::GetCartesianPath::Response res;
00665 
00666     if (considered_start_state_)
00667       robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
00668     req.group_name = opt_.group_name_;
00669     req.header.frame_id = getPoseReferenceFrame();
00670     req.header.stamp = ros::Time::now();
00671     req.waypoints = waypoints;
00672     req.max_step = step;
00673     req.jump_threshold = jump_threshold;
00674     req.avoid_collisions = avoid_collisions;
00675 
00676     if (cartesian_path_service_.call(req, res))
00677     {
00678       error_code = res.error_code;      
00679       if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00680       {
00681         msg = res.solution;
00682         return res.fraction;
00683       }
00684       else
00685         return -1.0;
00686     }
00687     else
00688     {      
00689       error_code.val =  error_code.FAILURE;      
00690       return -1.0;
00691     }    
00692   }
00693 
00694   void stop()
00695   {
00696     if (trajectory_event_publisher_)
00697     {
00698       std_msgs::String event;
00699       event.data = "stop";
00700       trajectory_event_publisher_.publish(event);
00701     }
00702   }
00703 
00704   bool attachObject(const std::string &object, const std::string &link, const std::vector<std::string> &touch_links)
00705   { 
00706     std::string l = link.empty() ? getEndEffectorLink() : link;
00707     if (l.empty())
00708     {
00709       const std::vector<std::string> &links = joint_model_group_->getLinkModelNames();
00710       if (!links.empty())
00711         l = links[0];
00712     }
00713     if (l.empty())
00714     {
00715       ROS_ERROR("No known link to attach object '%s' to", object.c_str());
00716       return false;
00717     }
00718     moveit_msgs::AttachedCollisionObject aco;
00719     aco.object.id = object;
00720     aco.link_name.swap(l);
00721     if (touch_links.empty())
00722       aco.touch_links.push_back(aco.link_name);
00723     else
00724       aco.touch_links = touch_links;
00725     aco.object.operation = moveit_msgs::CollisionObject::ADD;
00726     attached_object_publisher_.publish(aco);
00727     return true;
00728   }
00729     
00730   bool detachObject(const std::string &name)
00731   {
00732     moveit_msgs::AttachedCollisionObject aco;
00733     // if name is a link
00734     if (!name.empty() && joint_model_group_->hasLinkModel(name))
00735       aco.link_name = name;
00736     else
00737       aco.object.id = name;
00738     aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
00739     if (aco.link_name.empty() && aco.object.id.empty())
00740     {
00741       // we only want to detach objects for this group
00742       const std::vector<std::string> &lnames = joint_model_group_->getLinkModelNames();
00743       for (std::size_t i = 0 ; i < lnames.size() ; ++i)
00744       {
00745         aco.link_name = lnames[i];
00746         attached_object_publisher_.publish(aco);
00747       }
00748     }
00749     else
00750       attached_object_publisher_.publish(aco);
00751     return true;
00752   }
00753   
00754   double getGoalPositionTolerance() const
00755   {
00756     return goal_position_tolerance_;
00757   }
00758 
00759   double getGoalOrientationTolerance() const
00760   {
00761     return goal_orientation_tolerance_;
00762   }
00763 
00764   double getGoalJointTolerance() const
00765   {
00766     return goal_joint_tolerance_;
00767   }
00768 
00769   void setGoalJointTolerance(double tolerance)
00770   {
00771     goal_joint_tolerance_ = tolerance;
00772   }
00773 
00774   void setGoalPositionTolerance(double tolerance)
00775   {
00776     goal_position_tolerance_ = tolerance;
00777   }
00778 
00779   void setGoalOrientationTolerance(double tolerance)
00780   {
00781     goal_orientation_tolerance_ = tolerance;
00782   }
00783 
00784   void setPlanningTime(double seconds)
00785   {
00786     if (seconds > 0.0)
00787       planning_time_ = seconds;
00788   }
00789 
00790   double getPlanningTime() const
00791   {
00792     return planning_time_;
00793   }
00794 
00795   void allowLooking(bool flag)
00796   {
00797     can_look_ = flag;
00798     ROS_INFO("Looking around: %s", can_look_ ? "yes" : "no");
00799   }
00800 
00801   void allowReplanning(bool flag)
00802   {
00803     can_replan_ = flag;
00804     ROS_INFO("Replanning: %s", can_replan_ ? "yes" : "no");
00805   }
00806 
00807   void setReplanningDelay(double delay)
00808   {
00809     if (delay >= 0.0)
00810       replan_delay_ = delay;
00811   }
00812 
00813   double getReplanningDelay() const
00814   {
00815     return replan_delay_;
00816   }
00817 
00818   void constructGoal(moveit_msgs::MoveGroupGoal &goal_out)
00819   {
00820     moveit_msgs::MoveGroupGoal goal;
00821     goal.request.group_name = opt_.group_name_;
00822     goal.request.num_planning_attempts = num_planning_attempts_;
00823     goal.request.allowed_planning_time = planning_time_;
00824     goal.request.planner_id = planner_id_;
00825     goal.request.workspace_parameters = workspace_parameters_;
00826     goal.request.start_state.is_diff = true;
00827 
00828     if (considered_start_state_)
00829       robot_state::robotStateToRobotStateMsg(*considered_start_state_, goal.request.start_state);
00830 
00831     if (active_target_ == JOINT)
00832     {
00833       goal.request.goal_constraints.resize(1);
00834       goal.request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(getJointStateTarget(), joint_model_group_, goal_joint_tolerance_);
00835     }
00836     else
00837       if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
00838       {
00839         // find out how many goals are specified
00840         std::size_t goal_count = 0;
00841         for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin() ;
00842              it != pose_targets_.end() ; ++it)
00843           goal_count = std::max(goal_count, it->second.size());
00844 
00845         // start filling the goals;
00846         // each end effector has a number of possible poses (K) as valid goals
00847         // but there could be multiple end effectors specified, so we want each end effector
00848         // to reach the goal that corresponds to the goals of the other end effectors
00849         goal.request.goal_constraints.resize(goal_count);
00850 
00851         for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin() ;
00852              it != pose_targets_.end() ; ++it)
00853         {
00854           for (std::size_t i = 0 ; i < it->second.size() ; ++i)
00855           {
00856             moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
00857             if (active_target_ == ORIENTATION)
00858               c.position_constraints.clear();
00859             if (active_target_ == POSITION)
00860               c.orientation_constraints.clear();
00861             goal.request.goal_constraints[i] = kinematic_constraints::mergeConstraints(goal.request.goal_constraints[i], c);
00862           }
00863         }
00864       }
00865       else
00866         ROS_ERROR("Unable to construct goal representation");
00867 
00868     if (path_constraints_)
00869       goal.request.path_constraints = *path_constraints_;
00870     goal_out = goal;
00871   }
00872 
00873   void constructGoal(moveit_msgs::PickupGoal &goal_out, const std::string &object)
00874   {
00875     moveit_msgs::PickupGoal goal;
00876     goal.target_name = object;
00877     goal.group_name = opt_.group_name_;
00878     goal.end_effector = getEndEffector();
00879     goal.allowed_planning_time = planning_time_;
00880     goal.support_surface_name = support_surface_;
00881     goal.planner_id = planner_id_;
00882     if(!support_surface_.empty())
00883       goal.allow_gripper_support_collision = true;
00884 
00885     if (path_constraints_)
00886       goal.path_constraints = *path_constraints_;
00887 
00888     goal_out = goal;
00889   }
00890 
00891   void constructGoal(moveit_msgs::PlaceGoal &goal_out, const std::string &object)
00892   {
00893     moveit_msgs::PlaceGoal goal;
00894     goal.attached_object_name = object;
00895     goal.group_name = opt_.group_name_;
00896     goal.allowed_planning_time = planning_time_;
00897     goal.support_surface_name = support_surface_;
00898     goal.planner_id = planner_id_;
00899     if(!support_surface_.empty())
00900       goal.allow_gripper_support_collision = true;
00901 
00902     if (path_constraints_)
00903       goal.path_constraints = *path_constraints_;
00904 
00905     goal_out = goal;
00906   }
00907 
00908   void setPathConstraints(const moveit_msgs::Constraints &constraint)
00909   {
00910     path_constraints_.reset(new moveit_msgs::Constraints(constraint));
00911   }
00912 
00913   bool setPathConstraints(const std::string &constraint)
00914   {
00915     if (constraints_storage_)
00916     {
00917       moveit_warehouse::ConstraintsWithMetadata msg_m;
00918       if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
00919       {
00920         path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
00921     return true;
00922       }
00923       else
00924         return false;
00925     }
00926     else
00927       return false;
00928   }
00929 
00930   void clearPathConstraints()
00931   {
00932     path_constraints_.reset();
00933   }
00934 
00935   std::vector<std::string> getKnownConstraints() const
00936   {
00937     while (initializing_constraints_)
00938     {
00939       static ros::WallDuration d(0.01);
00940       d.sleep();
00941     }
00942 
00943     std::vector<std::string> c;
00944     if (constraints_storage_)
00945       constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
00946 
00947     return c;
00948   }
00949 
00950   moveit_msgs::Constraints getPathConstraints() const
00951   {
00952     if (path_constraints_)
00953        return *path_constraints_;
00954     else
00955        return moveit_msgs::Constraints();
00956   }
00957 
00958   void initializeConstraintsStorage(const std::string &host, unsigned int port)
00959   {
00960     initializing_constraints_ = true;
00961     if (constraints_init_thread_)
00962       constraints_init_thread_->join();
00963     constraints_init_thread_.reset(new boost::thread(boost::bind(&MoveGroupImpl::initializeConstraintsStorageThread, this, host, port)));
00964   }
00965 
00966   void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
00967   {
00968     workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
00969     workspace_parameters_.header.stamp = ros::Time::now();
00970     workspace_parameters_.min_corner.x = minx;
00971     workspace_parameters_.min_corner.y = miny;
00972     workspace_parameters_.min_corner.z = minz;
00973     workspace_parameters_.max_corner.x = maxx;
00974     workspace_parameters_.max_corner.y = maxy;
00975     workspace_parameters_.max_corner.z = maxz;
00976   }
00977 
00978 private:
00979 
00980   void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
00981   {
00982     try
00983     {
00984       constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(host, port));
00985       ROS_DEBUG("Connected to constraints database");
00986     }
00987     catch(std::runtime_error &ex)
00988     {
00989       ROS_DEBUG("%s", ex.what());
00990     }
00991     initializing_constraints_ = false;
00992   }
00993 
00994   Options opt_;
00995   ros::NodeHandle node_handle_;
00996   boost::shared_ptr<tf::Transformer> tf_;
00997   robot_model::RobotModelConstPtr robot_model_;
00998   planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
00999   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
01000   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
01001   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
01002 
01003   // general planning params
01004   robot_state::RobotStatePtr considered_start_state_;
01005   moveit_msgs::WorkspaceParameters workspace_parameters_;
01006   double planning_time_;
01007   std::string planner_id_;
01008   unsigned int num_planning_attempts_;
01009   double goal_joint_tolerance_;
01010   double goal_position_tolerance_;
01011   double goal_orientation_tolerance_;
01012   bool can_look_;
01013   bool can_replan_;
01014   double replan_delay_;
01015 
01016   // joint state goal
01017   robot_state::RobotStatePtr joint_state_target_;
01018   const robot_model::JointModelGroup *joint_model_group_;
01019   
01020   // pose goal;
01021   // for each link we have a set of possible goal locations;
01022   std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
01023 
01024   // common properties for goals
01025   ActiveTargetType active_target_;
01026   boost::scoped_ptr<moveit_msgs::Constraints> path_constraints_;
01027   std::string end_effector_link_;
01028   std::string pose_reference_frame_;
01029   std::string support_surface_;
01030 
01031   // ROS communication
01032   ros::Publisher trajectory_event_publisher_;
01033   ros::Publisher attached_object_publisher_;
01034   ros::ServiceClient execute_service_;
01035   ros::ServiceClient query_service_;
01036   ros::ServiceClient cartesian_path_service_;
01037   boost::scoped_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
01038   boost::scoped_ptr<boost::thread> constraints_init_thread_;
01039   bool initializing_constraints_;
01040 };
01041 }
01042 }
01043 
01044 moveit::planning_interface::MoveGroup::MoveGroup(const std::string &group_name, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
01045 {
01046   if (!ros::ok())
01047     throw std::runtime_error("ROS does not seem to be running");
01048   impl_ = new MoveGroupImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_server);
01049 }
01050 
01051 moveit::planning_interface::MoveGroup::MoveGroup(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf, const ros::Duration &wait_for_server)
01052 {
01053   impl_ = new MoveGroupImpl(opt, tf ? tf : getSharedTF(), wait_for_server);
01054 }
01055 
01056 moveit::planning_interface::MoveGroup::~MoveGroup()
01057 {
01058   delete impl_;
01059 }
01060 
01061 const std::string& moveit::planning_interface::MoveGroup::getName() const
01062 {
01063   return impl_->getOptions().group_name_;
01064 }
01065 
01066 bool moveit::planning_interface::MoveGroup::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
01067 {
01068   return impl_->getInterfaceDescription(desc);
01069 }
01070 
01071 void moveit::planning_interface::MoveGroup::setPlannerId(const std::string &planner_id)
01072 {
01073   impl_->setPlannerId(planner_id);
01074 }
01075 
01076 void moveit::planning_interface::MoveGroup::setNumPlanningAttempts(unsigned int num_planning_attempts)
01077 {
01078   impl_->setNumPlanningAttempts(num_planning_attempts);
01079 }
01080 
01081 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncMove()
01082 {
01083   return impl_->move(false);
01084 }
01085 
01086 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::move()
01087 {
01088   return impl_->move(true);
01089 }
01090 
01091 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncExecute(const Plan &plan)
01092 {
01093   return impl_->execute(plan, false);
01094 }
01095 
01096 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::execute(const Plan &plan)
01097 {
01098   return impl_->execute(plan, true);
01099 }
01100 
01101 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::plan(Plan &plan)
01102 {
01103   return impl_->plan(plan);
01104 }
01105 
01106 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string &object)
01107 {
01108   return impl_->pick(object, std::vector<moveit_msgs::Grasp>());
01109 }
01110 
01111 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string &object, const moveit_msgs::Grasp &grasp)
01112 {
01113   return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp));
01114 }
01115 
01116 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::pick(const std::string &object, const std::vector<moveit_msgs::Grasp> &grasps)
01117 {
01118   return impl_->pick(object, grasps);
01119 }
01120 
01121 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object)
01122 {
01123   return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>());
01124 }
01125 
01126 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object, const std::vector<moveit_msgs::PlaceLocation> &locations)
01127 {
01128   return impl_->place(object, locations);
01129 }
01130 
01131 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object, const std::vector<geometry_msgs::PoseStamped> &poses)
01132 {
01133   return impl_->place(object, poses);
01134 }
01135 
01136 moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::place(const std::string &object, const geometry_msgs::PoseStamped &pose)
01137 {
01138   return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose));
01139 }
01140 
01141 double moveit::planning_interface::MoveGroup::computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double eef_step, double jump_threshold,
01142                                                                    moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions, 
01143                                                                    moveit_msgs::MoveItErrorCodes *error_code)
01144 {
01145   if(error_code)
01146   {
01147     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, avoid_collisions, *error_code);
01148   }
01149   else
01150   {
01151     moveit_msgs::MoveItErrorCodes error_code_tmp;
01152     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, avoid_collisions, error_code_tmp);
01153   }  
01154 }
01155 
01156 void moveit::planning_interface::MoveGroup::stop()
01157 {
01158   impl_->stop();
01159 }
01160 
01161 void moveit::planning_interface::MoveGroup::setStartState(const moveit_msgs::RobotState &start_state)
01162 {
01163   robot_state::RobotStatePtr rs;
01164   impl_->getCurrentState(rs);
01165   robot_state::robotStateMsgToRobotState(start_state, *rs);
01166   setStartState(*rs);
01167 }
01168 
01169 void moveit::planning_interface::MoveGroup::setStartState(const robot_state::RobotState &start_state)
01170 {
01171   impl_->setStartState(start_state);
01172 }
01173 
01174 void moveit::planning_interface::MoveGroup::setStartStateToCurrentState()
01175 {
01176   impl_->setStartStateToCurrentState();
01177 }
01178 
01179 void moveit::planning_interface::MoveGroup::setRandomTarget()
01180 {
01181   impl_->getJointStateTarget().setToRandomPositions();
01182   impl_->setTargetType(JOINT);
01183 }
01184 
01185 bool moveit::planning_interface::MoveGroup::setNamedTarget(const std::string &name)
01186 {
01187   std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
01188   if (it != remembered_joint_values_.end())
01189   {
01190     return setJointValueTarget(it->second);
01191   }
01192   else
01193   {
01194     if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
01195     {
01196       impl_->setTargetType(JOINT);
01197       return true;
01198     }
01199     return false;
01200   }
01201 }
01202 
01203 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::vector<double> &joint_values)
01204 {
01205   if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
01206     return false;
01207   impl_->setTargetType(JOINT);
01208   impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
01209   return impl_->getJointStateTarget().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance());
01210 }
01211 
01212 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::map<std::string, double> &joint_values)
01213 {
01214   impl_->setTargetType(JOINT);
01215   impl_->getJointStateTarget().setVariablePositions(joint_values);
01216   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01217 }
01218 
01219 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const robot_state::RobotState &rstate)
01220 {
01221   impl_->setTargetType(JOINT);
01222   impl_->getJointStateTarget() = rstate;
01223   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01224 }
01225 
01226 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string &joint_name, double value)
01227 {
01228   std::vector<double> values(1, value);
01229   return setJointValueTarget(joint_name, values);
01230 }
01231 
01232 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const std::string &joint_name, const std::vector<double> &values)
01233 {
01234   impl_->setTargetType(JOINT);
01235   const robot_model::JointModel *jm = impl_->getJointStateTarget().getJointModel(joint_name);
01236   if (jm && jm->getVariableCount() == values.size())
01237   {
01238     impl_->getJointStateTarget().setJointPositions(jm, values);
01239     return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
01240   }
01241   return false;
01242 }
01243 
01244 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const sensor_msgs::JointState &state)
01245 {
01246   impl_->setTargetType(JOINT);
01247   impl_->getJointStateTarget().setVariableValues(state);
01248   return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
01249 }
01250 
01251 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link)
01252 {
01253   return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
01254 }
01255 
01256 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link)
01257 {
01258   return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
01259 }
01260 
01261 bool moveit::planning_interface::MoveGroup::setJointValueTarget(const Eigen::Affine3d &eef_pose, const std::string &end_effector_link)
01262 { 
01263   geometry_msgs::Pose msg;
01264   tf::poseEigenToMsg(eef_pose, msg);
01265   return setJointValueTarget(msg, end_effector_link);
01266 }
01267 
01268 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link)
01269 {
01270   return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
01271 }
01272 
01273 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link)
01274 {
01275   return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
01276 }
01277 
01278 bool moveit::planning_interface::MoveGroup::setApproximateJointValueTarget(const Eigen::Affine3d &eef_pose, const std::string &end_effector_link)
01279 { 
01280   geometry_msgs::Pose msg;
01281   tf::poseEigenToMsg(eef_pose, msg);
01282   return setApproximateJointValueTarget(msg, end_effector_link);
01283 }
01284 
01285 const robot_state::RobotState& moveit::planning_interface::MoveGroup::getJointValueTarget() const
01286 {
01287   return impl_->getJointStateTarget();
01288 }
01289 
01290 const std::string& moveit::planning_interface::MoveGroup::getEndEffectorLink() const
01291 {
01292   return impl_->getEndEffectorLink();
01293 }
01294 
01295 const std::string& moveit::planning_interface::MoveGroup::getEndEffector() const
01296 {
01297   return impl_->getEndEffector();
01298 }
01299 
01300 bool moveit::planning_interface::MoveGroup::setEndEffectorLink(const std::string &link_name)
01301 {
01302   if (impl_->getEndEffectorLink().empty() || link_name.empty())
01303     return false;
01304   impl_->setEndEffectorLink(link_name);
01305   impl_->setTargetType(POSE);
01306   return true;
01307 }
01308 
01309 bool moveit::planning_interface::MoveGroup::setEndEffector(const std::string &eef_name)
01310 {
01311   const robot_model::JointModelGroup *jmg = impl_->getRobotModel()->getEndEffector(eef_name);
01312   if (jmg)
01313     return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
01314   return false;
01315 }
01316 
01317 void moveit::planning_interface::MoveGroup::clearPoseTarget(const std::string &end_effector_link)
01318 {
01319   impl_->clearPoseTarget(end_effector_link);
01320 }
01321 
01322 void moveit::planning_interface::MoveGroup::clearPoseTargets()
01323 {
01324   impl_->clearPoseTargets();
01325 }
01326 
01327 bool moveit::planning_interface::MoveGroup::setPoseTarget(const Eigen::Affine3d &pose, const std::string &end_effector_link)
01328 {
01329   std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01330   tf::poseEigenToMsg(pose, pose_msg[0].pose);
01331   pose_msg[0].header.frame_id = getPoseReferenceFrame();
01332   pose_msg[0].header.stamp = ros::Time::now();
01333   return setPoseTargets(pose_msg, end_effector_link);
01334 }
01335 
01336 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::Pose &target, const std::string &end_effector_link)
01337 {
01338   std::vector<geometry_msgs::PoseStamped> pose_msg(1);
01339   pose_msg[0].pose = target;
01340   pose_msg[0].header.frame_id = getPoseReferenceFrame();
01341   pose_msg[0].header.stamp = ros::Time::now();
01342   return setPoseTargets(pose_msg, end_effector_link);
01343 }
01344 
01345 bool moveit::planning_interface::MoveGroup::setPoseTarget(const geometry_msgs::PoseStamped &target, const std::string &end_effector_link)
01346 {
01347   std::vector<geometry_msgs::PoseStamped> targets(1, target);
01348   return setPoseTargets(targets, end_effector_link);
01349 }
01350 
01351 bool moveit::planning_interface::MoveGroup::setPoseTargets(const EigenSTL::vector_Affine3d &target, const std::string &end_effector_link)
01352 {
01353   std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
01354   ros::Time tm = ros::Time::now();
01355   const std::string &frame_id = getPoseReferenceFrame();
01356   for (std::size_t i = 0 ; i < target.size() ; ++i)
01357   {
01358     tf::poseEigenToMsg(target[i], pose_out[i].pose);
01359     pose_out[i].header.stamp = tm;
01360     pose_out[i].header.frame_id = frame_id;
01361   }
01362   return setPoseTargets(pose_out, end_effector_link);
01363 }
01364 
01365 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::Pose> &target, const std::string &end_effector_link)
01366 {
01367   std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
01368   ros::Time tm = ros::Time::now();
01369   const std::string &frame_id = getPoseReferenceFrame();
01370   for (std::size_t i = 0 ; i < target.size() ; ++i)
01371   {
01372     target_stamped[i].pose = target[i];
01373     target_stamped[i].header.stamp = tm;
01374     target_stamped[i].header.frame_id = frame_id;
01375   }
01376   return setPoseTargets(target_stamped, end_effector_link);
01377 }
01378 
01379 bool moveit::planning_interface::MoveGroup::setPoseTargets(const std::vector<geometry_msgs::PoseStamped> &target, const std::string &end_effector_link)
01380 {
01381   if (target.empty())
01382   {
01383     ROS_ERROR("No pose specified as goal target");
01384     return false;
01385   }
01386   else
01387   {
01388     impl_->setTargetType(POSE);
01389     return impl_->setPoseTargets(target, end_effector_link);
01390   }
01391 }
01392 
01393 const geometry_msgs::PoseStamped& moveit::planning_interface::MoveGroup::getPoseTarget(const std::string &end_effector_link) const
01394 {
01395   return impl_->getPoseTarget(end_effector_link);
01396 }
01397 
01398 const std::vector<geometry_msgs::PoseStamped>& moveit::planning_interface::MoveGroup::getPoseTargets(const std::string &end_effector_link) const
01399 {
01400   return impl_->getPoseTargets(end_effector_link);
01401 }
01402 
01403 namespace
01404 {
01405 inline void transformPose(const tf::Transformer& tf, const std::string &desired_frame, geometry_msgs::PoseStamped &target)
01406 {
01407   if (desired_frame != target.header.frame_id)
01408   {
01409     tf::Pose pose;
01410     tf::poseMsgToTF(target.pose, pose);
01411     tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
01412     tf::Stamped<tf::Pose> stamped_target_out;
01413     tf.transformPose(desired_frame, stamped_target, stamped_target_out);
01414     target.header.frame_id = stamped_target_out.frame_id_;
01415     //    target.header.stamp = stamped_target_out.stamp_; // we leave the stamp to ros::Time(0) on purpose
01416     tf::poseTFToMsg(stamped_target_out, target.pose);
01417   }
01418 }
01419 }
01420 
01421 bool moveit::planning_interface::MoveGroup::setPositionTarget(double x, double y, double z, const std::string &end_effector_link)
01422 {
01423   geometry_msgs::PoseStamped target;
01424   if (impl_->hasPoseTarget(end_effector_link))
01425   {
01426     target = getPoseTarget(end_effector_link);
01427     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01428   }
01429   else
01430   {
01431     target.pose.orientation.x = 0.0;
01432     target.pose.orientation.y = 0.0;
01433     target.pose.orientation.z = 0.0;
01434     target.pose.orientation.w = 1.0;
01435     target.header.frame_id = impl_->getPoseReferenceFrame();
01436   }
01437 
01438   target.pose.position.x = x;
01439   target.pose.position.y = y;
01440   target.pose.position.z = z;
01441   bool result = setPoseTarget(target, end_effector_link);
01442   impl_->setTargetType(POSITION);
01443   return result;
01444 }
01445 
01446 bool moveit::planning_interface::MoveGroup::setRPYTarget(double r, double p, double y, const std::string &end_effector_link)
01447 {
01448   geometry_msgs::PoseStamped target;
01449   if (impl_->hasPoseTarget(end_effector_link))
01450   {
01451     target = getPoseTarget(end_effector_link);
01452     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01453   }
01454   else
01455   {
01456     target.pose.position.x = 0.0;
01457     target.pose.position.y = 0.0;
01458     target.pose.position.z = 0.0;
01459     target.header.frame_id = impl_->getPoseReferenceFrame();
01460   }
01461 
01462   tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
01463   bool result = setPoseTarget(target, end_effector_link);
01464   impl_->setTargetType(ORIENTATION);
01465   return result;
01466 }
01467 
01468 bool moveit::planning_interface::MoveGroup::setOrientationTarget(double x, double y, double z, double w, const std::string &end_effector_link)
01469 {
01470   geometry_msgs::PoseStamped target;
01471   if (impl_->hasPoseTarget(end_effector_link))
01472   {
01473     target = getPoseTarget(end_effector_link);
01474     transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
01475   }
01476   else
01477   {
01478     target.pose.position.x = 0.0;
01479     target.pose.position.y = 0.0;
01480     target.pose.position.z = 0.0;
01481     target.header.frame_id = impl_->getPoseReferenceFrame();
01482   }
01483 
01484   target.pose.orientation.x = x;
01485   target.pose.orientation.y = y;
01486   target.pose.orientation.z = z;
01487   target.pose.orientation.w = w;
01488   bool result = setPoseTarget(target, end_effector_link);
01489   impl_->setTargetType(ORIENTATION);
01490   return result;
01491 }
01492 
01493 void moveit::planning_interface::MoveGroup::setPoseReferenceFrame(const std::string &pose_reference_frame)
01494 {
01495   impl_->setPoseReferenceFrame(pose_reference_frame);
01496 }
01497 
01498 const std::string& moveit::planning_interface::MoveGroup::getPoseReferenceFrame() const
01499 {
01500   return impl_->getPoseReferenceFrame();
01501 }
01502 
01503 double moveit::planning_interface::MoveGroup::getGoalJointTolerance() const
01504 {
01505   return impl_->getGoalJointTolerance();
01506 }
01507 
01508 double moveit::planning_interface::MoveGroup::getGoalPositionTolerance() const
01509 {
01510   return impl_->getGoalPositionTolerance();
01511 }
01512 
01513 double moveit::planning_interface::MoveGroup::getGoalOrientationTolerance() const
01514 {
01515   return impl_->getGoalOrientationTolerance();
01516 }
01517 
01518 void moveit::planning_interface::MoveGroup::setGoalTolerance(double tolerance)
01519 {
01520   setGoalJointTolerance(tolerance);
01521   setGoalPositionTolerance(tolerance);
01522   setGoalOrientationTolerance(tolerance);
01523 }
01524 
01525 void moveit::planning_interface::MoveGroup::setGoalJointTolerance(double tolerance)
01526 {
01527   impl_->setGoalJointTolerance(tolerance);
01528 }
01529 
01530 void moveit::planning_interface::MoveGroup::setGoalPositionTolerance(double tolerance)
01531 {
01532   impl_->setGoalPositionTolerance(tolerance);
01533 }
01534 
01535 void moveit::planning_interface::MoveGroup::setGoalOrientationTolerance(double tolerance)
01536 {
01537   impl_->setGoalOrientationTolerance(tolerance);
01538 }
01539 
01540 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string &name)
01541 {
01542   rememberJointValues(name, getCurrentJointValues());
01543 }
01544 
01545 bool moveit::planning_interface::MoveGroup::startStateMonitor(double wait)
01546 {
01547   return impl_->startStateMonitor(wait);
01548 }
01549 
01550 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentJointValues()
01551 {
01552   robot_state::RobotStatePtr current_state;
01553   std::vector<double> values;
01554   if (impl_->getCurrentState(current_state))
01555     current_state->copyJointGroupPositions(getName(), values);
01556   return values;
01557 }
01558 
01559 std::vector<double> moveit::planning_interface::MoveGroup::getRandomJointValues()
01560 {
01561   std::vector<double> r;
01562   impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
01563   return r;
01564 }
01565 
01566 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getRandomPose(const std::string &end_effector_link)
01567 {
01568   const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01569   Eigen::Affine3d pose;
01570   pose.setIdentity();
01571   if (eef.empty())
01572     ROS_ERROR("No end-effector specified");
01573   else
01574   {
01575     robot_state::RobotStatePtr current_state;
01576     if (impl_->getCurrentState(current_state))
01577     {
01578       current_state->setToRandomPositions(impl_->getJointModelGroup());
01579       const robot_model::LinkModel *lm = current_state->getLinkModel(eef);
01580       if (lm)
01581         pose = current_state->getGlobalLinkTransform(lm);
01582     }
01583   }
01584   geometry_msgs::PoseStamped pose_msg;
01585   pose_msg.header.stamp = ros::Time::now();
01586   pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01587   tf::poseEigenToMsg(pose, pose_msg.pose);
01588   return pose_msg;
01589 }
01590 
01591 geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getCurrentPose(const std::string &end_effector_link)
01592 {
01593   const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01594   Eigen::Affine3d pose;
01595   pose.setIdentity();
01596   if (eef.empty())
01597     ROS_ERROR("No end-effector specified");
01598   else
01599   {
01600     robot_state::RobotStatePtr current_state;
01601     if (impl_->getCurrentState(current_state))
01602     {
01603       const robot_model::LinkModel *lm = current_state->getLinkModel(eef);
01604       if (lm)
01605         pose = current_state->getGlobalLinkTransform(lm);
01606     }
01607   }
01608   geometry_msgs::PoseStamped pose_msg;
01609   pose_msg.header.stamp = ros::Time::now();
01610   pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
01611   tf::poseEigenToMsg(pose, pose_msg.pose);
01612   return pose_msg;
01613 }
01614 
01615 std::vector<double> moveit::planning_interface::MoveGroup::getCurrentRPY(const std::string &end_effector_link)
01616 {
01617   std::vector<double> result;
01618   const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
01619   if (eef.empty())
01620     ROS_ERROR("No end-effector specified");
01621   else
01622   {
01623     robot_state::RobotStatePtr current_state;
01624     if (impl_->getCurrentState(current_state))
01625     {
01626       const robot_model::LinkModel *lm = current_state->getLinkModel(eef);
01627       if (lm)
01628       {
01629         result.resize(3);
01630         tf::Matrix3x3 ptf;
01631         tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
01632         tfScalar pitch, roll, yaw;
01633         ptf.getRPY(roll, pitch, yaw);
01634         result[0] = roll;
01635         result[1] = pitch;
01636         result[2] = yaw;
01637       }
01638     }
01639   }
01640   return result;
01641 }
01642 
01643 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getActiveJoints() const
01644 {
01645   return impl_->getJointModelGroup()->getActiveJointModelNames();
01646 }
01647 
01648 const std::vector<std::string>& moveit::planning_interface::MoveGroup::getJoints() const
01649 {
01650   return impl_->getJointModelGroup()->getJointModelNames();
01651 }
01652 
01653 unsigned int moveit::planning_interface::MoveGroup::getVariableCount() const
01654 {
01655   return impl_->getJointModelGroup()->getVariableCount();
01656 }
01657 
01658 robot_state::RobotStatePtr moveit::planning_interface::MoveGroup::getCurrentState()
01659 {
01660   robot_state::RobotStatePtr current_state;
01661   impl_->getCurrentState(current_state);
01662   return current_state;
01663 }
01664 
01665 void moveit::planning_interface::MoveGroup::rememberJointValues(const std::string &name, const std::vector<double> &values)
01666 {
01667   remembered_joint_values_[name] = values;
01668 }
01669 
01670 void moveit::planning_interface::MoveGroup::forgetJointValues(const std::string &name)
01671 {
01672   remembered_joint_values_.erase(name);
01673 }
01674 
01675 void moveit::planning_interface::MoveGroup::allowLooking(bool flag)
01676 {
01677   impl_->allowLooking(flag);
01678 }
01679 
01680 void moveit::planning_interface::MoveGroup::allowReplanning(bool flag)
01681 {
01682   impl_->allowReplanning(flag);
01683 }
01684 
01685 std::vector<std::string> moveit::planning_interface::MoveGroup::getKnownConstraints() const
01686 {
01687   return impl_->getKnownConstraints();
01688 }
01689 
01690 moveit_msgs::Constraints moveit::planning_interface::MoveGroup::getPathConstraints() const 
01691 {
01692    return impl_->getPathConstraints();
01693 }
01694 
01695 bool moveit::planning_interface::MoveGroup::setPathConstraints(const std::string &constraint)
01696 {
01697   return impl_->setPathConstraints(constraint);
01698 }
01699 
01700 void moveit::planning_interface::MoveGroup::setPathConstraints(const moveit_msgs::Constraints &constraint)
01701 {
01702   impl_->setPathConstraints(constraint);
01703 }
01704 
01705 void moveit::planning_interface::MoveGroup::clearPathConstraints()
01706 {
01707   impl_->clearPathConstraints();
01708 }
01709 
01710 void moveit::planning_interface::MoveGroup::setConstraintsDatabase(const std::string &host, unsigned int port)
01711 {
01712   impl_->initializeConstraintsStorage(host, port);
01713 }
01714 
01715 void moveit::planning_interface::MoveGroup::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
01716 {
01717   impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
01718 }
01719 
01720 void moveit::planning_interface::MoveGroup::setPlanningTime(double seconds)
01721 {
01722   impl_->setPlanningTime(seconds);
01723 }
01724 
01725 double moveit::planning_interface::MoveGroup::getPlanningTime() const
01726 {
01727   return impl_->getPlanningTime();
01728 }
01729 
01730 void moveit::planning_interface::MoveGroup::setSupportSurfaceName(const std::string &name)
01731 {
01732   impl_->setSupportSurfaceName(name);
01733 }
01734 
01735 const std::string& moveit::planning_interface::MoveGroup::getPlanningFrame() const
01736 {
01737   return impl_->getRobotModel()->getModelFrame();
01738 }
01739 
01740 bool moveit::planning_interface::MoveGroup::attachObject(const std::string &object, const std::string &link)
01741 {   
01742   return attachObject(object, link, std::vector<std::string>());
01743 }
01744 
01745 bool moveit::planning_interface::MoveGroup::attachObject(const std::string &object, const std::string &link, const std::vector<std::string> &touch_links)
01746 {
01747   return impl_->attachObject(object, link, touch_links);
01748 }
01749 
01750 bool moveit::planning_interface::MoveGroup::detachObject(const std::string &name)
01751 {
01752   return impl_->detachObject(name);
01753 }


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 12:44:13