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