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
130 
131  if (joint_model_group_->isChain())
132  end_effector_link_ = joint_model_group_->getLinkModelNames().back();
133  pose_reference_frame_ = getRobotModel()->getModelFrame();
134 
137  attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(
139 
141 
142  ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers;
143  if (wait_for_servers == ros::WallDuration())
144  timeout_for_servers = ros::WallTime(); // wait for ever
145  double allotted_time = wait_for_servers.toSec();
146 
147  move_action_client_.reset(
149  waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time);
150 
151  pick_action_client_.reset(
153  waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time);
154 
155  place_action_client_.reset(
157  waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time);
158 
161  waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time);
162 
164  node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
169 
172 
174 
175  ROS_INFO_STREAM_NAMED("move_group_interface",
176  "Ready to take commands for planning group " << opt.group_name_ << ".");
177  }
178 
179  template <typename T>
180  void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time)
181  {
182  ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str());
183 
184  // wait for the server (and spin as needed)
185  if (timeout == ros::WallTime()) // wait forever
186  {
187  while (node_handle_.ok() && !action->isServerConnected())
188  {
189  ros::WallDuration(0.001).sleep();
190  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
192  if (queue)
193  {
194  queue->callAvailable();
195  }
196  else // in case of nodelets and specific callback queue implementations
197  {
198  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
199  "handling.");
200  }
201  }
202  }
203  else // wait with timeout
204  {
205  while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
206  {
207  ros::WallDuration(0.001).sleep();
208  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
210  if (queue)
211  {
212  queue->callAvailable();
213  }
214  else // in case of nodelets and specific callback queue implementations
215  {
216  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
217  "handling.");
218  }
219  }
220  }
221 
222  if (!action->isServerConnected())
223  {
224  std::stringstream error;
225  error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
226  << "s)";
227  throw std::runtime_error(error.str());
228  }
229  else
230  {
231  ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str());
232  }
233  }
234 
236  {
238  constraints_init_thread_->join();
239  }
240 
241  const std::shared_ptr<tf2_ros::Buffer>& getTF() const
242  {
243  return tf_buffer_;
244  }
245 
246  const Options& getOptions() const
247  {
248  return opt_;
249  }
250 
251  const robot_model::RobotModelConstPtr& getRobotModel() const
252  {
253  return robot_model_;
254  }
255 
256  const robot_model::JointModelGroup* getJointModelGroup() const
257  {
258  return joint_model_group_;
259  }
260 
262  {
263  return *move_action_client_;
264  }
265 
266  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
267  {
268  moveit_msgs::QueryPlannerInterfaces::Request req;
269  moveit_msgs::QueryPlannerInterfaces::Response res;
270  if (query_service_.call(req, res))
271  if (!res.planner_interfaces.empty())
272  {
273  desc = res.planner_interfaces.front();
274  return true;
275  }
276  return false;
277  }
278 
279  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
280  {
281  moveit_msgs::GetPlannerParams::Request req;
282  moveit_msgs::GetPlannerParams::Response res;
283  req.planner_config = planner_id;
284  req.group = group;
285  std::map<std::string, std::string> result;
286  if (get_params_service_.call(req, res))
287  {
288  for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
289  result[res.params.keys[i]] = res.params.values[i];
290  }
291  return result;
292  }
293 
294  void setPlannerParams(const std::string& planner_id, const std::string& group,
295  const std::map<std::string, std::string>& params, bool replace = false)
296  {
297  moveit_msgs::SetPlannerParams::Request req;
298  moveit_msgs::SetPlannerParams::Response res;
299  req.planner_config = planner_id;
300  req.group = group;
301  req.replace = replace;
302  for (std::map<std::string, std::string>::const_iterator it = params.begin(), end = params.end(); it != end; ++it)
303  {
304  req.params.keys.push_back(it->first);
305  req.params.values.push_back(it->second);
306  }
307  set_params_service_.call(req, res);
308  }
309 
310  std::string getDefaultPlannerId(const std::string& group) const
311  {
312  std::stringstream param_name;
313  param_name << "move_group";
314  if (!group.empty())
315  param_name << "/" << group;
316  param_name << "/default_planner_config";
317 
318  std::string default_planner_config;
319  node_handle_.getParam(param_name.str(), default_planner_config);
320  return default_planner_config;
321  }
322 
323  void setPlannerId(const std::string& planner_id)
324  {
325  planner_id_ = planner_id;
326  }
327 
328  const std::string& getPlannerId() const
329  {
330  return planner_id_;
331  }
332 
333  void setNumPlanningAttempts(unsigned int num_planning_attempts)
334  {
335  num_planning_attempts_ = num_planning_attempts;
336  }
337 
338  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
339  {
340  max_velocity_scaling_factor_ = max_velocity_scaling_factor;
341  }
342 
343  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
344  {
345  max_acceleration_scaling_factor_ = max_acceleration_scaling_factor;
346  }
347 
348  robot_state::RobotState& getJointStateTarget()
349  {
350  return *joint_state_target_;
351  }
352 
353  void setStartState(const robot_state::RobotState& start_state)
354  {
355  considered_start_state_.reset(new robot_state::RobotState(start_state));
356  }
357 
359  {
360  considered_start_state_.reset();
361  }
362 
363  robot_state::RobotStatePtr getStartState()
364  {
367  else
368  {
369  robot_state::RobotStatePtr s;
370  getCurrentState(s);
371  return s;
372  }
373  }
374 
375  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
376  const std::string& frame, bool approx)
377  {
378  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
379  // this only works if we have an end-effector
380  if (!eef.empty())
381  {
382  // first we set the goal to be the same as the start state
383  moveit::core::RobotStatePtr c = getStartState();
384  if (c)
385  {
386  setTargetType(JOINT);
387  c->enforceBounds();
388  getJointStateTarget() = *c;
389  if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
390  return false;
391  }
392  else
393  return false;
394 
395  // we may need to do approximate IK
397  o.return_approximate_solution = approx;
398 
399  // if no frame transforms are needed, call IK directly
400  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
401  return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0.0,
403  else
404  {
405  if (c->knowsFrameTransform(frame))
406  {
407  // transform the pose first if possible, then do IK
408  const Eigen::Isometry3d& t = getJointStateTarget().getFrameTransform(frame);
409  Eigen::Isometry3d p;
410  tf2::fromMsg(eef_pose, p);
411  return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0.0,
413  }
414  else
415  {
416  ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
417  getRobotModel()->getModelFrame().c_str());
418  return false;
419  }
420  }
421  }
422  else
423  return false;
424  }
425 
426  void setEndEffectorLink(const std::string& end_effector)
427  {
428  end_effector_link_ = end_effector;
429  }
430 
431  void clearPoseTarget(const std::string& end_effector_link)
432  {
433  pose_targets_.erase(end_effector_link);
434  }
435 
437  {
438  pose_targets_.clear();
439  }
440 
441  const std::string& getEndEffectorLink() const
442  {
443  return end_effector_link_;
444  }
445 
446  const std::string& getEndEffector() const
447  {
448  if (!end_effector_link_.empty())
449  {
450  const std::vector<std::string>& possible_eefs =
451  getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
452  for (std::size_t i = 0; i < possible_eefs.size(); ++i)
453  if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
454  return possible_eefs[i];
455  }
456  static std::string empty;
457  return empty;
458  }
459 
460  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
461  {
462  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
463  if (eef.empty())
464  {
465  ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for");
466  return false;
467  }
468  else
469  {
470  pose_targets_[eef] = poses;
471  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
472  std::vector<geometry_msgs::PoseStamped>& stored_poses = pose_targets_[eef];
473  for (std::size_t i = 0; i < stored_poses.size(); ++i)
474  stored_poses[i].header.stamp = ros::Time(0);
475  }
476  return true;
477  }
478 
479  bool hasPoseTarget(const std::string& end_effector_link) const
480  {
481  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
482  return pose_targets_.find(eef) != pose_targets_.end();
483  }
484 
485  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
486  {
487  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
488 
489  // if multiple pose targets are set, return the first one
490  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
491  if (jt != pose_targets_.end())
492  if (!jt->second.empty())
493  return jt->second.at(0);
494 
495  // or return an error
496  static const geometry_msgs::PoseStamped UNKNOWN;
497  ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str());
498  return UNKNOWN;
499  }
500 
501  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
502  {
503  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
504 
505  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
506  if (jt != pose_targets_.end())
507  if (!jt->second.empty())
508  return jt->second;
509 
510  // or return an error
511  static const std::vector<geometry_msgs::PoseStamped> EMPTY;
512  ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str());
513  return EMPTY;
514  }
515 
516  void setPoseReferenceFrame(const std::string& pose_reference_frame)
517  {
518  pose_reference_frame_ = pose_reference_frame;
519  }
520 
521  void setSupportSurfaceName(const std::string& support_surface)
522  {
523  support_surface_ = support_surface;
524  }
525 
526  const std::string& getPoseReferenceFrame() const
527  {
528  return pose_reference_frame_;
529  }
530 
531  void setTargetType(ActiveTargetType type)
532  {
533  active_target_ = type;
534  }
535 
536  ActiveTargetType getTargetType() const
537  {
538  return active_target_;
539  }
540 
541  bool startStateMonitor(double wait)
542  {
544  {
545  ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state");
546  return false;
547  }
548 
549  // if needed, start the monitor and wait up to 1 second for a full robot state
550  if (!current_state_monitor_->isActive())
551  current_state_monitor_->startStateMonitor();
552 
553  current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
554  return true;
555  }
556 
557  bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0)
558  {
560  {
561  ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state");
562  return false;
563  }
564 
565  // if needed, start the monitor and wait up to 1 second for a full robot state
566  if (!current_state_monitor_->isActive())
567  current_state_monitor_->startStateMonitor();
568 
569  if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds))
570  {
571  ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state");
572  return false;
573  }
574 
575  current_state = current_state_monitor_->getCurrentState();
576  return true;
577  }
578 
580  std::vector<moveit_msgs::PlaceLocation> posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses)
581  {
582  std::vector<moveit_msgs::PlaceLocation> locations;
583  for (std::size_t i = 0; i < poses.size(); ++i)
584  {
585  moveit_msgs::PlaceLocation location;
586  location.pre_place_approach.direction.vector.z = -1.0;
587  location.post_place_retreat.direction.vector.x = -1.0;
588  location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
589  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
590 
591  location.pre_place_approach.min_distance = 0.1;
592  location.pre_place_approach.desired_distance = 0.2;
593  location.post_place_retreat.min_distance = 0.0;
594  location.post_place_retreat.desired_distance = 0.2;
595  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
596 
597  location.place_pose = poses[i];
598  locations.push_back(location);
599  }
600  ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
601  (unsigned int)locations.size());
602  return locations;
603  }
604 
605  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
606  {
608  {
609  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
610  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
611  }
612  if (!place_action_client_->isServerConnected())
613  {
614  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
615  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
616  }
617 
618  place_action_client_->sendGoal(goal);
619  ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
620  if (!place_action_client_->waitForResult())
621  {
622  ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
623  }
625  {
626  return MoveItErrorCode(place_action_client_->getResult()->error_code);
627  }
628  else
629  {
630  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
631  << place_action_client_->getState().getText());
632  return MoveItErrorCode(place_action_client_->getResult()->error_code);
633  }
634  }
635 
636  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
637  {
638  if (!pick_action_client_)
639  {
640  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
641  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
642  }
643  if (!pick_action_client_->isServerConnected())
644  {
645  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
646  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
647  }
648 
649  pick_action_client_->sendGoal(goal);
650  if (!pick_action_client_->waitForResult())
651  {
652  ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
653  }
655  {
656  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
657  }
658  else
659  {
660  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
661  << pick_action_client_->getState().getText());
662  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
663  }
664  }
665 
666  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
667  {
668  if (object.empty())
669  {
670  return planGraspsAndPick(moveit_msgs::CollisionObject());
671  }
672 
674  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
675 
676  if (objects.empty())
677  {
678  ROS_ERROR_STREAM_NAMED("move_group_interface",
679  "Asked for grasps for the object '" << object << "', but the object could not be found");
680  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
681  }
682 
683  return planGraspsAndPick(objects[object], plan_only);
684  }
685 
686  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
687  {
689  {
690  ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
691  << GRASP_PLANNING_SERVICE_NAME
692  << "' is not available."
693  " This has to be implemented and started separately.");
694  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
695  }
696 
697  moveit_msgs::GraspPlanning::Request request;
698  moveit_msgs::GraspPlanning::Response response;
699 
700  request.group_name = opt_.group_name_;
701  request.target = object;
702  request.support_surfaces.push_back(support_surface_);
703 
704  ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
705  if (!plan_grasps_service_.call(request, response) ||
706  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
707  {
708  ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
709  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
710  }
711 
712  return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only));
713  }
714 
716  {
717  if (!move_action_client_)
718  {
719  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
720  }
721  if (!move_action_client_->isServerConnected())
722  {
723  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
724  }
725 
726  moveit_msgs::MoveGroupGoal goal;
727  constructGoal(goal);
728  goal.planning_options.plan_only = true;
729  goal.planning_options.look_around = false;
730  goal.planning_options.replan = false;
731  goal.planning_options.planning_scene_diff.is_diff = true;
732  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
733 
734  move_action_client_->sendGoal(goal);
735  if (!move_action_client_->waitForResult())
736  {
737  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
738  }
740  {
741  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
742  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
743  plan.planning_time_ = move_action_client_->getResult()->planning_time;
744  return MoveItErrorCode(move_action_client_->getResult()->error_code);
745  }
746  else
747  {
748  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
749  << move_action_client_->getState().getText());
750  return MoveItErrorCode(move_action_client_->getResult()->error_code);
751  }
752  }
753 
755  {
756  if (!move_action_client_)
757  {
758  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
759  }
760  if (!move_action_client_->isServerConnected())
761  {
762  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
763  }
764 
765  moveit_msgs::MoveGroupGoal goal;
766  constructGoal(goal);
767  goal.planning_options.plan_only = false;
768  goal.planning_options.look_around = can_look_;
769  goal.planning_options.replan = can_replan_;
770  goal.planning_options.replan_delay = replan_delay_;
771  goal.planning_options.planning_scene_diff.is_diff = true;
772  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
773 
774  move_action_client_->sendGoal(goal);
775  if (!wait)
776  {
777  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
778  }
779 
780  if (!move_action_client_->waitForResult())
781  {
782  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
783  }
784 
786  {
787  return MoveItErrorCode(move_action_client_->getResult()->error_code);
788  }
789  else
790  {
791  ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
792  << ": " << move_action_client_->getState().getText());
793  return MoveItErrorCode(move_action_client_->getResult()->error_code);
794  }
795  }
796 
797  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
798  {
800  {
801  ROS_ERROR_STREAM_NAMED("move_group_interface", "execute action client not found");
802  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
803  }
804  if (!execute_action_client_->isServerConnected())
805  {
806  ROS_WARN_STREAM_NAMED("move_group_interface", "execute action server not connected");
807  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
808  }
809 
810  moveit_msgs::ExecuteTrajectoryGoal goal;
811  goal.trajectory = trajectory;
812 
813  execute_action_client_->sendGoal(goal);
814  if (!wait)
815  {
816  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
817  }
818 
819  if (!execute_action_client_->waitForResult())
820  {
821  ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early");
822  }
823 
825  {
826  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
827  }
828  else
829  {
830  ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString()
831  << ": " << execute_action_client_->getState().getText());
832  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
833  }
834  }
835 
836  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
837  moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
838  bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
839  {
840  moveit_msgs::GetCartesianPath::Request req;
841  moveit_msgs::GetCartesianPath::Response res;
842 
844  robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
845  else
846  req.start_state.is_diff = true;
847 
848  req.group_name = opt_.group_name_;
849  req.header.frame_id = getPoseReferenceFrame();
850  req.header.stamp = ros::Time::now();
851  req.waypoints = waypoints;
852  req.max_step = step;
853  req.jump_threshold = jump_threshold;
854  req.path_constraints = path_constraints;
855  req.avoid_collisions = avoid_collisions;
856  req.link_name = getEndEffectorLink();
857 
858  if (cartesian_path_service_.call(req, res))
859  {
860  error_code = res.error_code;
861  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
862  {
863  msg = res.solution;
864  return res.fraction;
865  }
866  else
867  return -1.0;
868  }
869  else
870  {
871  error_code.val = error_code.FAILURE;
872  return -1.0;
873  }
874  }
875 
876  void stop()
877  {
879  {
880  std_msgs::String event;
881  event.data = "stop";
883  }
884  }
885 
886  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
887  {
888  std::string l = link.empty() ? getEndEffectorLink() : link;
889  if (l.empty())
890  {
891  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
892  if (!links.empty())
893  l = links[0];
894  }
895  if (l.empty())
896  {
897  ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
898  return false;
899  }
900  moveit_msgs::AttachedCollisionObject aco;
901  aco.object.id = object;
902  aco.link_name.swap(l);
903  if (touch_links.empty())
904  aco.touch_links.push_back(aco.link_name);
905  else
906  aco.touch_links = touch_links;
907  aco.object.operation = moveit_msgs::CollisionObject::ADD;
909  return true;
910  }
911 
912  bool detachObject(const std::string& name)
913  {
914  moveit_msgs::AttachedCollisionObject aco;
915  // if name is a link
916  if (!name.empty() && joint_model_group_->hasLinkModel(name))
917  aco.link_name = name;
918  else
919  aco.object.id = name;
920  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
921  if (aco.link_name.empty() && aco.object.id.empty())
922  {
923  // we only want to detach objects for this group
924  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
925  for (std::size_t i = 0; i < lnames.size(); ++i)
926  {
927  aco.link_name = lnames[i];
929  }
930  }
931  else
933  return true;
934  }
935 
937  {
939  }
940 
942  {
944  }
945 
946  double getGoalJointTolerance() const
947  {
948  return goal_joint_tolerance_;
949  }
950 
951  void setGoalJointTolerance(double tolerance)
952  {
953  goal_joint_tolerance_ = tolerance;
954  }
955 
956  void setGoalPositionTolerance(double tolerance)
957  {
958  goal_position_tolerance_ = tolerance;
959  }
960 
961  void setGoalOrientationTolerance(double tolerance)
962  {
963  goal_orientation_tolerance_ = tolerance;
964  }
965 
966  void setPlanningTime(double seconds)
967  {
968  if (seconds > 0.0)
969  allowed_planning_time_ = seconds;
970  }
971 
972  double getPlanningTime() const
973  {
974  return allowed_planning_time_;
975  }
976 
977  void allowLooking(bool flag)
978  {
979  can_look_ = flag;
980  ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
981  }
982 
983  void allowReplanning(bool flag)
984  {
985  can_replan_ = flag;
986  ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
987  }
988 
989  void setReplanningDelay(double delay)
990  {
991  if (delay >= 0.0)
992  replan_delay_ = delay;
993  }
994 
995  double getReplanningDelay() const
996  {
997  return replan_delay_;
998  }
999 
1000  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request)
1001  {
1002  request.group_name = opt_.group_name_;
1003  request.num_planning_attempts = num_planning_attempts_;
1004  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1005  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1006  request.allowed_planning_time = allowed_planning_time_;
1007  request.planner_id = planner_id_;
1008  request.workspace_parameters = workspace_parameters_;
1009 
1011  robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
1012  else
1013  request.start_state.is_diff = true;
1014 
1015  if (active_target_ == JOINT)
1016  {
1017  request.goal_constraints.resize(1);
1018  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1020  }
1021  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1022  {
1023  // find out how many goals are specified
1024  std::size_t goal_count = 0;
1025  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1026  it != pose_targets_.end(); ++it)
1027  goal_count = std::max(goal_count, it->second.size());
1028 
1029  // start filling the goals;
1030  // each end effector has a number of possible poses (K) as valid goals
1031  // but there could be multiple end effectors specified, so we want each end effector
1032  // to reach the goal that corresponds to the goals of the other end effectors
1033  request.goal_constraints.resize(goal_count);
1034 
1035  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1036  it != pose_targets_.end(); ++it)
1037  {
1038  for (std::size_t i = 0; i < it->second.size(); ++i)
1039  {
1040  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1041  it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1042  if (active_target_ == ORIENTATION)
1043  c.position_constraints.clear();
1044  if (active_target_ == POSITION)
1045  c.orientation_constraints.clear();
1046  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1047  }
1048  }
1049  }
1050  else
1051  ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation");
1052 
1053  if (path_constraints_)
1054  request.path_constraints = *path_constraints_;
1056  request.trajectory_constraints = *trajectory_constraints_;
1057  }
1058 
1059  void constructGoal(moveit_msgs::MoveGroupGoal& goal)
1060  {
1061  constructMotionPlanRequest(goal.request);
1062  }
1063 
1064  moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector<moveit_msgs::Grasp>&& grasps,
1065  bool plan_only = false)
1066  {
1067  moveit_msgs::PickupGoal goal;
1068  goal.target_name = object;
1069  goal.group_name = opt_.group_name_;
1070  goal.end_effector = getEndEffector();
1071  goal.support_surface_name = support_surface_;
1072  goal.possible_grasps = std::move(grasps);
1073  if (!support_surface_.empty())
1074  goal.allow_gripper_support_collision = true;
1075 
1076  if (path_constraints_)
1077  goal.path_constraints = *path_constraints_;
1078 
1079  goal.planner_id = planner_id_;
1080  goal.allowed_planning_time = allowed_planning_time_;
1081 
1082  goal.planning_options.plan_only = plan_only;
1083  goal.planning_options.look_around = can_look_;
1084  goal.planning_options.replan = can_replan_;
1085  goal.planning_options.replan_delay = replan_delay_;
1086  goal.planning_options.planning_scene_diff.is_diff = true;
1087  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1088  return goal;
1089  }
1090 
1091  moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object,
1092  std::vector<moveit_msgs::PlaceLocation>&& locations, bool plan_only = false)
1093  {
1094  moveit_msgs::PlaceGoal goal;
1095  goal.group_name = opt_.group_name_;
1096  goal.attached_object_name = object;
1097  goal.support_surface_name = support_surface_;
1098  goal.place_locations = std::move(locations);
1099  if (!support_surface_.empty())
1100  goal.allow_gripper_support_collision = true;
1101 
1102  if (path_constraints_)
1103  goal.path_constraints = *path_constraints_;
1104 
1105  goal.planner_id = planner_id_;
1106  goal.allowed_planning_time = allowed_planning_time_;
1107 
1108  goal.planning_options.plan_only = plan_only;
1109  goal.planning_options.look_around = can_look_;
1110  goal.planning_options.replan = can_replan_;
1111  goal.planning_options.replan_delay = replan_delay_;
1112  goal.planning_options.planning_scene_diff.is_diff = true;
1113  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
1114  return goal;
1115  }
1116 
1117  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1118  {
1119  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1120  }
1121 
1122  bool setPathConstraints(const std::string& constraint)
1123  {
1125  {
1127  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1128  {
1129  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1130  return true;
1131  }
1132  else
1133  return false;
1134  }
1135  else
1136  return false;
1137  }
1138 
1140  {
1141  path_constraints_.reset();
1142  }
1143 
1144  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
1145  {
1146  trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint));
1147  }
1148 
1150  {
1151  trajectory_constraints_.reset();
1152  }
1153 
1154  std::vector<std::string> getKnownConstraints() const
1155  {
1157  {
1158  static ros::WallDuration d(0.01);
1159  d.sleep();
1160  }
1161 
1162  std::vector<std::string> c;
1164  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1165 
1166  return c;
1167  }
1168 
1169  moveit_msgs::Constraints getPathConstraints() const
1170  {
1171  if (path_constraints_)
1172  return *path_constraints_;
1173  else
1174  return moveit_msgs::Constraints();
1175  }
1176 
1177  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
1178  {
1180  return *trajectory_constraints_;
1181  else
1182  return moveit_msgs::TrajectoryConstraints();
1183  }
1184 
1185  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1186  {
1189  constraints_init_thread_->join();
1191  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1192  }
1193 
1194  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1195  {
1196  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1197  workspace_parameters_.header.stamp = ros::Time::now();
1198  workspace_parameters_.min_corner.x = minx;
1199  workspace_parameters_.min_corner.y = miny;
1200  workspace_parameters_.min_corner.z = minz;
1201  workspace_parameters_.max_corner.x = maxx;
1202  workspace_parameters_.max_corner.y = maxy;
1203  workspace_parameters_.max_corner.z = maxz;
1204  }
1205 
1206 private:
1207  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1208  {
1209  // Set up db
1210  try
1211  {
1213  conn->setParams(host, port);
1214  if (conn->connect())
1215  {
1217  }
1218  }
1219  catch (std::exception& ex)
1220  {
1221  ROS_ERROR_NAMED("move_group_interface", "%s", ex.what());
1222  }
1223  initializing_constraints_ = false;
1224  }
1225 
1228  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
1229  robot_model::RobotModelConstPtr robot_model_;
1230  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1231  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1232  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1233  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1234  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1235 
1236  // general planning params
1237  robot_state::RobotStatePtr considered_start_state_;
1238  moveit_msgs::WorkspaceParameters workspace_parameters_;
1240  std::string planner_id_;
1250 
1251  // joint state goal
1252  robot_state::RobotStatePtr joint_state_target_;
1253  const robot_model::JointModelGroup* joint_model_group_;
1254 
1255  // pose goal;
1256  // for each link we have a set of possible goal locations;
1257  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1258 
1259  // common properties for goals
1260  ActiveTargetType active_target_;
1261  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1262  std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
1263  std::string end_effector_link_;
1265  std::string support_surface_;
1266 
1267  // ROS communication
1275  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1276  std::unique_ptr<boost::thread> constraints_init_thread_;
1278 };
1279 } // namespace planning_interface
1280 } // namespace moveit
1281 
1283  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1284  const ros::WallDuration& wait_for_servers)
1285 {
1286  if (!ros::ok())
1287  throw std::runtime_error("ROS does not seem to be running");
1288  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1289 }
1290 
1292  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1293  const ros::Duration& wait_for_servers)
1294  : MoveGroupInterface(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1295 {
1296 }
1297 
1299  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1300  const ros::WallDuration& wait_for_servers)
1301 {
1302  impl_ = new MoveGroupInterfaceImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1303 }
1304 
1307  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const ros::Duration& wait_for_servers)
1308  : MoveGroupInterface(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1309 {
1310 }
1311 
1313 {
1314  delete impl_;
1315 }
1316 
1319 {
1320  other.impl_ = nullptr;
1321 }
1322 
1325 {
1326  if (this != &other)
1327  {
1328  delete impl_;
1329  impl_ = other.impl_;
1330  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1331  other.impl_ = nullptr;
1332  }
1333 
1334  return *this;
1335 }
1336 
1338 {
1339  return impl_->getOptions().group_name_;
1340 }
1341 
1343 {
1344  const robot_model::RobotModelConstPtr& robot = getRobotModel();
1345  const std::string& group = getName();
1346  const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1347 
1348  if (joint_group)
1349  {
1350  return joint_group->getDefaultStateNames();
1351  }
1352 
1353  std::vector<std::string> empty;
1354  return empty;
1355 }
1356 
1358 {
1359  return impl_->getRobotModel();
1360 }
1361 
1363 {
1364  return impl_->getOptions().node_handle_;
1365 }
1366 
1368  moveit_msgs::PlannerInterfaceDescription& desc)
1369 {
1370  return impl_->getInterfaceDescription(desc);
1371 }
1372 
1373 std::map<std::string, std::string>
1374 moveit::planning_interface::MoveGroupInterface::getPlannerParams(const std::string& planner_id, const std::string& group)
1375 {
1376  return impl_->getPlannerParams(planner_id, group);
1377 }
1378 
1380  const std::string& group,
1381  const std::map<std::string, std::string>& params,
1382  bool replace)
1383 {
1384  impl_->setPlannerParams(planner_id, group, params, replace);
1385 }
1386 
1388 {
1389  return impl_->getDefaultPlannerId(group);
1390 }
1391 
1393 {
1394  impl_->setPlannerId(planner_id);
1395 }
1396 
1398 {
1399  return impl_->getPlannerId();
1400 }
1401 
1403 {
1404  impl_->setNumPlanningAttempts(num_planning_attempts);
1405 }
1406 
1408 {
1409  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1410 }
1411 
1413  double max_acceleration_scaling_factor)
1414 {
1415  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1416 }
1417 
1419 {
1420  return impl_->move(false);
1421 }
1422 
1425 {
1426  return impl_->getMoveGroupClient();
1427 }
1428 
1430 {
1431  return impl_->move(true);
1432 }
1433 
1436 {
1437  return impl_->execute(plan.trajectory_, false);
1438 }
1439 
1441 moveit::planning_interface::MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory)
1442 {
1443  return impl_->execute(trajectory, false);
1444 }
1445 
1447 {
1448  return impl_->execute(plan.trajectory_, true);
1449 }
1450 
1452 moveit::planning_interface::MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory)
1453 {
1454  return impl_->execute(trajectory, true);
1455 }
1456 
1458 {
1459  return impl_->plan(plan);
1460 }
1461 
1463  const std::string& object, std::vector<moveit_msgs::Grasp> grasps, bool plan_only = false)
1464 {
1465  return impl_->constructPickupGoal(object, std::move(grasps), plan_only);
1466 }
1467 
1469  const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations, bool plan_only = false)
1470 {
1471  return impl_->constructPlaceGoal(object, std::move(locations), plan_only);
1472 }
1473 
1475  const std::vector<geometry_msgs::PoseStamped>& poses)
1476 {
1477  return impl_->posesToPlaceLocations(poses);
1478 }
1479 
1481 moveit::planning_interface::MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
1482 {
1483  return impl_->pick(goal);
1484 }
1485 
1488 {
1489  return impl_->planGraspsAndPick(object, plan_only);
1490 }
1491 
1494  bool plan_only)
1495 {
1496  return impl_->planGraspsAndPick(object, plan_only);
1497 }
1498 
1501 {
1502  return impl_->place(goal);
1503 }
1504 
1506  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1507  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1508 {
1509  moveit_msgs::Constraints path_constraints_tmp;
1510  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1511  error_code);
1512 }
1513 
1515  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1516  moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1517  moveit_msgs::MoveItErrorCodes* error_code)
1518 {
1519  if (error_code)
1520  {
1521  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1522  avoid_collisions, *error_code);
1523  }
1524  else
1525  {
1526  moveit_msgs::MoveItErrorCodes error_code_tmp;
1527  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1528  avoid_collisions, error_code_tmp);
1529  }
1530 }
1531 
1533 {
1534  impl_->stop();
1535 }
1536 
1537 void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1538 {
1539  robot_state::RobotStatePtr rs;
1540  impl_->getCurrentState(rs);
1541  robot_state::robotStateMsgToRobotState(start_state, *rs);
1542  setStartState(*rs);
1543 }
1544 
1545 void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state)
1546 {
1547  impl_->setStartState(start_state);
1548 }
1549 
1551 {
1553 }
1554 
1556 {
1557  impl_->getJointStateTarget().setToRandomPositions();
1558  impl_->setTargetType(JOINT);
1559 }
1560 
1562 {
1563  return impl_->getJointModelGroup()->getVariableNames();
1564 }
1565 
1567 {
1568  return impl_->getJointModelGroup()->getLinkModelNames();
1569 }
1570 
1571 std::map<std::string, double>
1573 {
1574  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1575  std::map<std::string, double> positions;
1576 
1577  if (it != remembered_joint_values_.end())
1578  {
1579  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1580  for (size_t x = 0; x < names.size(); ++x)
1581  {
1582  positions[names[x]] = it->second[x];
1583  }
1584  }
1585  else
1586  {
1587  impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
1588  }
1589  return positions;
1590 }
1591 
1593 {
1594  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1595  if (it != remembered_joint_values_.end())
1596  {
1597  return setJointValueTarget(it->second);
1598  }
1599  else
1600  {
1601  if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
1602  {
1603  impl_->setTargetType(JOINT);
1604  return true;
1605  }
1606  ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
1607  return false;
1608  }
1609 }
1610 
1611 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1612 {
1613  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
1614  return false;
1615  impl_->setTargetType(JOINT);
1616  impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1618 }
1619 
1621  const std::map<std::string, double>& joint_values)
1622 {
1623  impl_->setTargetType(JOINT);
1624  impl_->getJointStateTarget().setVariablePositions(joint_values);
1625  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1626 }
1627 
1629 {
1630  impl_->setTargetType(JOINT);
1631  impl_->getJointStateTarget() = rstate;
1632  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1633 }
1634 
1635 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1636 {
1637  std::vector<double> values(1, value);
1638  return setJointValueTarget(joint_name, values);
1639 }
1640 
1642  const std::vector<double>& values)
1643 {
1644  impl_->setTargetType(JOINT);
1645  const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
1646  if (jm && jm->getVariableCount() == values.size())
1647  {
1648  impl_->getJointStateTarget().setJointPositions(jm, values);
1649  return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1650  }
1651  return false;
1652 }
1653 
1655 {
1656  impl_->setTargetType(JOINT);
1657  impl_->getJointStateTarget().setVariableValues(state);
1658  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1659 }
1660 
1662  const std::string& end_effector_link)
1663 {
1664  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1665 }
1666 
1667 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1668  const std::string& end_effector_link)
1669 {
1670  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1671 }
1672 
1674  const std::string& end_effector_link)
1675 {
1676  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1677  return setJointValueTarget(msg, end_effector_link);
1678 }
1679 
1681  const std::string& end_effector_link)
1682 {
1683  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1684 }
1685 
1687  const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link)
1688 {
1689  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1690 }
1691 
1693  const std::string& end_effector_link)
1694 {
1695  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1696  return setApproximateJointValueTarget(msg, end_effector_link);
1697 }
1698 
1700 {
1701  return impl_->getJointStateTarget();
1702 }
1703 
1705 {
1706  return impl_->getEndEffectorLink();
1707 }
1708 
1710 {
1711  return impl_->getEndEffector();
1712 }
1713 
1715 {
1716  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1717  return false;
1718  impl_->setEndEffectorLink(link_name);
1719  impl_->setTargetType(POSE);
1720  return true;
1721 }
1722 
1724 {
1725  const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1726  if (jmg)
1727  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1728  return false;
1729 }
1730 
1732 {
1733  impl_->clearPoseTarget(end_effector_link);
1734 }
1735 
1737 {
1739 }
1740 
1742  const std::string& end_effector_link)
1743 {
1744  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1745  pose_msg[0].pose = tf2::toMsg(pose);
1746  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1747  pose_msg[0].header.stamp = ros::Time::now();
1748  return setPoseTargets(pose_msg, end_effector_link);
1749 }
1750 
1752  const std::string& end_effector_link)
1753 {
1754  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1755  pose_msg[0].pose = target;
1756  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1757  pose_msg[0].header.stamp = ros::Time::now();
1758  return setPoseTargets(pose_msg, end_effector_link);
1759 }
1760 
1761 bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target,
1762  const std::string& end_effector_link)
1763 {
1764  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1765  return setPoseTargets(targets, end_effector_link);
1766 }
1767 
1769  const std::string& end_effector_link)
1770 {
1771  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1772  ros::Time tm = ros::Time::now();
1773  const std::string& frame_id = getPoseReferenceFrame();
1774  for (std::size_t i = 0; i < target.size(); ++i)
1775  {
1776  pose_out[i].pose = tf2::toMsg(target[i]);
1777  pose_out[i].header.stamp = tm;
1778  pose_out[i].header.frame_id = frame_id;
1779  }
1780  return setPoseTargets(pose_out, end_effector_link);
1781 }
1782 
1783 bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1784  const std::string& end_effector_link)
1785 {
1786  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1787  ros::Time tm = ros::Time::now();
1788  const std::string& frame_id = getPoseReferenceFrame();
1789  for (std::size_t i = 0; i < target.size(); ++i)
1790  {
1791  target_stamped[i].pose = target[i];
1792  target_stamped[i].header.stamp = tm;
1793  target_stamped[i].header.frame_id = frame_id;
1794  }
1795  return setPoseTargets(target_stamped, end_effector_link);
1796 }
1797 
1799  const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link)
1800 {
1801  if (target.empty())
1802  {
1803  ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
1804  return false;
1805  }
1806  else
1807  {
1808  impl_->setTargetType(POSE);
1809  return impl_->setPoseTargets(target, end_effector_link);
1810  }
1811 }
1812 
1813 const geometry_msgs::PoseStamped&
1814 moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1815 {
1816  return impl_->getPoseTarget(end_effector_link);
1817 }
1818 
1819 const std::vector<geometry_msgs::PoseStamped>&
1820 moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1821 {
1822  return impl_->getPoseTargets(end_effector_link);
1823 }
1824 
1825 namespace
1826 {
1827 inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame,
1828  geometry_msgs::PoseStamped& target)
1829 {
1830  if (desired_frame != target.header.frame_id)
1831  {
1832  geometry_msgs::PoseStamped target_in(target);
1833  tf_buffer.transform(target_in, target, desired_frame);
1834  // we leave the stamp to ros::Time(0) on purpose
1835  target.header.stamp = ros::Time(0);
1836  }
1837 }
1838 } // namespace
1839 
1841  const std::string& end_effector_link)
1842 {
1843  geometry_msgs::PoseStamped target;
1844  if (impl_->hasPoseTarget(end_effector_link))
1845  {
1846  target = getPoseTarget(end_effector_link);
1847  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1848  }
1849  else
1850  {
1851  target.pose.orientation.x = 0.0;
1852  target.pose.orientation.y = 0.0;
1853  target.pose.orientation.z = 0.0;
1854  target.pose.orientation.w = 1.0;
1855  target.header.frame_id = impl_->getPoseReferenceFrame();
1856  }
1857 
1858  target.pose.position.x = x;
1859  target.pose.position.y = y;
1860  target.pose.position.z = z;
1861  bool result = setPoseTarget(target, end_effector_link);
1862  impl_->setTargetType(POSITION);
1863  return result;
1864 }
1865 
1867  const std::string& end_effector_link)
1868 {
1869  geometry_msgs::PoseStamped target;
1870  if (impl_->hasPoseTarget(end_effector_link))
1871  {
1872  target = getPoseTarget(end_effector_link);
1873  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1874  }
1875  else
1876  {
1877  target.pose.position.x = 0.0;
1878  target.pose.position.y = 0.0;
1879  target.pose.position.z = 0.0;
1880  target.header.frame_id = impl_->getPoseReferenceFrame();
1881  }
1883  q.setRPY(r, p, y);
1884  target.pose.orientation = tf2::toMsg(q);
1885  bool result = setPoseTarget(target, end_effector_link);
1886  impl_->setTargetType(ORIENTATION);
1887  return result;
1888 }
1889 
1891  const std::string& end_effector_link)
1892 {
1893  geometry_msgs::PoseStamped target;
1894  if (impl_->hasPoseTarget(end_effector_link))
1895  {
1896  target = getPoseTarget(end_effector_link);
1897  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1898  }
1899  else
1900  {
1901  target.pose.position.x = 0.0;
1902  target.pose.position.y = 0.0;
1903  target.pose.position.z = 0.0;
1904  target.header.frame_id = impl_->getPoseReferenceFrame();
1905  }
1906 
1907  target.pose.orientation.x = x;
1908  target.pose.orientation.y = y;
1909  target.pose.orientation.z = z;
1910  target.pose.orientation.w = w;
1911  bool result = setPoseTarget(target, end_effector_link);
1912  impl_->setTargetType(ORIENTATION);
1913  return result;
1914 }
1915 
1917 {
1918  impl_->setPoseReferenceFrame(pose_reference_frame);
1919 }
1920 
1922 {
1923  return impl_->getPoseReferenceFrame();
1924 }
1925 
1927 {
1928  return impl_->getGoalJointTolerance();
1929 }
1930 
1932 {
1933  return impl_->getGoalPositionTolerance();
1934 }
1935 
1937 {
1939 }
1940 
1942 {
1943  setGoalJointTolerance(tolerance);
1944  setGoalPositionTolerance(tolerance);
1945  setGoalOrientationTolerance(tolerance);
1946 }
1947 
1949 {
1950  impl_->setGoalJointTolerance(tolerance);
1951 }
1952 
1954 {
1955  impl_->setGoalPositionTolerance(tolerance);
1956 }
1957 
1959 {
1960  impl_->setGoalOrientationTolerance(tolerance);
1961 }
1962 
1964 {
1966 }
1967 
1969 {
1970  return impl_->startStateMonitor(wait);
1971 }
1972 
1974 {
1975  robot_state::RobotStatePtr current_state;
1976  std::vector<double> values;
1977  if (impl_->getCurrentState(current_state))
1978  current_state->copyJointGroupPositions(getName(), values);
1979  return values;
1980 }
1981 
1983 {
1984  std::vector<double> r;
1985  impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
1986  return r;
1987 }
1988 
1989 geometry_msgs::PoseStamped
1991 {
1992  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
1993  Eigen::Isometry3d pose;
1994  pose.setIdentity();
1995  if (eef.empty())
1996  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
1997  else
1998  {
1999  robot_state::RobotStatePtr current_state;
2000  if (impl_->getCurrentState(current_state))
2001  {
2002  current_state->setToRandomPositions(impl_->getJointModelGroup());
2003  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2004  if (lm)
2005  pose = current_state->getGlobalLinkTransform(lm);
2006  }
2007  }
2008  geometry_msgs::PoseStamped pose_msg;
2009  pose_msg.header.stamp = ros::Time::now();
2010  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2011  pose_msg.pose = tf2::toMsg(pose);
2012  return pose_msg;
2013 }
2014 
2015 geometry_msgs::PoseStamped
2017 {
2018  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2019  Eigen::Isometry3d pose;
2020  pose.setIdentity();
2021  if (eef.empty())
2022  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2023  else
2024  {
2025  robot_state::RobotStatePtr current_state;
2026  if (impl_->getCurrentState(current_state))
2027  {
2028  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2029  if (lm)
2030  pose = current_state->getGlobalLinkTransform(lm);
2031  }
2032  }
2033  geometry_msgs::PoseStamped pose_msg;
2034  pose_msg.header.stamp = ros::Time::now();
2035  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2036  pose_msg.pose = tf2::toMsg(pose);
2037  return pose_msg;
2038 }
2039 
2040 std::vector<double> moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link)
2041 {
2042  std::vector<double> result;
2043  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2044  if (eef.empty())
2045  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2046  else
2047  {
2048  robot_state::RobotStatePtr current_state;
2049  if (impl_->getCurrentState(current_state))
2050  {
2051  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2052  if (lm)
2053  {
2054  result.resize(3);
2055  geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2056  double pitch, roll, yaw;
2057  tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2058  result[0] = roll;
2059  result[1] = pitch;
2060  result[2] = yaw;
2061  }
2062  }
2063  }
2064  return result;
2065 }
2066 
2068 {
2069  return impl_->getJointModelGroup()->getActiveJointModelNames();
2070 }
2071 
2072 const std::vector<std::string>& moveit::planning_interface::MoveGroupInterface::getJoints() const
2073 {
2074  return impl_->getJointModelGroup()->getJointModelNames();
2075 }
2076 
2078 {
2079  return impl_->getJointModelGroup()->getVariableCount();
2080 }
2081 
2083 {
2084  robot_state::RobotStatePtr current_state;
2085  impl_->getCurrentState(current_state, wait);
2086  return current_state;
2087 }
2088 
2090  const std::vector<double>& values)
2091 {
2092  remembered_joint_values_[name] = values;
2093 }
2094 
2096 {
2097  remembered_joint_values_.erase(name);
2098 }
2099 
2101 {
2102  impl_->allowLooking(flag);
2103 }
2104 
2106 {
2107  impl_->allowReplanning(flag);
2108 }
2109 
2111 {
2112  return impl_->getKnownConstraints();
2113 }
2114 
2116 {
2117  return impl_->getPathConstraints();
2118 }
2119 
2121 {
2122  return impl_->setPathConstraints(constraint);
2123 }
2124 
2125 void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2126 {
2127  impl_->setPathConstraints(constraint);
2128 }
2129 
2131 {
2133 }
2134 
2136 {
2137  return impl_->getTrajectoryConstraints();
2138 }
2139 
2141  const moveit_msgs::TrajectoryConstraints& constraint)
2142 {
2143  impl_->setTrajectoryConstraints(constraint);
2144 }
2145 
2147 {
2149 }
2150 
2151 void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2152 {
2153  impl_->initializeConstraintsStorage(host, port);
2154 }
2155 
2156 void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx,
2157  double maxy, double maxz)
2158 {
2159  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2160 }
2161 
2164 {
2165  impl_->setPlanningTime(seconds);
2166 }
2167 
2170 {
2171  return impl_->getPlanningTime();
2172 }
2173 
2175 {
2177 }
2178 
2180 {
2181  return impl_->getRobotModel()->getModelFrame();
2182 }
2183 
2185 {
2186  return impl_->getRobotModel()->getJointModelGroupNames();
2187 }
2188 
2189 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2190 {
2191  return attachObject(object, link, std::vector<std::string>());
2192 }
2193 
2194 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2195  const std::vector<std::string>& touch_links)
2196 {
2197  return impl_->attachObject(object, link, touch_links);
2198 }
2199 
2201 {
2202  return impl_->detachObject(name);
2203 }
2204 
2206 {
2207  impl_->constructMotionPlanRequest(goal_out);
2208 }
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)
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_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 for a given group (or global default)
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 Mon Nov 23 2020 03:54:12