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", "Ready to take commands for planning group " << opt.group_name_
176  << ".");
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", "Asked for grasps for the object '"
679  << 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 
1374  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 
1493  const moveit_msgs::CollisionObject& object, bool plan_only)
1494 {
1495  return impl_->planGraspsAndPick(object, plan_only);
1496 }
1497 
1500 {
1501  return impl_->place(goal);
1502 }
1503 
1505  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1506  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1507 {
1508  moveit_msgs::Constraints path_constraints_tmp;
1509  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1510  error_code);
1511 }
1512 
1514  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1515  moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1516  moveit_msgs::MoveItErrorCodes* error_code)
1517 {
1518  if (error_code)
1519  {
1520  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1521  avoid_collisions, *error_code);
1522  }
1523  else
1524  {
1525  moveit_msgs::MoveItErrorCodes error_code_tmp;
1526  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1527  avoid_collisions, error_code_tmp);
1528  }
1529 }
1530 
1532 {
1533  impl_->stop();
1534 }
1535 
1536 void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1537 {
1538  robot_state::RobotStatePtr rs;
1539  impl_->getCurrentState(rs);
1540  robot_state::robotStateMsgToRobotState(start_state, *rs);
1541  setStartState(*rs);
1542 }
1543 
1544 void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state)
1545 {
1546  impl_->setStartState(start_state);
1547 }
1548 
1550 {
1552 }
1553 
1555 {
1556  impl_->getJointStateTarget().setToRandomPositions();
1557  impl_->setTargetType(JOINT);
1558 }
1559 
1561 {
1562  return impl_->getJointModelGroup()->getVariableNames();
1563 }
1564 
1566 {
1567  return impl_->getJointModelGroup()->getLinkModelNames();
1568 }
1569 
1570 std::map<std::string, double>
1572 {
1573  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1574  std::map<std::string, double> positions;
1575 
1576  if (it != remembered_joint_values_.end())
1577  {
1578  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1579  for (size_t x = 0; x < names.size(); ++x)
1580  {
1581  positions[names[x]] = it->second[x];
1582  }
1583  }
1584  else
1585  {
1586  impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
1587  }
1588  return positions;
1589 }
1590 
1592 {
1593  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1594  if (it != remembered_joint_values_.end())
1595  {
1596  return setJointValueTarget(it->second);
1597  }
1598  else
1599  {
1600  if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
1601  {
1602  impl_->setTargetType(JOINT);
1603  return true;
1604  }
1605  ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
1606  return false;
1607  }
1608 }
1609 
1610 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1611 {
1612  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
1613  return false;
1614  impl_->setTargetType(JOINT);
1615  impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1617 }
1618 
1620  const std::map<std::string, double>& joint_values)
1621 {
1622  impl_->setTargetType(JOINT);
1623  impl_->getJointStateTarget().setVariablePositions(joint_values);
1624  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1625 }
1626 
1628 {
1629  impl_->setTargetType(JOINT);
1630  impl_->getJointStateTarget() = rstate;
1631  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1632 }
1633 
1634 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1635 {
1636  std::vector<double> values(1, value);
1637  return setJointValueTarget(joint_name, values);
1638 }
1639 
1641  const std::vector<double>& values)
1642 {
1643  impl_->setTargetType(JOINT);
1644  const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
1645  if (jm && jm->getVariableCount() == values.size())
1646  {
1647  impl_->getJointStateTarget().setJointPositions(jm, values);
1648  return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1649  }
1650  return false;
1651 }
1652 
1654 {
1655  impl_->setTargetType(JOINT);
1656  impl_->getJointStateTarget().setVariableValues(state);
1657  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1658 }
1659 
1661  const std::string& end_effector_link)
1662 {
1663  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1664 }
1665 
1666 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1667  const std::string& end_effector_link)
1668 {
1669  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1670 }
1671 
1673  const std::string& end_effector_link)
1674 {
1675  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1676  return setJointValueTarget(msg, end_effector_link);
1677 }
1678 
1680  const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link)
1681 {
1682  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1683 }
1684 
1686  const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link)
1687 {
1688  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1689 }
1690 
1692  const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link)
1693 {
1694  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1695  return setApproximateJointValueTarget(msg, end_effector_link);
1696 }
1697 
1699 {
1700  return impl_->getJointStateTarget();
1701 }
1702 
1704 {
1705  return impl_->getEndEffectorLink();
1706 }
1707 
1709 {
1710  return impl_->getEndEffector();
1711 }
1712 
1714 {
1715  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1716  return false;
1717  impl_->setEndEffectorLink(link_name);
1718  impl_->setTargetType(POSE);
1719  return true;
1720 }
1721 
1723 {
1724  const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1725  if (jmg)
1726  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1727  return false;
1728 }
1729 
1731 {
1732  impl_->clearPoseTarget(end_effector_link);
1733 }
1734 
1736 {
1738 }
1739 
1741  const std::string& end_effector_link)
1742 {
1743  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1744  pose_msg[0].pose = tf2::toMsg(pose);
1745  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1746  pose_msg[0].header.stamp = ros::Time::now();
1747  return setPoseTargets(pose_msg, end_effector_link);
1748 }
1749 
1751  const std::string& end_effector_link)
1752 {
1753  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1754  pose_msg[0].pose = target;
1755  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1756  pose_msg[0].header.stamp = ros::Time::now();
1757  return setPoseTargets(pose_msg, end_effector_link);
1758 }
1759 
1760 bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target,
1761  const std::string& end_effector_link)
1762 {
1763  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1764  return setPoseTargets(targets, end_effector_link);
1765 }
1766 
1768  const std::string& end_effector_link)
1769 {
1770  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1771  ros::Time tm = ros::Time::now();
1772  const std::string& frame_id = getPoseReferenceFrame();
1773  for (std::size_t i = 0; i < target.size(); ++i)
1774  {
1775  pose_out[i].pose = tf2::toMsg(target[i]);
1776  pose_out[i].header.stamp = tm;
1777  pose_out[i].header.frame_id = frame_id;
1778  }
1779  return setPoseTargets(pose_out, end_effector_link);
1780 }
1781 
1782 bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1783  const std::string& end_effector_link)
1784 {
1785  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1786  ros::Time tm = ros::Time::now();
1787  const std::string& frame_id = getPoseReferenceFrame();
1788  for (std::size_t i = 0; i < target.size(); ++i)
1789  {
1790  target_stamped[i].pose = target[i];
1791  target_stamped[i].header.stamp = tm;
1792  target_stamped[i].header.frame_id = frame_id;
1793  }
1794  return setPoseTargets(target_stamped, end_effector_link);
1795 }
1796 
1798  const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link)
1799 {
1800  if (target.empty())
1801  {
1802  ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
1803  return false;
1804  }
1805  else
1806  {
1807  impl_->setTargetType(POSE);
1808  return impl_->setPoseTargets(target, end_effector_link);
1809  }
1810 }
1811 
1812 const geometry_msgs::PoseStamped&
1813 moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1814 {
1815  return impl_->getPoseTarget(end_effector_link);
1816 }
1817 
1818 const std::vector<geometry_msgs::PoseStamped>&
1819 moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1820 {
1821  return impl_->getPoseTargets(end_effector_link);
1822 }
1823 
1824 namespace
1825 {
1826 inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame,
1827  geometry_msgs::PoseStamped& target)
1828 {
1829  if (desired_frame != target.header.frame_id)
1830  {
1831  geometry_msgs::PoseStamped target_in(target);
1832  tf_buffer.transform(target_in, target, desired_frame);
1833  // we leave the stamp to ros::Time(0) on purpose
1834  target.header.stamp = ros::Time(0);
1835  }
1836 }
1837 } // namespace
1838 
1840  const std::string& end_effector_link)
1841 {
1842  geometry_msgs::PoseStamped target;
1843  if (impl_->hasPoseTarget(end_effector_link))
1844  {
1845  target = getPoseTarget(end_effector_link);
1846  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1847  }
1848  else
1849  {
1850  target.pose.orientation.x = 0.0;
1851  target.pose.orientation.y = 0.0;
1852  target.pose.orientation.z = 0.0;
1853  target.pose.orientation.w = 1.0;
1854  target.header.frame_id = impl_->getPoseReferenceFrame();
1855  }
1856 
1857  target.pose.position.x = x;
1858  target.pose.position.y = y;
1859  target.pose.position.z = z;
1860  bool result = setPoseTarget(target, end_effector_link);
1861  impl_->setTargetType(POSITION);
1862  return result;
1863 }
1864 
1866  const std::string& end_effector_link)
1867 {
1868  geometry_msgs::PoseStamped target;
1869  if (impl_->hasPoseTarget(end_effector_link))
1870  {
1871  target = getPoseTarget(end_effector_link);
1872  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1873  }
1874  else
1875  {
1876  target.pose.position.x = 0.0;
1877  target.pose.position.y = 0.0;
1878  target.pose.position.z = 0.0;
1879  target.header.frame_id = impl_->getPoseReferenceFrame();
1880  }
1882  q.setRPY(r, p, y);
1883  target.pose.orientation = tf2::toMsg(q);
1884  bool result = setPoseTarget(target, end_effector_link);
1885  impl_->setTargetType(ORIENTATION);
1886  return result;
1887 }
1888 
1890  const std::string& end_effector_link)
1891 {
1892  geometry_msgs::PoseStamped target;
1893  if (impl_->hasPoseTarget(end_effector_link))
1894  {
1895  target = getPoseTarget(end_effector_link);
1896  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1897  }
1898  else
1899  {
1900  target.pose.position.x = 0.0;
1901  target.pose.position.y = 0.0;
1902  target.pose.position.z = 0.0;
1903  target.header.frame_id = impl_->getPoseReferenceFrame();
1904  }
1905 
1906  target.pose.orientation.x = x;
1907  target.pose.orientation.y = y;
1908  target.pose.orientation.z = z;
1909  target.pose.orientation.w = w;
1910  bool result = setPoseTarget(target, end_effector_link);
1911  impl_->setTargetType(ORIENTATION);
1912  return result;
1913 }
1914 
1916 {
1917  impl_->setPoseReferenceFrame(pose_reference_frame);
1918 }
1919 
1921 {
1922  return impl_->getPoseReferenceFrame();
1923 }
1924 
1926 {
1927  return impl_->getGoalJointTolerance();
1928 }
1929 
1931 {
1932  return impl_->getGoalPositionTolerance();
1933 }
1934 
1936 {
1938 }
1939 
1941 {
1942  setGoalJointTolerance(tolerance);
1943  setGoalPositionTolerance(tolerance);
1944  setGoalOrientationTolerance(tolerance);
1945 }
1946 
1948 {
1949  impl_->setGoalJointTolerance(tolerance);
1950 }
1951 
1953 {
1954  impl_->setGoalPositionTolerance(tolerance);
1955 }
1956 
1958 {
1959  impl_->setGoalOrientationTolerance(tolerance);
1960 }
1961 
1963 {
1965 }
1966 
1968 {
1969  return impl_->startStateMonitor(wait);
1970 }
1971 
1973 {
1974  robot_state::RobotStatePtr current_state;
1975  std::vector<double> values;
1976  if (impl_->getCurrentState(current_state))
1977  current_state->copyJointGroupPositions(getName(), values);
1978  return values;
1979 }
1980 
1982 {
1983  std::vector<double> r;
1984  impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
1985  return r;
1986 }
1987 
1988 geometry_msgs::PoseStamped
1990 {
1991  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
1992  Eigen::Isometry3d pose;
1993  pose.setIdentity();
1994  if (eef.empty())
1995  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
1996  else
1997  {
1998  robot_state::RobotStatePtr current_state;
1999  if (impl_->getCurrentState(current_state))
2000  {
2001  current_state->setToRandomPositions(impl_->getJointModelGroup());
2002  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2003  if (lm)
2004  pose = current_state->getGlobalLinkTransform(lm);
2005  }
2006  }
2007  geometry_msgs::PoseStamped pose_msg;
2008  pose_msg.header.stamp = ros::Time::now();
2009  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2010  pose_msg.pose = tf2::toMsg(pose);
2011  return pose_msg;
2012 }
2013 
2014 geometry_msgs::PoseStamped
2016 {
2017  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2018  Eigen::Isometry3d pose;
2019  pose.setIdentity();
2020  if (eef.empty())
2021  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2022  else
2023  {
2024  robot_state::RobotStatePtr current_state;
2025  if (impl_->getCurrentState(current_state))
2026  {
2027  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2028  if (lm)
2029  pose = current_state->getGlobalLinkTransform(lm);
2030  }
2031  }
2032  geometry_msgs::PoseStamped pose_msg;
2033  pose_msg.header.stamp = ros::Time::now();
2034  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2035  pose_msg.pose = tf2::toMsg(pose);
2036  return pose_msg;
2037 }
2038 
2039 std::vector<double> moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link)
2040 {
2041  std::vector<double> result;
2042  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2043  if (eef.empty())
2044  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2045  else
2046  {
2047  robot_state::RobotStatePtr current_state;
2048  if (impl_->getCurrentState(current_state))
2049  {
2050  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2051  if (lm)
2052  {
2053  result.resize(3);
2054  geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2055  double pitch, roll, yaw;
2056  tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2057  result[0] = roll;
2058  result[1] = pitch;
2059  result[2] = yaw;
2060  }
2061  }
2062  }
2063  return result;
2064 }
2065 
2067 {
2068  return impl_->getJointModelGroup()->getActiveJointModelNames();
2069 }
2070 
2071 const std::vector<std::string>& moveit::planning_interface::MoveGroupInterface::getJoints() const
2072 {
2073  return impl_->getJointModelGroup()->getJointModelNames();
2074 }
2075 
2077 {
2078  return impl_->getJointModelGroup()->getVariableCount();
2079 }
2080 
2082 {
2083  robot_state::RobotStatePtr current_state;
2084  impl_->getCurrentState(current_state, wait);
2085  return current_state;
2086 }
2087 
2089  const std::vector<double>& values)
2090 {
2091  remembered_joint_values_[name] = values;
2092 }
2093 
2095 {
2096  remembered_joint_values_.erase(name);
2097 }
2098 
2100 {
2101  impl_->allowLooking(flag);
2102 }
2103 
2105 {
2106  impl_->allowReplanning(flag);
2107 }
2108 
2110 {
2111  return impl_->getKnownConstraints();
2112 }
2113 
2115 {
2116  return impl_->getPathConstraints();
2117 }
2118 
2120 {
2121  return impl_->setPathConstraints(constraint);
2122 }
2123 
2124 void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2125 {
2126  impl_->setPathConstraints(constraint);
2127 }
2128 
2130 {
2132 }
2133 
2135 {
2136  return impl_->getTrajectoryConstraints();
2137 }
2138 
2140  const moveit_msgs::TrajectoryConstraints& constraint)
2141 {
2142  impl_->setTrajectoryConstraints(constraint);
2143 }
2144 
2146 {
2148 }
2149 
2150 void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2151 {
2152  impl_->initializeConstraintsStorage(host, port);
2153 }
2154 
2155 void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx,
2156  double maxy, double maxz)
2157 {
2158  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2159 }
2160 
2163 {
2164  impl_->setPlanningTime(seconds);
2165 }
2166 
2169 {
2170  return impl_->getPlanningTime();
2171 }
2172 
2174 {
2176 }
2177 
2179 {
2180  return impl_->getRobotModel()->getModelFrame();
2181 }
2182 
2184 {
2185  return impl_->getRobotModel()->getJointModelGroupNames();
2186 }
2187 
2188 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2189 {
2190  return attachObject(object, link, std::vector<std::string>());
2191 }
2192 
2193 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2194  const std::vector<std::string>& touch_links)
2195 {
2196  return impl_->attachObject(object, link, touch_links);
2197 }
2198 
2200 {
2201  return impl_->detachObject(name);
2202 }
2203 
2205  moveit_msgs::MotionPlanRequest& goal_out)
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 Sat Jul 11 2020 03:52:28