move_group_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, SRI International
5  * Copyright (c) 2013, Ioan A. Sucan
6  * Copyright (c) 2012, Willow Garage, Inc.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Ioan Sucan, Sachin Chitta */
38 
39 #include <stdexcept>
40 #include <sstream>
41 #include <memory>
53 #include <moveit_msgs/PickupAction.h>
54 #include <moveit_msgs/ExecuteTrajectoryAction.h>
55 #include <moveit_msgs/PlaceAction.h>
56 #include <moveit_msgs/ExecuteKnownTrajectory.h>
57 #include <moveit_msgs/QueryPlannerInterfaces.h>
58 #include <moveit_msgs/GetCartesianPath.h>
59 #include <moveit_msgs/GraspPlanning.h>
60 #include <moveit_msgs/GetPlannerParams.h>
61 #include <moveit_msgs/SetPlannerParams.h>
62 
63 #include <std_msgs/String.h>
64 #include <geometry_msgs/TransformStamped.h>
65 #include <tf2/utils.h>
66 #include <tf2_eigen/tf2_eigen.h>
68 #include <ros/console.h>
69 #include <ros/ros.h>
70 
71 namespace moveit
72 {
73 namespace planning_interface
74 {
75 const std::string MoveGroupInterface::ROBOT_DESCRIPTION =
76  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
77 
78 const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps
79 
80 namespace
81 {
83 {
84  JOINT,
85  POSE,
86  POSITION,
87  ORIENTATION
88 };
89 }
90 
92 {
93 public:
94  MoveGroupInterfaceImpl(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
95  const ros::WallDuration& wait_for_servers)
96  : opt_(opt), node_handle_(opt.node_handle_), tf_buffer_(tf_buffer)
97  {
99  if (!getRobotModel())
100  {
101  std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
102  "parameter server.";
103  ROS_FATAL_STREAM_NAMED("move_group_interface", error);
104  throw std::runtime_error(error);
105  }
106 
107  if (!getRobotModel()->hasJointModelGroup(opt.group_name_))
108  {
109  std::string error = "Group '" + opt.group_name_ + "' was not found.";
110  ROS_FATAL_STREAM_NAMED("move_group_interface", error);
111  throw std::runtime_error(error);
112  }
113 
114  joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
115 
116  joint_state_target_.reset(new robot_state::RobotState(getRobotModel()));
117  joint_state_target_->setToDefaultValues();
118  active_target_ = JOINT;
119  can_look_ = false;
120  can_replan_ = false;
121  replan_delay_ = 2.0;
122  goal_joint_tolerance_ = 1e-4;
123  goal_position_tolerance_ = 1e-4; // 0.1 mm
124  goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
127  node_handle_.param<double>("robot_description_planning/default_velocity_scaling_factor",
129  node_handle_.param<double>("robot_description_planning/default_acceleration_scaling_factor",
132 
133  if (joint_model_group_->isChain())
134  end_effector_link_ = joint_model_group_->getLinkModelNames().back();
135  pose_reference_frame_ = getRobotModel()->getModelFrame();
136 
139  attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(
141 
143 
144  ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers;
145  if (wait_for_servers == ros::WallDuration())
146  timeout_for_servers = ros::WallTime(); // wait for ever
147  double allotted_time = wait_for_servers.toSec();
148 
149  move_action_client_.reset(
151  waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time);
152 
153  pick_action_client_.reset(
155  waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time);
156 
157  place_action_client_.reset(
159  waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time);
160 
163  waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time);
164 
166  node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
171 
174 
176 
177  ROS_INFO_STREAM_NAMED("move_group_interface",
178  "Ready to take commands for planning group " << opt.group_name_ << ".");
179  }
180 
181  template <typename T>
182  void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time)
183  {
184  ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str());
185 
186  // wait for the server (and spin as needed)
187  if (timeout == ros::WallTime()) // wait forever
188  {
189  while (node_handle_.ok() && !action->isServerConnected())
190  {
191  ros::WallDuration(0.001).sleep();
192  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
194  if (queue)
195  {
196  queue->callAvailable();
197  }
198  else // in case of nodelets and specific callback queue implementations
199  {
200  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
201  "handling.");
202  }
203  }
204  }
205  else // wait with timeout
206  {
207  while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
208  {
209  ros::WallDuration(0.001).sleep();
210  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
212  if (queue)
213  {
214  queue->callAvailable();
215  }
216  else // in case of nodelets and specific callback queue implementations
217  {
218  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
219  "handling.");
220  }
221  }
222  }
223 
224  if (!action->isServerConnected())
225  {
226  std::stringstream error;
227  error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
228  << "s)";
229  throw std::runtime_error(error.str());
230  }
231  else
232  {
233  ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str());
234  }
235  }
236 
238  {
240  constraints_init_thread_->join();
241  }
242 
243  const std::shared_ptr<tf2_ros::Buffer>& getTF() const
244  {
245  return tf_buffer_;
246  }
247 
248  const Options& getOptions() const
249  {
250  return opt_;
251  }
252 
253  const robot_model::RobotModelConstPtr& getRobotModel() const
254  {
255  return robot_model_;
256  }
257 
258  const robot_model::JointModelGroup* getJointModelGroup() const
259  {
260  return joint_model_group_;
261  }
262 
264  {
265  return *move_action_client_;
266  }
267 
268  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
269  {
270  moveit_msgs::QueryPlannerInterfaces::Request req;
271  moveit_msgs::QueryPlannerInterfaces::Response res;
272  if (query_service_.call(req, res))
273  if (!res.planner_interfaces.empty())
274  {
275  desc = res.planner_interfaces.front();
276  return true;
277  }
278  return false;
279  }
280 
281  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
282  {
283  moveit_msgs::GetPlannerParams::Request req;
284  moveit_msgs::GetPlannerParams::Response res;
285  req.planner_config = planner_id;
286  req.group = group;
287  std::map<std::string, std::string> result;
288  if (get_params_service_.call(req, res))
289  {
290  for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
291  result[res.params.keys[i]] = res.params.values[i];
292  }
293  return result;
294  }
295 
296  void setPlannerParams(const std::string& planner_id, const std::string& group,
297  const std::map<std::string, std::string>& params, bool replace = false)
298  {
299  moveit_msgs::SetPlannerParams::Request req;
300  moveit_msgs::SetPlannerParams::Response res;
301  req.planner_config = planner_id;
302  req.group = group;
303  req.replace = replace;
304  for (std::map<std::string, std::string>::const_iterator it = params.begin(), end = params.end(); it != end; ++it)
305  {
306  req.params.keys.push_back(it->first);
307  req.params.values.push_back(it->second);
308  }
309  set_params_service_.call(req, res);
310  }
311 
312  std::string getDefaultPlannerId(const std::string& group) const
313  {
314  std::stringstream param_name;
315  param_name << "move_group";
316  if (!group.empty())
317  param_name << "/" << group;
318  param_name << "/default_planner_config";
319 
320  std::string default_planner_config;
321  node_handle_.getParam(param_name.str(), default_planner_config);
322  return default_planner_config;
323  }
324 
325  void setPlannerId(const std::string& planner_id)
326  {
327  planner_id_ = planner_id;
328  }
329 
330  const std::string& getPlannerId() const
331  {
332  return planner_id_;
333  }
334 
335  void setNumPlanningAttempts(unsigned int num_planning_attempts)
336  {
337  num_planning_attempts_ = num_planning_attempts;
338  }
339 
340  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
341  {
342  max_velocity_scaling_factor_ = max_velocity_scaling_factor;
343  }
344 
345  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
346  {
347  max_acceleration_scaling_factor_ = max_acceleration_scaling_factor;
348  }
349 
350  robot_state::RobotState& getJointStateTarget()
351  {
352  return *joint_state_target_;
353  }
354 
355  void setStartState(const robot_state::RobotState& start_state)
356  {
357  considered_start_state_.reset(new robot_state::RobotState(start_state));
358  }
359 
361  {
362  considered_start_state_.reset();
363  }
364 
365  robot_state::RobotStatePtr getStartState()
366  {
369  else
370  {
371  robot_state::RobotStatePtr s;
372  getCurrentState(s);
373  return s;
374  }
375  }
376 
377  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
378  const std::string& frame, bool approx)
379  {
380  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
381  // this only works if we have an end-effector
382  if (!eef.empty())
383  {
384  // first we set the goal to be the same as the start state
385  moveit::core::RobotStatePtr c = getStartState();
386  if (c)
387  {
388  setTargetType(JOINT);
389  c->enforceBounds();
390  getJointStateTarget() = *c;
391  if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
392  return false;
393  }
394  else
395  return false;
396 
397  // we may need to do approximate IK
399  o.return_approximate_solution = approx;
400 
401  // if no frame transforms are needed, call IK directly
402  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
403  return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0.0,
405  else
406  {
407  if (c->knowsFrameTransform(frame))
408  {
409  // transform the pose first if possible, then do IK
410  const Eigen::Isometry3d& t = getJointStateTarget().getFrameTransform(frame);
411  Eigen::Isometry3d p;
412  tf2::fromMsg(eef_pose, p);
413  return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0.0,
415  }
416  else
417  {
418  ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
419  getRobotModel()->getModelFrame().c_str());
420  return false;
421  }
422  }
423  }
424  else
425  return false;
426  }
427 
428  void setEndEffectorLink(const std::string& end_effector)
429  {
430  end_effector_link_ = end_effector;
431  }
432 
433  void clearPoseTarget(const std::string& end_effector_link)
434  {
435  pose_targets_.erase(end_effector_link);
436  }
437 
439  {
440  pose_targets_.clear();
441  }
442 
443  const std::string& getEndEffectorLink() const
444  {
445  return end_effector_link_;
446  }
447 
448  const std::string& getEndEffector() const
449  {
450  if (!end_effector_link_.empty())
451  {
452  const std::vector<std::string>& possible_eefs =
453  getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
454  for (std::size_t i = 0; i < possible_eefs.size(); ++i)
455  if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
456  return possible_eefs[i];
457  }
458  static std::string empty;
459  return empty;
460  }
461 
462  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
463  {
464  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
465  if (eef.empty())
466  {
467  ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for");
468  return false;
469  }
470  else
471  {
472  pose_targets_[eef] = poses;
473  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
474  std::vector<geometry_msgs::PoseStamped>& stored_poses = pose_targets_[eef];
475  for (std::size_t i = 0; i < stored_poses.size(); ++i)
476  stored_poses[i].header.stamp = ros::Time(0);
477  }
478  return true;
479  }
480 
481  bool hasPoseTarget(const std::string& end_effector_link) const
482  {
483  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
484  return pose_targets_.find(eef) != pose_targets_.end();
485  }
486 
487  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
488  {
489  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
490 
491  // if multiple pose targets are set, return the first one
492  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
493  if (jt != pose_targets_.end())
494  if (!jt->second.empty())
495  return jt->second.at(0);
496 
497  // or return an error
498  static const geometry_msgs::PoseStamped UNKNOWN;
499  ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str());
500  return UNKNOWN;
501  }
502 
503  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
504  {
505  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
506 
507  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
508  if (jt != pose_targets_.end())
509  if (!jt->second.empty())
510  return jt->second;
511 
512  // or return an error
513  static const std::vector<geometry_msgs::PoseStamped> EMPTY;
514  ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str());
515  return EMPTY;
516  }
517 
518  void setPoseReferenceFrame(const std::string& pose_reference_frame)
519  {
520  pose_reference_frame_ = pose_reference_frame;
521  }
522 
523  void setSupportSurfaceName(const std::string& support_surface)
524  {
525  support_surface_ = support_surface;
526  }
527 
528  const std::string& getPoseReferenceFrame() const
529  {
530  return pose_reference_frame_;
531  }
532 
533  void setTargetType(ActiveTargetType type)
534  {
535  active_target_ = type;
536  }
537 
538  ActiveTargetType getTargetType() const
539  {
540  return active_target_;
541  }
542 
543  bool startStateMonitor(double wait)
544  {
546  {
547  ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state");
548  return false;
549  }
550 
551  // if needed, start the monitor and wait up to 1 second for a full robot state
552  if (!current_state_monitor_->isActive())
553  current_state_monitor_->startStateMonitor();
554 
555  current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
556  return true;
557  }
558 
559  bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0)
560  {
562  {
563  ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state");
564  return false;
565  }
566 
567  // if needed, start the monitor and wait up to 1 second for a full robot state
568  if (!current_state_monitor_->isActive())
569  current_state_monitor_->startStateMonitor();
570 
571  if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds))
572  {
573  ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state");
574  return false;
575  }
576 
577  current_state = current_state_monitor_->getCurrentState();
578  return true;
579  }
580 
582  std::vector<moveit_msgs::PlaceLocation> posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses)
583  {
584  std::vector<moveit_msgs::PlaceLocation> locations;
585  for (std::size_t i = 0; i < poses.size(); ++i)
586  {
587  moveit_msgs::PlaceLocation location;
588  location.pre_place_approach.direction.vector.z = -1.0;
589  location.post_place_retreat.direction.vector.x = -1.0;
590  location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
591  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
592 
593  location.pre_place_approach.min_distance = 0.1;
594  location.pre_place_approach.desired_distance = 0.2;
595  location.post_place_retreat.min_distance = 0.0;
596  location.post_place_retreat.desired_distance = 0.2;
597  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
598 
599  location.place_pose = poses[i];
600  locations.push_back(location);
601  }
602  ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
603  (unsigned int)locations.size());
604  return locations;
605  }
606 
607  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
608  {
610  {
611  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
612  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
613  }
614  if (!place_action_client_->isServerConnected())
615  {
616  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
617  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
618  }
619 
620  place_action_client_->sendGoal(goal);
621  ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
622  if (!place_action_client_->waitForResult())
623  {
624  ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
625  }
627  {
628  return MoveItErrorCode(place_action_client_->getResult()->error_code);
629  }
630  else
631  {
632  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
633  << place_action_client_->getState().getText());
634  return MoveItErrorCode(place_action_client_->getResult()->error_code);
635  }
636  }
637 
638  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
639  {
640  if (!pick_action_client_)
641  {
642  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
643  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
644  }
645  if (!pick_action_client_->isServerConnected())
646  {
647  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
648  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
649  }
650 
651  pick_action_client_->sendGoal(goal);
652  if (!pick_action_client_->waitForResult())
653  {
654  ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
655  }
657  {
658  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
659  }
660  else
661  {
662  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
663  << pick_action_client_->getState().getText());
664  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
665  }
666  }
667 
668  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
669  {
670  if (object.empty())
671  {
672  return planGraspsAndPick(moveit_msgs::CollisionObject());
673  }
674 
676  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
677 
678  if (objects.empty())
679  {
680  ROS_ERROR_STREAM_NAMED("move_group_interface",
681  "Asked for grasps for the object '" << object << "', but the object could not be found");
682  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
683  }
684 
685  return planGraspsAndPick(objects[object], plan_only);
686  }
687 
688  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
689  {
691  {
692  ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
693  << GRASP_PLANNING_SERVICE_NAME
694  << "' is not available."
695  " This has to be implemented and started separately.");
696  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
697  }
698 
699  moveit_msgs::GraspPlanning::Request request;
700  moveit_msgs::GraspPlanning::Response response;
701 
702  request.group_name = opt_.group_name_;
703  request.target = object;
704  request.support_surfaces.push_back(support_surface_);
705 
706  ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
707  if (!plan_grasps_service_.call(request, response) ||
708  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
709  {
710  ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
711  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
712  }
713 
714  return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only));
715  }
716 
718  {
719  if (!move_action_client_)
720  {
721  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
722  }
723  if (!move_action_client_->isServerConnected())
724  {
725  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
726  }
727 
728  moveit_msgs::MoveGroupGoal goal;
729  constructGoal(goal);
730  goal.planning_options.plan_only = true;
731  goal.planning_options.look_around = false;
732  goal.planning_options.replan = false;
733  goal.planning_options.planning_scene_diff.is_diff = true;
734  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
735 
736  move_action_client_->sendGoal(goal);
737  if (!move_action_client_->waitForResult())
738  {
739  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
740  }
742  {
743  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
744  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
745  plan.planning_time_ = move_action_client_->getResult()->planning_time;
746  return MoveItErrorCode(move_action_client_->getResult()->error_code);
747  }
748  else
749  {
750  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
751  << move_action_client_->getState().getText());
752  return MoveItErrorCode(move_action_client_->getResult()->error_code);
753  }
754  }
755 
757  {
758  if (!move_action_client_)
759  {
760  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
761  }
762  if (!move_action_client_->isServerConnected())
763  {
764  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
765  }
766 
767  moveit_msgs::MoveGroupGoal goal;
768  constructGoal(goal);
769  goal.planning_options.plan_only = false;
770  goal.planning_options.look_around = can_look_;
771  goal.planning_options.replan = can_replan_;
772  goal.planning_options.replan_delay = replan_delay_;
773  goal.planning_options.planning_scene_diff.is_diff = true;
774  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
775 
776  move_action_client_->sendGoal(goal);
777  if (!wait)
778  {
779  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
780  }
781 
782  if (!move_action_client_->waitForResult())
783  {
784  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
785  }
786 
788  {
789  return MoveItErrorCode(move_action_client_->getResult()->error_code);
790  }
791  else
792  {
793  ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
794  << ": " << move_action_client_->getState().getText());
795  return MoveItErrorCode(move_action_client_->getResult()->error_code);
796  }
797  }
798 
799  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
800  {
802  {
803  ROS_ERROR_STREAM_NAMED("move_group_interface", "execute action client not found");
804  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
805  }
806  if (!execute_action_client_->isServerConnected())
807  {
808  ROS_WARN_STREAM_NAMED("move_group_interface", "execute action server not connected");
809  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
810  }
811 
812  moveit_msgs::ExecuteTrajectoryGoal goal;
813  goal.trajectory = trajectory;
814 
815  execute_action_client_->sendGoal(goal);
816  if (!wait)
817  {
818  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
819  }
820 
821  if (!execute_action_client_->waitForResult())
822  {
823  ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early");
824  }
825 
827  {
828  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
829  }
830  else
831  {
832  ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString()
833  << ": " << execute_action_client_->getState().getText());
834  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
835  }
836  }
837 
838  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
839  moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
840  bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
841  {
842  moveit_msgs::GetCartesianPath::Request req;
843  moveit_msgs::GetCartesianPath::Response res;
844 
846  robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
847  else
848  req.start_state.is_diff = true;
849 
850  req.group_name = opt_.group_name_;
851  req.header.frame_id = getPoseReferenceFrame();
852  req.header.stamp = ros::Time::now();
853  req.waypoints = waypoints;
854  req.max_step = step;
855  req.jump_threshold = jump_threshold;
856  req.path_constraints = path_constraints;
857  req.avoid_collisions = avoid_collisions;
858  req.link_name = getEndEffectorLink();
859 
860  if (cartesian_path_service_.call(req, res))
861  {
862  error_code = res.error_code;
863  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
864  {
865  msg = res.solution;
866  return res.fraction;
867  }
868  else
869  return -1.0;
870  }
871  else
872  {
873  error_code.val = error_code.FAILURE;
874  return -1.0;
875  }
876  }
877 
878  void stop()
879  {
881  {
882  std_msgs::String event;
883  event.data = "stop";
885  }
886  }
887 
888  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
889  {
890  std::string l = link.empty() ? getEndEffectorLink() : link;
891  if (l.empty())
892  {
893  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
894  if (!links.empty())
895  l = links[0];
896  }
897  if (l.empty())
898  {
899  ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
900  return false;
901  }
902  moveit_msgs::AttachedCollisionObject aco;
903  aco.object.id = object;
904  aco.link_name.swap(l);
905  if (touch_links.empty())
906  aco.touch_links.push_back(aco.link_name);
907  else
908  aco.touch_links = touch_links;
909  aco.object.operation = moveit_msgs::CollisionObject::ADD;
911  return true;
912  }
913 
914  bool detachObject(const std::string& name)
915  {
916  moveit_msgs::AttachedCollisionObject aco;
917  // if name is a link
918  if (!name.empty() && joint_model_group_->hasLinkModel(name))
919  aco.link_name = name;
920  else
921  aco.object.id = name;
922  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
923  if (aco.link_name.empty() && aco.object.id.empty())
924  {
925  // we only want to detach objects for this group
926  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
927  for (std::size_t i = 0; i < lnames.size(); ++i)
928  {
929  aco.link_name = lnames[i];
931  }
932  }
933  else
935  return true;
936  }
937 
939  {
941  }
942 
944  {
946  }
947 
948  double getGoalJointTolerance() const
949  {
950  return goal_joint_tolerance_;
951  }
952 
953  void setGoalJointTolerance(double tolerance)
954  {
955  goal_joint_tolerance_ = tolerance;
956  }
957 
958  void setGoalPositionTolerance(double tolerance)
959  {
960  goal_position_tolerance_ = tolerance;
961  }
962 
963  void setGoalOrientationTolerance(double tolerance)
964  {
965  goal_orientation_tolerance_ = tolerance;
966  }
967 
968  void setPlanningTime(double seconds)
969  {
970  if (seconds > 0.0)
971  allowed_planning_time_ = seconds;
972  }
973 
974  double getPlanningTime() const
975  {
976  return allowed_planning_time_;
977  }
978 
979  void allowLooking(bool flag)
980  {
981  can_look_ = flag;
982  ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
983  }
984 
985  void allowReplanning(bool flag)
986  {
987  can_replan_ = flag;
988  ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
989  }
990 
991  void setReplanningDelay(double delay)
992  {
993  if (delay >= 0.0)
994  replan_delay_ = delay;
995  }
996 
997  double getReplanningDelay() const
998  {
999  return replan_delay_;
1000  }
1001 
1002  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request)
1003  {
1004  request.group_name = opt_.group_name_;
1005  request.num_planning_attempts = num_planning_attempts_;
1006  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1007  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1008  request.allowed_planning_time = allowed_planning_time_;
1009  request.planner_id = planner_id_;
1010  request.workspace_parameters = workspace_parameters_;
1011 
1013  robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
1014  else
1015  request.start_state.is_diff = true;
1016 
1017  if (active_target_ == JOINT)
1018  {
1019  request.goal_constraints.resize(1);
1020  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1022  }
1023  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1024  {
1025  // find out how many goals are specified
1026  std::size_t goal_count = 0;
1027  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1028  it != pose_targets_.end(); ++it)
1029  goal_count = std::max(goal_count, it->second.size());
1030 
1031  // start filling the goals;
1032  // each end effector has a number of possible poses (K) as valid goals
1033  // but there could be multiple end effectors specified, so we want each end effector
1034  // to reach the goal that corresponds to the goals of the other end effectors
1035  request.goal_constraints.resize(goal_count);
1036 
1037  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1038  it != pose_targets_.end(); ++it)
1039  {
1040  for (std::size_t i = 0; i < it->second.size(); ++i)
1041  {
1042  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1043  it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1044  if (active_target_ == ORIENTATION)
1045  c.position_constraints.clear();
1046  if (active_target_ == POSITION)
1047  c.orientation_constraints.clear();
1048  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1049  }
1050  }
1051  }
1052  else
1053  ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation");
1054 
1055  if (path_constraints_)
1056  request.path_constraints = *path_constraints_;
1058  request.trajectory_constraints = *trajectory_constraints_;
1059  }
1060 
1061  void constructGoal(moveit_msgs::MoveGroupGoal& goal)
1062  {
1063  constructMotionPlanRequest(goal.request);
1064  }
1065 
1066  moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector<moveit_msgs::Grasp>&& grasps,
1067  bool plan_only = false)
1068  {
1069  moveit_msgs::PickupGoal goal;
1070  goal.target_name = object;
1071  goal.group_name = opt_.group_name_;
1072  goal.end_effector = getEndEffector();
1073  goal.support_surface_name = support_surface_;
1074  goal.possible_grasps = std::move(grasps);
1075  if (!support_surface_.empty())
1076  goal.allow_gripper_support_collision = true;
1077 
1078  if (path_constraints_)
1079  goal.path_constraints = *path_constraints_;
1080 
1081  goal.planner_id = planner_id_;
1082  goal.allowed_planning_time = allowed_planning_time_;
1083 
1084  goal.planning_options.plan_only = plan_only;
1085  goal.planning_options.look_around = can_look_;
1086  goal.planning_options.replan = can_replan_;
1087  goal.planning_options.replan_delay = replan_delay_;
1088  goal.planning_options.planning_scene_diff.is_diff = true;
1089  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1090  return goal;
1091  }
1092 
1093  moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object,
1094  std::vector<moveit_msgs::PlaceLocation>&& locations, bool plan_only = false)
1095  {
1096  moveit_msgs::PlaceGoal goal;
1097  goal.group_name = opt_.group_name_;
1098  goal.attached_object_name = object;
1099  goal.support_surface_name = support_surface_;
1100  goal.place_locations = std::move(locations);
1101  if (!support_surface_.empty())
1102  goal.allow_gripper_support_collision = true;
1103 
1104  if (path_constraints_)
1105  goal.path_constraints = *path_constraints_;
1106 
1107  goal.planner_id = planner_id_;
1108  goal.allowed_planning_time = allowed_planning_time_;
1109 
1110  goal.planning_options.plan_only = plan_only;
1111  goal.planning_options.look_around = can_look_;
1112  goal.planning_options.replan = can_replan_;
1113  goal.planning_options.replan_delay = replan_delay_;
1114  goal.planning_options.planning_scene_diff.is_diff = true;
1115  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1116  return goal;
1117  }
1118 
1119  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1120  {
1121  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1122  }
1123 
1124  bool setPathConstraints(const std::string& constraint)
1125  {
1127  {
1129  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1130  {
1131  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1132  return true;
1133  }
1134  else
1135  return false;
1136  }
1137  else
1138  return false;
1139  }
1140 
1142  {
1143  path_constraints_.reset();
1144  }
1145 
1146  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
1147  {
1148  trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint));
1149  }
1150 
1152  {
1153  trajectory_constraints_.reset();
1154  }
1155 
1156  std::vector<std::string> getKnownConstraints() const
1157  {
1159  {
1160  static ros::WallDuration d(0.01);
1161  d.sleep();
1162  }
1163 
1164  std::vector<std::string> c;
1166  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1167 
1168  return c;
1169  }
1170 
1171  moveit_msgs::Constraints getPathConstraints() const
1172  {
1173  if (path_constraints_)
1174  return *path_constraints_;
1175  else
1176  return moveit_msgs::Constraints();
1177  }
1178 
1179  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
1180  {
1182  return *trajectory_constraints_;
1183  else
1184  return moveit_msgs::TrajectoryConstraints();
1185  }
1186 
1187  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1188  {
1191  constraints_init_thread_->join();
1193  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1194  }
1195 
1196  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1197  {
1198  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1199  workspace_parameters_.header.stamp = ros::Time::now();
1200  workspace_parameters_.min_corner.x = minx;
1201  workspace_parameters_.min_corner.y = miny;
1202  workspace_parameters_.min_corner.z = minz;
1203  workspace_parameters_.max_corner.x = maxx;
1204  workspace_parameters_.max_corner.y = maxy;
1205  workspace_parameters_.max_corner.z = maxz;
1206  }
1207 
1208 private:
1209  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1210  {
1211  // Set up db
1212  try
1213  {
1215  conn->setParams(host, port);
1216  if (conn->connect())
1217  {
1219  }
1220  }
1221  catch (std::exception& ex)
1222  {
1223  ROS_ERROR_NAMED("move_group_interface", "%s", ex.what());
1224  }
1225  initializing_constraints_ = false;
1226  }
1227 
1230  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
1231  robot_model::RobotModelConstPtr robot_model_;
1232  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1233  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1234  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1235  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1236  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1237 
1238  // general planning params
1239  robot_state::RobotStatePtr considered_start_state_;
1240  moveit_msgs::WorkspaceParameters workspace_parameters_;
1242  std::string planner_id_;
1252 
1253  // joint state goal
1254  robot_state::RobotStatePtr joint_state_target_;
1255  const robot_model::JointModelGroup* joint_model_group_;
1256 
1257  // pose goal;
1258  // for each link we have a set of possible goal locations;
1259  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1260 
1261  // common properties for goals
1262  ActiveTargetType active_target_;
1263  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1264  std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
1265  std::string end_effector_link_;
1267  std::string support_surface_;
1268 
1269  // ROS communication
1277  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1278  std::unique_ptr<boost::thread> constraints_init_thread_;
1280 };
1281 } // namespace planning_interface
1282 } // namespace moveit
1283 
1285  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1286  const ros::WallDuration& wait_for_servers)
1287 {
1288  if (!ros::ok())
1289  throw std::runtime_error("ROS does not seem to be running");
1290  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1291 }
1292 
1294  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1295  const ros::Duration& wait_for_servers)
1296  : MoveGroupInterface(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1297 {
1298 }
1299 
1301  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1302  const ros::WallDuration& wait_for_servers)
1303 {
1304  impl_ = new MoveGroupInterfaceImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1305 }
1306 
1309  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const ros::Duration& wait_for_servers)
1310  : MoveGroupInterface(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1311 {
1312 }
1313 
1315 {
1316  delete impl_;
1317 }
1318 
1321 {
1322  other.impl_ = nullptr;
1323 }
1324 
1327 {
1328  if (this != &other)
1329  {
1330  delete impl_;
1331  impl_ = other.impl_;
1332  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1333  other.impl_ = nullptr;
1334  }
1335 
1336  return *this;
1337 }
1338 
1340 {
1341  return impl_->getOptions().group_name_;
1342 }
1343 
1345 {
1346  const robot_model::RobotModelConstPtr& robot = getRobotModel();
1347  const std::string& group = getName();
1348  const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1349 
1350  if (joint_group)
1351  {
1352  return joint_group->getDefaultStateNames();
1353  }
1354 
1355  std::vector<std::string> empty;
1356  return empty;
1357 }
1358 
1360 {
1361  return impl_->getRobotModel();
1362 }
1363 
1365 {
1366  return impl_->getOptions().node_handle_;
1367 }
1368 
1370  moveit_msgs::PlannerInterfaceDescription& desc)
1371 {
1372  return impl_->getInterfaceDescription(desc);
1373 }
1374 
1375 std::map<std::string, std::string>
1376 moveit::planning_interface::MoveGroupInterface::getPlannerParams(const std::string& planner_id, const std::string& group)
1377 {
1378  return impl_->getPlannerParams(planner_id, group);
1379 }
1380 
1382  const std::string& group,
1383  const std::map<std::string, std::string>& params,
1384  bool replace)
1385 {
1386  impl_->setPlannerParams(planner_id, group, params, replace);
1387 }
1388 
1390 {
1391  return impl_->getDefaultPlannerId(group);
1392 }
1393 
1395 {
1396  impl_->setPlannerId(planner_id);
1397 }
1398 
1400 {
1401  return impl_->getPlannerId();
1402 }
1403 
1405 {
1406  impl_->setNumPlanningAttempts(num_planning_attempts);
1407 }
1408 
1410 {
1411  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1412 }
1413 
1415  double max_acceleration_scaling_factor)
1416 {
1417  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1418 }
1419 
1421 {
1422  return impl_->move(false);
1423 }
1424 
1427 {
1428  return impl_->getMoveGroupClient();
1429 }
1430 
1432 {
1433  return impl_->move(true);
1434 }
1435 
1438 {
1439  return impl_->execute(plan.trajectory_, false);
1440 }
1441 
1443 moveit::planning_interface::MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory)
1444 {
1445  return impl_->execute(trajectory, false);
1446 }
1447 
1449 {
1450  return impl_->execute(plan.trajectory_, true);
1451 }
1452 
1454 moveit::planning_interface::MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory)
1455 {
1456  return impl_->execute(trajectory, true);
1457 }
1458 
1460 {
1461  return impl_->plan(plan);
1462 }
1463 
1465  const std::string& object, std::vector<moveit_msgs::Grasp> grasps, bool plan_only = false)
1466 {
1467  return impl_->constructPickupGoal(object, std::move(grasps), plan_only);
1468 }
1469 
1471  const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations, bool plan_only = false)
1472 {
1473  return impl_->constructPlaceGoal(object, std::move(locations), plan_only);
1474 }
1475 
1477  const std::vector<geometry_msgs::PoseStamped>& poses)
1478 {
1479  return impl_->posesToPlaceLocations(poses);
1480 }
1481 
1483 moveit::planning_interface::MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
1484 {
1485  return impl_->pick(goal);
1486 }
1487 
1490 {
1491  return impl_->planGraspsAndPick(object, plan_only);
1492 }
1493 
1496  bool plan_only)
1497 {
1498  return impl_->planGraspsAndPick(object, plan_only);
1499 }
1500 
1503 {
1504  return impl_->place(goal);
1505 }
1506 
1508  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1509  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1510 {
1511  moveit_msgs::Constraints path_constraints_tmp;
1512  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1513  error_code);
1514 }
1515 
1517  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1518  moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1519  moveit_msgs::MoveItErrorCodes* error_code)
1520 {
1521  if (error_code)
1522  {
1523  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1524  avoid_collisions, *error_code);
1525  }
1526  else
1527  {
1528  moveit_msgs::MoveItErrorCodes error_code_tmp;
1529  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1530  avoid_collisions, error_code_tmp);
1531  }
1532 }
1533 
1535 {
1536  impl_->stop();
1537 }
1538 
1539 void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1540 {
1541  robot_state::RobotStatePtr rs;
1542  impl_->getCurrentState(rs);
1543  robot_state::robotStateMsgToRobotState(start_state, *rs);
1544  setStartState(*rs);
1545 }
1546 
1547 void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state)
1548 {
1549  impl_->setStartState(start_state);
1550 }
1551 
1553 {
1555 }
1556 
1558 {
1559  impl_->getJointStateTarget().setToRandomPositions();
1560  impl_->setTargetType(JOINT);
1561 }
1562 
1564 {
1565  return impl_->getJointModelGroup()->getVariableNames();
1566 }
1567 
1569 {
1570  return impl_->getJointModelGroup()->getLinkModelNames();
1571 }
1572 
1573 std::map<std::string, double>
1575 {
1576  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1577  std::map<std::string, double> positions;
1578 
1579  if (it != remembered_joint_values_.end())
1580  {
1581  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1582  for (size_t x = 0; x < names.size(); ++x)
1583  {
1584  positions[names[x]] = it->second[x];
1585  }
1586  }
1587  else
1588  {
1589  impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
1590  }
1591  return positions;
1592 }
1593 
1595 {
1596  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1597  if (it != remembered_joint_values_.end())
1598  {
1599  return setJointValueTarget(it->second);
1600  }
1601  else
1602  {
1603  if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
1604  {
1605  impl_->setTargetType(JOINT);
1606  return true;
1607  }
1608  ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
1609  return false;
1610  }
1611 }
1612 
1613 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1614 {
1615  auto const n_group_joints = impl_->getJointModelGroup()->getVariableCount();
1616  if (joint_values.size() != n_group_joints)
1617  {
1618  ROS_DEBUG_STREAM("Provided joint value list has length " << joint_values.size() << " but group "
1619  << impl_->getJointModelGroup()->getName() << " has "
1620  << n_group_joints << " joints");
1621  return false;
1622  }
1623  impl_->setTargetType(JOINT);
1624  impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1626 }
1627 
1629  const std::map<std::string, double>& joint_values)
1630 {
1631  impl_->setTargetType(JOINT);
1632  impl_->getJointStateTarget().setVariablePositions(joint_values);
1633  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1634 }
1635 
1637 {
1638  impl_->setTargetType(JOINT);
1639  impl_->getJointStateTarget() = rstate;
1640  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1641 }
1642 
1643 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1644 {
1645  std::vector<double> values(1, value);
1646  return setJointValueTarget(joint_name, values);
1647 }
1648 
1650  const std::vector<double>& values)
1651 {
1652  impl_->setTargetType(JOINT);
1653  const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
1654  if (jm && jm->getVariableCount() == values.size())
1655  {
1656  impl_->getJointStateTarget().setJointPositions(jm, values);
1657  return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1658  }
1659  return false;
1660 }
1661 
1663 {
1664  impl_->setTargetType(JOINT);
1665  impl_->getJointStateTarget().setVariableValues(state);
1666  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1667 }
1668 
1670  const std::string& end_effector_link)
1671 {
1672  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1673 }
1674 
1675 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1676  const std::string& end_effector_link)
1677 {
1678  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1679 }
1680 
1682  const std::string& end_effector_link)
1683 {
1684  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1685  return setJointValueTarget(msg, end_effector_link);
1686 }
1687 
1689  const std::string& end_effector_link)
1690 {
1691  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1692 }
1693 
1695  const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link)
1696 {
1697  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1698 }
1699 
1701  const std::string& end_effector_link)
1702 {
1703  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1704  return setApproximateJointValueTarget(msg, end_effector_link);
1705 }
1706 
1708 {
1709  return impl_->getJointStateTarget();
1710 }
1711 
1713 {
1714  return impl_->getEndEffectorLink();
1715 }
1716 
1718 {
1719  return impl_->getEndEffector();
1720 }
1721 
1723 {
1724  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1725  return false;
1726  impl_->setEndEffectorLink(link_name);
1727  impl_->setTargetType(POSE);
1728  return true;
1729 }
1730 
1732 {
1733  const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1734  if (jmg)
1735  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1736  return false;
1737 }
1738 
1740 {
1741  impl_->clearPoseTarget(end_effector_link);
1742 }
1743 
1745 {
1747 }
1748 
1750  const std::string& end_effector_link)
1751 {
1752  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1753  pose_msg[0].pose = tf2::toMsg(pose);
1754  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1755  pose_msg[0].header.stamp = ros::Time::now();
1756  return setPoseTargets(pose_msg, end_effector_link);
1757 }
1758 
1760  const std::string& end_effector_link)
1761 {
1762  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1763  pose_msg[0].pose = target;
1764  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1765  pose_msg[0].header.stamp = ros::Time::now();
1766  return setPoseTargets(pose_msg, end_effector_link);
1767 }
1768 
1769 bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target,
1770  const std::string& end_effector_link)
1771 {
1772  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1773  return setPoseTargets(targets, end_effector_link);
1774 }
1775 
1777  const std::string& end_effector_link)
1778 {
1779  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1780  ros::Time tm = ros::Time::now();
1781  const std::string& frame_id = getPoseReferenceFrame();
1782  for (std::size_t i = 0; i < target.size(); ++i)
1783  {
1784  pose_out[i].pose = tf2::toMsg(target[i]);
1785  pose_out[i].header.stamp = tm;
1786  pose_out[i].header.frame_id = frame_id;
1787  }
1788  return setPoseTargets(pose_out, end_effector_link);
1789 }
1790 
1791 bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1792  const std::string& end_effector_link)
1793 {
1794  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1795  ros::Time tm = ros::Time::now();
1796  const std::string& frame_id = getPoseReferenceFrame();
1797  for (std::size_t i = 0; i < target.size(); ++i)
1798  {
1799  target_stamped[i].pose = target[i];
1800  target_stamped[i].header.stamp = tm;
1801  target_stamped[i].header.frame_id = frame_id;
1802  }
1803  return setPoseTargets(target_stamped, end_effector_link);
1804 }
1805 
1807  const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link)
1808 {
1809  if (target.empty())
1810  {
1811  ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
1812  return false;
1813  }
1814  else
1815  {
1816  impl_->setTargetType(POSE);
1817  return impl_->setPoseTargets(target, end_effector_link);
1818  }
1819 }
1820 
1821 const geometry_msgs::PoseStamped&
1822 moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1823 {
1824  return impl_->getPoseTarget(end_effector_link);
1825 }
1826 
1827 const std::vector<geometry_msgs::PoseStamped>&
1828 moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1829 {
1830  return impl_->getPoseTargets(end_effector_link);
1831 }
1832 
1833 namespace
1834 {
1835 inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame,
1836  geometry_msgs::PoseStamped& target)
1837 {
1838  if (desired_frame != target.header.frame_id)
1839  {
1840  geometry_msgs::PoseStamped target_in(target);
1841  tf_buffer.transform(target_in, target, desired_frame);
1842  // we leave the stamp to ros::Time(0) on purpose
1843  target.header.stamp = ros::Time(0);
1844  }
1845 }
1846 } // namespace
1847 
1849  const std::string& end_effector_link)
1850 {
1851  geometry_msgs::PoseStamped target;
1852  if (impl_->hasPoseTarget(end_effector_link))
1853  {
1854  target = getPoseTarget(end_effector_link);
1855  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1856  }
1857  else
1858  {
1859  target.pose.orientation.x = 0.0;
1860  target.pose.orientation.y = 0.0;
1861  target.pose.orientation.z = 0.0;
1862  target.pose.orientation.w = 1.0;
1863  target.header.frame_id = impl_->getPoseReferenceFrame();
1864  }
1865 
1866  target.pose.position.x = x;
1867  target.pose.position.y = y;
1868  target.pose.position.z = z;
1869  bool result = setPoseTarget(target, end_effector_link);
1870  impl_->setTargetType(POSITION);
1871  return result;
1872 }
1873 
1875  const std::string& end_effector_link)
1876 {
1877  geometry_msgs::PoseStamped target;
1878  if (impl_->hasPoseTarget(end_effector_link))
1879  {
1880  target = getPoseTarget(end_effector_link);
1881  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1882  }
1883  else
1884  {
1885  target.pose.position.x = 0.0;
1886  target.pose.position.y = 0.0;
1887  target.pose.position.z = 0.0;
1888  target.header.frame_id = impl_->getPoseReferenceFrame();
1889  }
1891  q.setRPY(r, p, y);
1892  target.pose.orientation = tf2::toMsg(q);
1893  bool result = setPoseTarget(target, end_effector_link);
1894  impl_->setTargetType(ORIENTATION);
1895  return result;
1896 }
1897 
1899  const std::string& end_effector_link)
1900 {
1901  geometry_msgs::PoseStamped target;
1902  if (impl_->hasPoseTarget(end_effector_link))
1903  {
1904  target = getPoseTarget(end_effector_link);
1905  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1906  }
1907  else
1908  {
1909  target.pose.position.x = 0.0;
1910  target.pose.position.y = 0.0;
1911  target.pose.position.z = 0.0;
1912  target.header.frame_id = impl_->getPoseReferenceFrame();
1913  }
1914 
1915  target.pose.orientation.x = x;
1916  target.pose.orientation.y = y;
1917  target.pose.orientation.z = z;
1918  target.pose.orientation.w = w;
1919  bool result = setPoseTarget(target, end_effector_link);
1920  impl_->setTargetType(ORIENTATION);
1921  return result;
1922 }
1923 
1925 {
1926  impl_->setPoseReferenceFrame(pose_reference_frame);
1927 }
1928 
1930 {
1931  return impl_->getPoseReferenceFrame();
1932 }
1933 
1935 {
1936  return impl_->getGoalJointTolerance();
1937 }
1938 
1940 {
1941  return impl_->getGoalPositionTolerance();
1942 }
1943 
1945 {
1947 }
1948 
1950 {
1951  setGoalJointTolerance(tolerance);
1952  setGoalPositionTolerance(tolerance);
1953  setGoalOrientationTolerance(tolerance);
1954 }
1955 
1957 {
1958  impl_->setGoalJointTolerance(tolerance);
1959 }
1960 
1962 {
1963  impl_->setGoalPositionTolerance(tolerance);
1964 }
1965 
1967 {
1968  impl_->setGoalOrientationTolerance(tolerance);
1969 }
1970 
1972 {
1974 }
1975 
1977 {
1978  return impl_->startStateMonitor(wait);
1979 }
1980 
1982 {
1983  robot_state::RobotStatePtr current_state;
1984  std::vector<double> values;
1985  if (impl_->getCurrentState(current_state))
1986  current_state->copyJointGroupPositions(getName(), values);
1987  return values;
1988 }
1989 
1991 {
1992  std::vector<double> r;
1993  impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
1994  return r;
1995 }
1996 
1997 geometry_msgs::PoseStamped
1999 {
2000  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2001  Eigen::Isometry3d pose;
2002  pose.setIdentity();
2003  if (eef.empty())
2004  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2005  else
2006  {
2007  robot_state::RobotStatePtr current_state;
2008  if (impl_->getCurrentState(current_state))
2009  {
2010  current_state->setToRandomPositions(impl_->getJointModelGroup());
2011  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2012  if (lm)
2013  pose = current_state->getGlobalLinkTransform(lm);
2014  }
2015  }
2016  geometry_msgs::PoseStamped pose_msg;
2017  pose_msg.header.stamp = ros::Time::now();
2018  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2019  pose_msg.pose = tf2::toMsg(pose);
2020  return pose_msg;
2021 }
2022 
2023 geometry_msgs::PoseStamped
2025 {
2026  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2027  Eigen::Isometry3d pose;
2028  pose.setIdentity();
2029  if (eef.empty())
2030  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2031  else
2032  {
2033  robot_state::RobotStatePtr current_state;
2034  if (impl_->getCurrentState(current_state))
2035  {
2036  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2037  if (lm)
2038  pose = current_state->getGlobalLinkTransform(lm);
2039  }
2040  }
2041  geometry_msgs::PoseStamped pose_msg;
2042  pose_msg.header.stamp = ros::Time::now();
2043  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2044  pose_msg.pose = tf2::toMsg(pose);
2045  return pose_msg;
2046 }
2047 
2048 std::vector<double> moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link)
2049 {
2050  std::vector<double> result;
2051  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2052  if (eef.empty())
2053  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2054  else
2055  {
2056  robot_state::RobotStatePtr current_state;
2057  if (impl_->getCurrentState(current_state))
2058  {
2059  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2060  if (lm)
2061  {
2062  result.resize(3);
2063  geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2064  double pitch, roll, yaw;
2065  tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2066  result[0] = roll;
2067  result[1] = pitch;
2068  result[2] = yaw;
2069  }
2070  }
2071  }
2072  return result;
2073 }
2074 
2076 {
2077  return impl_->getJointModelGroup()->getActiveJointModelNames();
2078 }
2079 
2080 const std::vector<std::string>& moveit::planning_interface::MoveGroupInterface::getJoints() const
2081 {
2082  return impl_->getJointModelGroup()->getJointModelNames();
2083 }
2084 
2086 {
2087  return impl_->getJointModelGroup()->getVariableCount();
2088 }
2089 
2091 {
2092  robot_state::RobotStatePtr current_state;
2093  impl_->getCurrentState(current_state, wait);
2094  return current_state;
2095 }
2096 
2098  const std::vector<double>& values)
2099 {
2100  remembered_joint_values_[name] = values;
2101 }
2102 
2104 {
2105  remembered_joint_values_.erase(name);
2106 }
2107 
2109 {
2110  impl_->allowLooking(flag);
2111 }
2112 
2114 {
2115  impl_->allowReplanning(flag);
2116 }
2117 
2119 {
2120  return impl_->getKnownConstraints();
2121 }
2122 
2124 {
2125  return impl_->getPathConstraints();
2126 }
2127 
2129 {
2130  return impl_->setPathConstraints(constraint);
2131 }
2132 
2133 void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2134 {
2135  impl_->setPathConstraints(constraint);
2136 }
2137 
2139 {
2141 }
2142 
2144 {
2145  return impl_->getTrajectoryConstraints();
2146 }
2147 
2149  const moveit_msgs::TrajectoryConstraints& constraint)
2150 {
2151  impl_->setTrajectoryConstraints(constraint);
2152 }
2153 
2155 {
2157 }
2158 
2159 void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2160 {
2161  impl_->initializeConstraintsStorage(host, port);
2162 }
2163 
2164 void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx,
2165  double maxy, double maxz)
2166 {
2167  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2168 }
2169 
2172 {
2173  impl_->setPlanningTime(seconds);
2174 }
2175 
2178 {
2179  return impl_->getPlanningTime();
2180 }
2181 
2183 {
2185 }
2186 
2188 {
2189  return impl_->getRobotModel()->getModelFrame();
2190 }
2191 
2193 {
2194  return impl_->getRobotModel()->getJointModelGroupNames();
2195 }
2196 
2197 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2198 {
2199  return attachObject(object, link, std::vector<std::string>());
2200 }
2201 
2202 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2203  const std::vector<std::string>& touch_links)
2204 {
2205  return impl_->attachObject(object, link, touch_links);
2206 }
2207 
2209 {
2210  return impl_->detachObject(name);
2211 }
2212 
2214 {
2215  impl_->constructMotionPlanRequest(goal_out);
2216 }
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
bool startStateMonitor(double wait=1.0)
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically co...
d
Definition: setup.py:4
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool attachObject(const std::string &object, const std::string &link, const std::vector< std::string > &touch_links)
MoveItErrorCode asyncMove()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
bool detachObject(const std::string &name="")
Detach an object. name specifies the name of the object attached to this group, or the name of the li...
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
void setGoalJointTolerance(double tolerance)
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint va...
moveit_msgs::RobotTrajectory trajectory_
The trajectory of the robot (may not contain joints that are the same as for the start_state_) ...
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
#define ROS_INFO_NAMED(name,...)
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot&#39;s URDF. Set to &#39;robot_description&#39;.
q
void stop()
Stop any trajectory execution, if one is active.
bool setEndEffector(const std::string &eef_name)
Specify the name of the end-effector to use. This is equivalent to setting the EndEffectorLink to the...
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
void setRandomTarget()
Set the joint state goal to a random joint configuration.
#define ROS_ERROR_STREAM_NAMED(name, args)
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
static const std::string MOVE_ACTION
bool setPositionTarget(double x, double y, double z, const std::string &end_effector_link="")
Set the goal position of the end-effector end_effector_link to be (x, y, z).
const std::string & getEndEffectorLink() const
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly ...
MoveItErrorCode place(const std::string &object, bool plan_only=false)
Place an object somewhere safe in the world (a safe location will be detected)
const std::string & getPoseReferenceFrame() const
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the...
std::vector< double > values
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
void clearPoseTargets()
Forget any poses specified for all end-effectors.
MoveGroupInterfaceImpl(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, const ros::WallDuration &wait_for_servers)
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
double getGoalPositionTolerance() const
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where ...
bool setJointValueTarget(const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
std::map< std::string, std::vector< geometry_msgs::PoseStamped > > pose_targets_
const std::vector< std::string > & getLinkNames()
Get vector of names of links available in move group.
std::vector< moveit_msgs::PlaceLocation > posesToPlaceLocations(const std::vector< geometry_msgs::PoseStamped > &poses)
Convert a vector of PoseStamped to a vector of PlaceLocation.
std::vector< double > getCurrentJointValues()
Get the current joint values for the joints planned for by this instance (see getJoints()) ...
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > &&grasps, bool plan_only=false)
XmlRpcServer s
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > &&locations, bool plan_only=false)
std::map< std::string, std::vector< double > > remembered_joint_values_
std::vector< std::string > getKnownConstraints() const
Get the names of the known constraints as read from the Mongo database, if a connection was achieved...
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
void clearPathConstraints()
Specify that no path constraints are to be used. This removes any path constraints set in previous ca...
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest &request)
Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and sto...
bool call(MReq &req, MRes &res)
static bool sameFrame(const std::string &frame1, const std::string &frame2)
void setPoseReferenceFrame(const std::string &pose_reference_frame)
Specify which reference frame to assume for poses specified without a reference frame.
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > &params, bool bReplace=false)
Set the planner parameters for given group and planner_id.
double computeCartesianPath(const std::vector< geometry_msgs::Pose > &waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions=true, moveit_msgs::MoveItErrorCodes *error_code=NULL)
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters...
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the objects identified by the given object ids list. If no ids are provided, return all the known...
#define ROS_INFO_STREAM_NAMED(name, args)
const std::vector< std::string > getNamedTargets()
Get the names of the named robot states available as targets, both either remembered states or defaul...
bool setRPYTarget(double roll, double pitch, double yaw, const std::string &end_effector_link="")
Set the goal orientation of the end-effector end_effector_link to be (roll,pitch,yaw) radians...
const std::vector< std::string > & getJointModelGroupNames() const
Get the available planning group names.
void setPlannerId(const std::string &planner_id)
Specify a planner to be used for further planning.
MoveItErrorCode planGraspsAndPick(const std::string &object, bool plan_only=false)
void forgetJointValues(const std::string &name)
Forget the joint values remembered under name.
static const std::string PICKUP_ACTION
std::vector< double > getCurrentRPY(const std::string &end_effector_link="")
Get the roll-pitch-yaw (XYZ) for the end-effector end_effector_link. If end_effector_link is empty (t...
geometry_msgs::TransformStamped t
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
Specification of options to use when constructing the MoveGroupInterface class.
void setGoalOrientationTolerance(double tolerance)
Set the orientation tolerance that is used for reaching the goal when moving to a pose...
std::string robot_description_
The robot description parameter name (if different from default)
MoveItErrorCode execute(const moveit_msgs::RobotTrajectory &trajectory, bool wait)
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
robot_model::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
static const std::string CARTESIAN_PATH_SERVICE_NAME
const std::string GRASP_PLANNING_SERVICE_NAME
bool setEndEffectorLink(const std::string &end_effector_link)
Specify the parent link of the end-effector. This end_effector_link will be used in calls to pose tar...
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
void setGoalPositionTolerance(double tolerance)
Set the position tolerance that is used for reaching the goal when moving to a pose.
#define ROS_DEBUG_NAMED(name,...)
const std::string & getName() const
Get the name of the group this instance operates on.
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PlaceAction > > place_action_client_
unsigned int getVariableCount() const
Get the number of variables used to describe the state of this group. This is larger or equal to the ...
bool getCurrentState(robot_state::RobotStatePtr &current_state, double wait_seconds=1.0)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
bool attachObject(const std::string &object, const std::string &link="")
Given the name of an object in the planning scene, make the object attached to a link of the robot...
bool setPathConstraints(const std::string &constraint)
Specify a set of path constraints to use. The constraints are looked up by name from the Mongo databa...
MoveItErrorCode asyncExecute(const Plan &plan)
Given a plan, execute it without waiting for completion.
const std::string & getPlanningFrame() const
Get the name of the frame in which the robot is planning.
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
bool setApproximateJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelCons...
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
Get the description of the planning plugin loaded by the action server.
static const std::string PLACE_ACTION
std::unique_ptr< moveit_warehouse::ConstraintsStorage > constraints_storage_
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
void rememberJointValues(const std::string &name)
Remember the current joint values (of the robot being monitored) under name. These can be used by set...
#define ROS_FATAL_STREAM_NAMED(name, args)
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
bool getParam(const std::string &key, std::string &s) const
void setStartState(const moveit_msgs::RobotState &start_state)
If a different start state should be considered instead of the current state of the robot...
void initializeConstraintsStorage(const std::string &host, unsigned int port)
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::ExecuteTrajectoryAction > > execute_action_client_
geometry_msgs::PoseStamped getRandomPose(const std::string &end_effector_link="")
Get a random reachable pose for the end-effector end_effector_link. If end_effector_link is empty (th...
bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
robot_state::RobotStatePtr getCurrentState(double wait=1)
Get the current state of the robot within the duration specified by wait.
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
Get the planner parameters for given group and planner_id.
void fromMsg(const A &, B &b)
moveit_msgs::PickupGoal constructPickupGoal(const std::string &object, std::vector< moveit_msgs::Grasp > grasps, bool plan_only)
Build a PickupGoal for an object named object and store it in goal_out.
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0...
ROSCPP_DECL bool ok()
actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > & getMoveGroupClient() const
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying ...
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") const
void clearPoseTarget(const std::string &end_effector_link="")
Forget pose(s) specified for end_effector_link.
bool setOrientationTarget(double x, double y, double z, double w, const std::string &end_effector_link="")
Set the goal orientation of the end-effector end_effector_link to be the quaternion (x...
geometry_msgs::PoseStamped getCurrentPose(const std::string &end_effector_link="")
Get the pose for the end-effector end_effector_link. If end_effector_link is empty (the default value...
MoveItErrorCode move()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the speci...
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep() const
MoveItErrorCode execute(const Plan &plan)
Given a plan, execute it while waiting for completion.
MoveItErrorCode planGraspsAndPick(const std::string &object="", bool plan_only=false)
Pick up an object.
CallbackQueueInterface * getCallbackQueue() const
MoveItErrorCode plan(Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the ...
#define ROS_DEBUG_STREAM(args)
#define ROS_WARN_ONCE_NAMED(name,...)
r
bool setPoseTargets(const std::vector< geometry_msgs::PoseStamped > &poses, const std::string &end_effector_link)
static const std::string QUERY_PLANNERS_SERVICE_NAME
void setNumPlanningAttempts(unsigned int num_planning_attempts)
Set the number of times the motion plan is to be computed from scratch before the shortest solution i...
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::MoveGroupAction > > move_action_client_
Client class to conveniently use the ROS interfaces provided by the move_group node.
B toMsg(const A &a)
static const std::string EXECUTE_ACTION_NAME
void setGoalTolerance(double tolerance)
Set the tolerance that is used for reaching the goal. For joint state goals, this will be distance fo...
void setConstraintsDatabase(const std::string &host, unsigned int port)
Specify where the database server that holds known constraints resides.
robot_model::RobotModelConstPtr robot_model_
Optionally, an instance of the RobotModel to use can be also specified.
MoveGroupInterface(const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const ros::WallDuration &wait_for_servers=ros::WallDuration())
Construct a MoveGroupInterface instance call using a specified set of options opt.
tf2_ros::Buffer * tf_buffer
const std::vector< std::string > & getActiveJoints() const
Get only the active (actuated) joints this instance operates on.
moveit_msgs::RobotState start_state_
The full starting state used for planning.
res
static WallTime now()
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll...
std::vector< double > getRandomJointValues()
Get random joint values for the joints planned for by this instance (see getJoints()) ...
MoveGroupInterface & operator=(const MoveGroupInterface &)=delete
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
void allowReplanning(bool flag)
Specify whether the robot is allowed to replan if it detects changes in the environment.
void setSupportSurfaceName(const std::string &name)
For pick/place operations, the name of the support surface is used to specify the fact that attached ...
const std::vector< std::string > & getJointNames()
Get vector of names of joints available in move group.
#define ROS_ERROR_NAMED(name,...)
const std::vector< std::string > & getJoints() const
Get all the joints this instance operates on (including fixed joints)
std::vector< moveit_msgs::PlaceLocation > posesToPlaceLocations(const std::vector< geometry_msgs::PoseStamped > &poses)
Convert a vector of PoseStamped to a vector of PlaceLocation.
static Time now()
std::unique_ptr< moveit_msgs::TrajectoryConstraints > trajectory_constraints_
double getGoalJointTolerance() const
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configur...
robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
const std::string header
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
const std::string & getEndEffector() const
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by s...
void setPlannerParams(const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > &params, bool replace=false)
std::map< std::string, double > getNamedTargetValues(const std::string &name)
Get the joint angles for targets specified by name.
void waitForAction(const T &action, const std::string &name, const ros::WallTime &timeout, double allotted_time)
void allowLooking(bool flag)
Specify whether the robot is allowed to look around before moving if it determines it should (default...
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0...
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner of the current planning pipeline for the given group (or the pipeline&#39;s defau...
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string &object, std::vector< moveit_msgs::PlaceLocation > locations, bool plan_only)
Build a PlaceGoal for an object named object and store it in goal_out.
bool ok() const
bool setPoseTarget(const Eigen::Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set the goal pose of the end-effector end_effector_link.
double x
moveit_msgs::Constraints getPathConstraints() const
Get the actual set of constraints in use with this MoveGroupInterface.
std::string group_name_
The group to construct the class instance for.
double computeCartesianPath(const std::vector< geometry_msgs::Pose > &waypoints, double step, double jump_threshold, moveit_msgs::RobotTrajectory &msg, const moveit_msgs::Constraints &path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes &error_code)
double planning_time_
The amount of time it took to generate the plan.
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link) const
The representation of a motion plan (as ROS messasges)
bool setNamedTarget(const std::string &name)
Set the current joint values to be ones previously remembered by rememberJointValues() or...
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Specify the workspace bounding box. The box is specified in the planning frame (i.e. relative to the robot root link start position). This is useful when the planning group contains the root joint of the robot – i.e. when planning motion for the robot relative to the world.
const std::string & getPlannerId() const
Get the current planner_id.
void setStartStateToCurrentState()
Set the starting state for planning to be that reported by the robot&#39;s joint state publication...
const ros::NodeHandle & getNodeHandle() const
Get the ROS node handle of this instance operates on.
const std::string response
MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject &object, bool plan_only=false)
const robot_state::RobotState & getJointValueTarget() const
Get the currently set joint state goal.
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
#define ROS_WARN_STREAM_NAMED(name, args)
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PickupAction > > pick_action_client_
bool setPoseTargets(const EigenSTL::vector_Isometry3d &end_effector_pose, const std::string &end_effector_link="")
Set goal poses for end_effector_link.


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed May 5 2021 02:54:38