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