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  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
581  bool plan_only = false)
582  {
583  std::vector<moveit_msgs::PlaceLocation> locations;
584  for (std::size_t i = 0; i < poses.size(); ++i)
585  {
586  moveit_msgs::PlaceLocation location;
587  location.pre_place_approach.direction.vector.z = -1.0;
588  location.post_place_retreat.direction.vector.x = -1.0;
589  location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
590  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
591 
592  location.pre_place_approach.min_distance = 0.1;
593  location.pre_place_approach.desired_distance = 0.2;
594  location.post_place_retreat.min_distance = 0.0;
595  location.post_place_retreat.desired_distance = 0.2;
596  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
597 
598  location.place_pose = poses[i];
599  locations.push_back(location);
600  }
601  ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
602  (unsigned int)locations.size());
603  return place(object, locations, plan_only);
604  }
605 
606  MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations,
607  bool plan_only = false)
608  {
610  {
611  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
612  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
613  }
614  if (!place_action_client_->isServerConnected())
615  {
616  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
617  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
618  }
619  moveit_msgs::PlaceGoal goal;
620  constructGoal(goal, object);
621  goal.place_locations = locations;
622  goal.planning_options.plan_only = plan_only;
623  goal.planning_options.look_around = can_look_;
624  goal.planning_options.replan = can_replan_;
625  goal.planning_options.replan_delay = replan_delay_;
626  goal.planning_options.planning_scene_diff.is_diff = true;
627  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
628 
629  place_action_client_->sendGoal(goal);
630  ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
631  if (!place_action_client_->waitForResult())
632  {
633  ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
634  }
636  {
637  return MoveItErrorCode(place_action_client_->getResult()->error_code);
638  }
639  else
640  {
641  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
642  << place_action_client_->getState().getText());
643  return MoveItErrorCode(place_action_client_->getResult()->error_code);
644  }
645  }
646 
647  MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps, bool plan_only = false)
648  {
649  if (!pick_action_client_)
650  {
651  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
652  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
653  }
654  if (!pick_action_client_->isServerConnected())
655  {
656  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
657  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
658  }
659  moveit_msgs::PickupGoal goal;
660  constructGoal(goal, object);
661  goal.possible_grasps = grasps;
662  goal.planning_options.plan_only = plan_only;
663  goal.planning_options.look_around = can_look_;
664  goal.planning_options.replan = can_replan_;
665  goal.planning_options.replan_delay = replan_delay_;
666  goal.planning_options.planning_scene_diff.is_diff = true;
667  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
668 
669  pick_action_client_->sendGoal(goal);
670  if (!pick_action_client_->waitForResult())
671  {
672  ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
673  }
675  {
676  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
677  }
678  else
679  {
680  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
681  << pick_action_client_->getState().getText());
682  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
683  }
684  }
685 
686  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
687  {
688  if (object.empty())
689  {
690  return planGraspsAndPick(moveit_msgs::CollisionObject());
691  }
693 
694  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
695 
696  if (objects.empty())
697  {
698  ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '"
699  << object << "', but the object could not be found");
700  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
701  }
702 
703  return planGraspsAndPick(objects[object], plan_only);
704  }
705 
706  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
707  {
709  {
710  ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
711  << GRASP_PLANNING_SERVICE_NAME
712  << "' is not available."
713  " This has to be implemented and started separately.");
714  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
715  }
716 
717  moveit_msgs::GraspPlanning::Request request;
718  moveit_msgs::GraspPlanning::Response response;
719 
720  request.group_name = opt_.group_name_;
721  request.target = object;
722  request.support_surfaces.push_back(support_surface_);
723 
724  ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
725  if (!plan_grasps_service_.call(request, response) ||
726  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
727  {
728  ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
729  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
730  }
731 
732  return pick(object.id, response.grasps, plan_only);
733  }
734 
736  {
737  if (!move_action_client_)
738  {
739  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
740  }
741  if (!move_action_client_->isServerConnected())
742  {
743  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
744  }
745 
746  moveit_msgs::MoveGroupGoal goal;
747  constructGoal(goal);
748  goal.planning_options.plan_only = true;
749  goal.planning_options.look_around = false;
750  goal.planning_options.replan = false;
751  goal.planning_options.planning_scene_diff.is_diff = true;
752  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
753 
754  move_action_client_->sendGoal(goal);
755  if (!move_action_client_->waitForResult())
756  {
757  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
758  }
760  {
761  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
762  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
763  plan.planning_time_ = move_action_client_->getResult()->planning_time;
764  return MoveItErrorCode(move_action_client_->getResult()->error_code);
765  }
766  else
767  {
768  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
769  << move_action_client_->getState().getText());
770  return MoveItErrorCode(move_action_client_->getResult()->error_code);
771  }
772  }
773 
775  {
776  if (!move_action_client_)
777  {
778  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
779  }
780  if (!move_action_client_->isServerConnected())
781  {
782  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
783  }
784 
785  moveit_msgs::MoveGroupGoal goal;
786  constructGoal(goal);
787  goal.planning_options.plan_only = false;
788  goal.planning_options.look_around = can_look_;
789  goal.planning_options.replan = can_replan_;
790  goal.planning_options.replan_delay = replan_delay_;
791  goal.planning_options.planning_scene_diff.is_diff = true;
792  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
793 
794  move_action_client_->sendGoal(goal);
795  if (!wait)
796  {
797  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
798  }
799 
800  if (!move_action_client_->waitForResult())
801  {
802  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
803  }
804 
806  {
807  return MoveItErrorCode(move_action_client_->getResult()->error_code);
808  }
809  else
810  {
811  ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
812  << ": " << move_action_client_->getState().getText());
813  return MoveItErrorCode(move_action_client_->getResult()->error_code);
814  }
815  }
816 
817  MoveItErrorCode execute(const Plan& plan, bool wait)
818  {
819  if (!execute_action_client_->isServerConnected())
820  {
821  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
822  }
823 
824  moveit_msgs::ExecuteTrajectoryGoal goal;
825  goal.trajectory = plan.trajectory_;
826 
827  execute_action_client_->sendGoal(goal);
828  if (!wait)
829  {
830  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
831  }
832 
833  if (!execute_action_client_->waitForResult())
834  {
835  ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early");
836  }
837 
839  {
840  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
841  }
842  else
843  {
844  ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString()
845  << ": " << execute_action_client_->getState().getText());
846  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
847  }
848  }
849 
850  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
851  moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
852  bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
853  {
854  moveit_msgs::GetCartesianPath::Request req;
855  moveit_msgs::GetCartesianPath::Response res;
856 
858  robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
859  else
860  req.start_state.is_diff = true;
861 
862  req.group_name = opt_.group_name_;
863  req.header.frame_id = getPoseReferenceFrame();
864  req.header.stamp = ros::Time::now();
865  req.waypoints = waypoints;
866  req.max_step = step;
867  req.jump_threshold = jump_threshold;
868  req.path_constraints = path_constraints;
869  req.avoid_collisions = avoid_collisions;
870  req.link_name = getEndEffectorLink();
871 
872  if (cartesian_path_service_.call(req, res))
873  {
874  error_code = res.error_code;
875  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
876  {
877  msg = res.solution;
878  return res.fraction;
879  }
880  else
881  return -1.0;
882  }
883  else
884  {
885  error_code.val = error_code.FAILURE;
886  return -1.0;
887  }
888  }
889 
890  void stop()
891  {
893  {
894  std_msgs::String event;
895  event.data = "stop";
897  }
898  }
899 
900  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
901  {
902  std::string l = link.empty() ? getEndEffectorLink() : link;
903  if (l.empty())
904  {
905  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
906  if (!links.empty())
907  l = links[0];
908  }
909  if (l.empty())
910  {
911  ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
912  return false;
913  }
914  moveit_msgs::AttachedCollisionObject aco;
915  aco.object.id = object;
916  aco.link_name.swap(l);
917  if (touch_links.empty())
918  aco.touch_links.push_back(aco.link_name);
919  else
920  aco.touch_links = touch_links;
921  aco.object.operation = moveit_msgs::CollisionObject::ADD;
923  return true;
924  }
925 
926  bool detachObject(const std::string& name)
927  {
928  moveit_msgs::AttachedCollisionObject aco;
929  // if name is a link
930  if (!name.empty() && joint_model_group_->hasLinkModel(name))
931  aco.link_name = name;
932  else
933  aco.object.id = name;
934  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
935  if (aco.link_name.empty() && aco.object.id.empty())
936  {
937  // we only want to detach objects for this group
938  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
939  for (std::size_t i = 0; i < lnames.size(); ++i)
940  {
941  aco.link_name = lnames[i];
943  }
944  }
945  else
947  return true;
948  }
949 
951  {
953  }
954 
956  {
958  }
959 
960  double getGoalJointTolerance() const
961  {
962  return goal_joint_tolerance_;
963  }
964 
965  void setGoalJointTolerance(double tolerance)
966  {
967  goal_joint_tolerance_ = tolerance;
968  }
969 
970  void setGoalPositionTolerance(double tolerance)
971  {
972  goal_position_tolerance_ = tolerance;
973  }
974 
975  void setGoalOrientationTolerance(double tolerance)
976  {
977  goal_orientation_tolerance_ = tolerance;
978  }
979 
980  void setPlanningTime(double seconds)
981  {
982  if (seconds > 0.0)
983  allowed_planning_time_ = seconds;
984  }
985 
986  double getPlanningTime() const
987  {
988  return allowed_planning_time_;
989  }
990 
991  void allowLooking(bool flag)
992  {
993  can_look_ = flag;
994  ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
995  }
996 
997  void allowReplanning(bool flag)
998  {
999  can_replan_ = flag;
1000  ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
1001  }
1002 
1003  void setReplanningDelay(double delay)
1004  {
1005  if (delay >= 0.0)
1006  replan_delay_ = delay;
1007  }
1008 
1009  double getReplanningDelay() const
1010  {
1011  return replan_delay_;
1012  }
1013 
1014  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request)
1015  {
1016  request.group_name = opt_.group_name_;
1017  request.num_planning_attempts = num_planning_attempts_;
1018  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1019  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1020  request.allowed_planning_time = allowed_planning_time_;
1021  request.planner_id = planner_id_;
1022  request.workspace_parameters = workspace_parameters_;
1023 
1025  robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
1026  else
1027  request.start_state.is_diff = true;
1028 
1029  if (active_target_ == JOINT)
1030  {
1031  request.goal_constraints.resize(1);
1032  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1034  }
1035  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1036  {
1037  // find out how many goals are specified
1038  std::size_t goal_count = 0;
1039  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1040  it != pose_targets_.end(); ++it)
1041  goal_count = std::max(goal_count, it->second.size());
1042 
1043  // start filling the goals;
1044  // each end effector has a number of possible poses (K) as valid goals
1045  // but there could be multiple end effectors specified, so we want each end effector
1046  // to reach the goal that corresponds to the goals of the other end effectors
1047  request.goal_constraints.resize(goal_count);
1048 
1049  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1050  it != pose_targets_.end(); ++it)
1051  {
1052  for (std::size_t i = 0; i < it->second.size(); ++i)
1053  {
1054  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1055  it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1056  if (active_target_ == ORIENTATION)
1057  c.position_constraints.clear();
1058  if (active_target_ == POSITION)
1059  c.orientation_constraints.clear();
1060  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1061  }
1062  }
1063  }
1064  else
1065  ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation");
1066 
1067  if (path_constraints_)
1068  request.path_constraints = *path_constraints_;
1070  request.trajectory_constraints = *trajectory_constraints_;
1071  }
1072 
1073  void constructGoal(moveit_msgs::MoveGroupGoal& goal)
1074  {
1075  constructMotionPlanRequest(goal.request);
1076  }
1077 
1078  void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object)
1079  {
1080  moveit_msgs::PickupGoal goal;
1081  goal.target_name = object;
1082  goal.group_name = opt_.group_name_;
1083  goal.end_effector = getEndEffector();
1084  goal.allowed_planning_time = allowed_planning_time_;
1085  goal.support_surface_name = support_surface_;
1086  goal.planner_id = planner_id_;
1087  if (!support_surface_.empty())
1088  goal.allow_gripper_support_collision = true;
1089 
1090  if (path_constraints_)
1091  goal.path_constraints = *path_constraints_;
1092 
1093  goal_out = goal;
1094  }
1095 
1096  void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object)
1097  {
1098  moveit_msgs::PlaceGoal goal;
1099  goal.attached_object_name = object;
1100  goal.group_name = opt_.group_name_;
1101  goal.allowed_planning_time = allowed_planning_time_;
1102  goal.support_surface_name = support_surface_;
1103  goal.planner_id = planner_id_;
1104  if (!support_surface_.empty())
1105  goal.allow_gripper_support_collision = true;
1106 
1107  if (path_constraints_)
1108  goal.path_constraints = *path_constraints_;
1109 
1110  goal_out = goal;
1111  }
1112 
1113  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1114  {
1115  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1116  }
1117 
1118  bool setPathConstraints(const std::string& constraint)
1119  {
1121  {
1123  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1124  {
1125  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1126  return true;
1127  }
1128  else
1129  return false;
1130  }
1131  else
1132  return false;
1133  }
1134 
1136  {
1137  path_constraints_.reset();
1138  }
1139 
1140  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
1141  {
1142  trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint));
1143  }
1144 
1146  {
1147  trajectory_constraints_.reset();
1148  }
1149 
1150  std::vector<std::string> getKnownConstraints() const
1151  {
1153  {
1154  static ros::WallDuration d(0.01);
1155  d.sleep();
1156  }
1157 
1158  std::vector<std::string> c;
1160  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1161 
1162  return c;
1163  }
1164 
1165  moveit_msgs::Constraints getPathConstraints() const
1166  {
1167  if (path_constraints_)
1168  return *path_constraints_;
1169  else
1170  return moveit_msgs::Constraints();
1171  }
1172 
1173  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
1174  {
1176  return *trajectory_constraints_;
1177  else
1178  return moveit_msgs::TrajectoryConstraints();
1179  }
1180 
1181  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1182  {
1185  constraints_init_thread_->join();
1187  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1188  }
1189 
1190  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1191  {
1192  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1193  workspace_parameters_.header.stamp = ros::Time::now();
1194  workspace_parameters_.min_corner.x = minx;
1195  workspace_parameters_.min_corner.y = miny;
1196  workspace_parameters_.min_corner.z = minz;
1197  workspace_parameters_.max_corner.x = maxx;
1198  workspace_parameters_.max_corner.y = maxy;
1199  workspace_parameters_.max_corner.z = maxz;
1200  }
1201 
1202 private:
1203  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1204  {
1205  // Set up db
1206  try
1207  {
1209  conn->setParams(host, port);
1210  if (conn->connect())
1211  {
1213  }
1214  }
1215  catch (std::exception& ex)
1216  {
1217  ROS_ERROR_NAMED("move_group_interface", "%s", ex.what());
1218  }
1219  initializing_constraints_ = false;
1220  }
1221 
1224  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
1225  robot_model::RobotModelConstPtr robot_model_;
1226  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1227  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1228  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1229  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1230  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1231 
1232  // general planning params
1233  robot_state::RobotStatePtr considered_start_state_;
1234  moveit_msgs::WorkspaceParameters workspace_parameters_;
1236  std::string planner_id_;
1246 
1247  // joint state goal
1248  robot_state::RobotStatePtr joint_state_target_;
1249  const robot_model::JointModelGroup* joint_model_group_;
1250 
1251  // pose goal;
1252  // for each link we have a set of possible goal locations;
1253  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1254 
1255  // common properties for goals
1256  ActiveTargetType active_target_;
1257  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1258  std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
1259  std::string end_effector_link_;
1261  std::string support_surface_;
1262 
1263  // ROS communication
1271  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1272  std::unique_ptr<boost::thread> constraints_init_thread_;
1274 };
1275 } // namespace planning_interface
1276 } // namespace moveit
1277 
1279  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1280  const ros::WallDuration& wait_for_servers)
1281 {
1282  if (!ros::ok())
1283  throw std::runtime_error("ROS does not seem to be running");
1284  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1285 }
1286 
1288  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1289  const ros::Duration& wait_for_servers)
1290  : MoveGroupInterface(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1291 {
1292 }
1293 
1295  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
1296  const ros::WallDuration& wait_for_servers)
1297 {
1298  impl_ = new MoveGroupInterfaceImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers);
1299 }
1300 
1303  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const ros::Duration& wait_for_servers)
1304  : MoveGroupInterface(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec()))
1305 {
1306 }
1307 
1309 {
1310  delete impl_;
1311 }
1312 
1315 {
1316  other.impl_ = nullptr;
1317 }
1318 
1321 {
1322  if (this != &other)
1323  {
1324  delete impl_;
1325  impl_ = other.impl_;
1326  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1327  other.impl_ = nullptr;
1328  }
1329 
1330  return *this;
1331 }
1332 
1334 {
1335  return impl_->getOptions().group_name_;
1336 }
1337 
1339 {
1340  const robot_model::RobotModelConstPtr& robot = getRobotModel();
1341  const std::string& group = getName();
1342  const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1343 
1344  if (joint_group)
1345  {
1346  return joint_group->getDefaultStateNames();
1347  }
1348 
1349  std::vector<std::string> empty;
1350  return empty;
1351 }
1352 
1354 {
1355  return impl_->getRobotModel();
1356 }
1357 
1359 {
1360  return impl_->getOptions().node_handle_;
1361 }
1362 
1364  moveit_msgs::PlannerInterfaceDescription& desc)
1365 {
1366  return impl_->getInterfaceDescription(desc);
1367 }
1368 
1370  const std::string& planner_id, const std::string& group)
1371 {
1372  return impl_->getPlannerParams(planner_id, group);
1373 }
1374 
1376  const std::string& group,
1377  const std::map<std::string, std::string>& params,
1378  bool replace)
1379 {
1380  impl_->setPlannerParams(planner_id, group, params, replace);
1381 }
1382 
1384 {
1385  return impl_->getDefaultPlannerId(group);
1386 }
1387 
1389 {
1390  impl_->setPlannerId(planner_id);
1391 }
1392 
1394 {
1395  return impl_->getPlannerId();
1396 }
1397 
1399 {
1400  impl_->setNumPlanningAttempts(num_planning_attempts);
1401 }
1402 
1404 {
1405  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1406 }
1407 
1409  double max_acceleration_scaling_factor)
1410 {
1411  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1412 }
1413 
1415 {
1416  return impl_->move(false);
1417 }
1418 
1421 {
1422  return impl_->getMoveGroupClient();
1423 }
1424 
1426 {
1427  return impl_->move(true);
1428 }
1429 
1432 {
1433  return impl_->execute(plan, false);
1434 }
1435 
1437 {
1438  return impl_->execute(plan, true);
1439 }
1440 
1442 {
1443  return impl_->plan(plan);
1444 }
1445 
1447 moveit::planning_interface::MoveGroupInterface::pick(const std::string& object, bool plan_only)
1448 {
1449  return impl_->pick(object, std::vector<moveit_msgs::Grasp>(), plan_only);
1450 }
1451 
1453  const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only)
1454 {
1455  return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp), plan_only);
1456 }
1457 
1459  const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps, bool plan_only)
1460 {
1461  return impl_->pick(object, grasps, plan_only);
1462 }
1463 
1466 {
1467  return impl_->planGraspsAndPick(object, plan_only);
1468 }
1469 
1471  const moveit_msgs::CollisionObject& object, bool plan_only)
1472 {
1473  return impl_->planGraspsAndPick(object, plan_only);
1474 }
1475 
1477 moveit::planning_interface::MoveGroupInterface::place(const std::string& object, bool plan_only)
1478 {
1479  return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>(), plan_only);
1480 }
1481 
1483  const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations, bool plan_only)
1484 {
1485  return impl_->place(object, locations, plan_only);
1486 }
1487 
1489  const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses, bool plan_only)
1490 {
1491  return impl_->place(object, poses, plan_only);
1492 }
1493 
1495  const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only)
1496 {
1497  return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose), plan_only);
1498 }
1499 
1501  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1502  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1503 {
1504  moveit_msgs::Constraints path_constraints_tmp;
1505  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1506  error_code);
1507 }
1508 
1510  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1511  moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1512  moveit_msgs::MoveItErrorCodes* error_code)
1513 {
1514  if (error_code)
1515  {
1516  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1517  avoid_collisions, *error_code);
1518  }
1519  else
1520  {
1521  moveit_msgs::MoveItErrorCodes error_code_tmp;
1522  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1523  avoid_collisions, error_code_tmp);
1524  }
1525 }
1526 
1528 {
1529  impl_->stop();
1530 }
1531 
1532 void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1533 {
1534  robot_state::RobotStatePtr rs;
1535  impl_->getCurrentState(rs);
1536  robot_state::robotStateMsgToRobotState(start_state, *rs);
1537  setStartState(*rs);
1538 }
1539 
1540 void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state)
1541 {
1542  impl_->setStartState(start_state);
1543 }
1544 
1546 {
1548 }
1549 
1551 {
1552  impl_->getJointStateTarget().setToRandomPositions();
1553  impl_->setTargetType(JOINT);
1554 }
1555 
1557 {
1558  return impl_->getJointModelGroup()->getVariableNames();
1559 }
1560 
1562 {
1563  return impl_->getJointModelGroup()->getLinkModelNames();
1564 }
1565 
1566 std::map<std::string, double>
1568 {
1569  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1570  std::map<std::string, double> positions;
1571 
1572  if (it != remembered_joint_values_.end())
1573  {
1574  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1575  for (size_t x = 0; x < names.size(); ++x)
1576  {
1577  positions[names[x]] = it->second[x];
1578  }
1579  }
1580  else
1581  {
1582  impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
1583  }
1584  return positions;
1585 }
1586 
1588 {
1589  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1590  if (it != remembered_joint_values_.end())
1591  {
1592  return setJointValueTarget(it->second);
1593  }
1594  else
1595  {
1596  if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
1597  {
1598  impl_->setTargetType(JOINT);
1599  return true;
1600  }
1601  ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
1602  return false;
1603  }
1604 }
1605 
1606 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1607 {
1608  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
1609  return false;
1610  impl_->setTargetType(JOINT);
1611  impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1613 }
1614 
1616  const std::map<std::string, double>& joint_values)
1617 {
1618  impl_->setTargetType(JOINT);
1619  impl_->getJointStateTarget().setVariablePositions(joint_values);
1620  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1621 }
1622 
1624 {
1625  impl_->setTargetType(JOINT);
1626  impl_->getJointStateTarget() = rstate;
1627  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1628 }
1629 
1630 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1631 {
1632  std::vector<double> values(1, value);
1633  return setJointValueTarget(joint_name, values);
1634 }
1635 
1637  const std::vector<double>& values)
1638 {
1639  impl_->setTargetType(JOINT);
1640  const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
1641  if (jm && jm->getVariableCount() == values.size())
1642  {
1643  impl_->getJointStateTarget().setJointPositions(jm, values);
1644  return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1645  }
1646  return false;
1647 }
1648 
1650 {
1651  impl_->setTargetType(JOINT);
1652  impl_->getJointStateTarget().setVariableValues(state);
1653  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1654 }
1655 
1657  const std::string& end_effector_link)
1658 {
1659  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1660 }
1661 
1662 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1663  const std::string& end_effector_link)
1664 {
1665  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1666 }
1667 
1669  const std::string& end_effector_link)
1670 {
1671  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1672  return setJointValueTarget(msg, end_effector_link);
1673 }
1674 
1676  const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link)
1677 {
1678  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1679 }
1680 
1682  const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link)
1683 {
1684  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1685 }
1686 
1688  const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link)
1689 {
1690  geometry_msgs::Pose msg = tf2::toMsg(eef_pose);
1691  return setApproximateJointValueTarget(msg, end_effector_link);
1692 }
1693 
1695 {
1696  return impl_->getJointStateTarget();
1697 }
1698 
1700 {
1701  return impl_->getEndEffectorLink();
1702 }
1703 
1705 {
1706  return impl_->getEndEffector();
1707 }
1708 
1710 {
1711  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1712  return false;
1713  impl_->setEndEffectorLink(link_name);
1714  impl_->setTargetType(POSE);
1715  return true;
1716 }
1717 
1719 {
1720  const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1721  if (jmg)
1722  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1723  return false;
1724 }
1725 
1727 {
1728  impl_->clearPoseTarget(end_effector_link);
1729 }
1730 
1732 {
1734 }
1735 
1737  const std::string& end_effector_link)
1738 {
1739  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1740  pose_msg[0].pose = tf2::toMsg(pose);
1741  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1742  pose_msg[0].header.stamp = ros::Time::now();
1743  return setPoseTargets(pose_msg, end_effector_link);
1744 }
1745 
1747  const std::string& end_effector_link)
1748 {
1749  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1750  pose_msg[0].pose = target;
1751  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1752  pose_msg[0].header.stamp = ros::Time::now();
1753  return setPoseTargets(pose_msg, end_effector_link);
1754 }
1755 
1756 bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target,
1757  const std::string& end_effector_link)
1758 {
1759  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1760  return setPoseTargets(targets, end_effector_link);
1761 }
1762 
1764  const std::string& end_effector_link)
1765 {
1766  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1767  ros::Time tm = ros::Time::now();
1768  const std::string& frame_id = getPoseReferenceFrame();
1769  for (std::size_t i = 0; i < target.size(); ++i)
1770  {
1771  pose_out[i].pose = tf2::toMsg(target[i]);
1772  pose_out[i].header.stamp = tm;
1773  pose_out[i].header.frame_id = frame_id;
1774  }
1775  return setPoseTargets(pose_out, end_effector_link);
1776 }
1777 
1778 bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1779  const std::string& end_effector_link)
1780 {
1781  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1782  ros::Time tm = ros::Time::now();
1783  const std::string& frame_id = getPoseReferenceFrame();
1784  for (std::size_t i = 0; i < target.size(); ++i)
1785  {
1786  target_stamped[i].pose = target[i];
1787  target_stamped[i].header.stamp = tm;
1788  target_stamped[i].header.frame_id = frame_id;
1789  }
1790  return setPoseTargets(target_stamped, end_effector_link);
1791 }
1792 
1794  const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link)
1795 {
1796  if (target.empty())
1797  {
1798  ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
1799  return false;
1800  }
1801  else
1802  {
1803  impl_->setTargetType(POSE);
1804  return impl_->setPoseTargets(target, end_effector_link);
1805  }
1806 }
1807 
1808 const geometry_msgs::PoseStamped&
1809 moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1810 {
1811  return impl_->getPoseTarget(end_effector_link);
1812 }
1813 
1814 const std::vector<geometry_msgs::PoseStamped>&
1815 moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1816 {
1817  return impl_->getPoseTargets(end_effector_link);
1818 }
1819 
1820 namespace
1821 {
1822 inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame,
1823  geometry_msgs::PoseStamped& target)
1824 {
1825  if (desired_frame != target.header.frame_id)
1826  {
1827  geometry_msgs::PoseStamped target_in(target);
1828  tf_buffer.transform(target_in, target, desired_frame);
1829  // we leave the stamp to ros::Time(0) on purpose
1830  target.header.stamp = ros::Time(0);
1831  }
1832 }
1833 } // namespace
1834 
1836  const std::string& end_effector_link)
1837 {
1838  geometry_msgs::PoseStamped target;
1839  if (impl_->hasPoseTarget(end_effector_link))
1840  {
1841  target = getPoseTarget(end_effector_link);
1842  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1843  }
1844  else
1845  {
1846  target.pose.orientation.x = 0.0;
1847  target.pose.orientation.y = 0.0;
1848  target.pose.orientation.z = 0.0;
1849  target.pose.orientation.w = 1.0;
1850  target.header.frame_id = impl_->getPoseReferenceFrame();
1851  }
1852 
1853  target.pose.position.x = x;
1854  target.pose.position.y = y;
1855  target.pose.position.z = z;
1856  bool result = setPoseTarget(target, end_effector_link);
1857  impl_->setTargetType(POSITION);
1858  return result;
1859 }
1860 
1862  const std::string& end_effector_link)
1863 {
1864  geometry_msgs::PoseStamped target;
1865  if (impl_->hasPoseTarget(end_effector_link))
1866  {
1867  target = getPoseTarget(end_effector_link);
1868  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1869  }
1870  else
1871  {
1872  target.pose.position.x = 0.0;
1873  target.pose.position.y = 0.0;
1874  target.pose.position.z = 0.0;
1875  target.header.frame_id = impl_->getPoseReferenceFrame();
1876  }
1877  tf2::Quaternion q;
1878  q.setRPY(r, p, y);
1879  target.pose.orientation = tf2::toMsg(q);
1880  bool result = setPoseTarget(target, end_effector_link);
1881  impl_->setTargetType(ORIENTATION);
1882  return result;
1883 }
1884 
1886  const std::string& end_effector_link)
1887 {
1888  geometry_msgs::PoseStamped target;
1889  if (impl_->hasPoseTarget(end_effector_link))
1890  {
1891  target = getPoseTarget(end_effector_link);
1892  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1893  }
1894  else
1895  {
1896  target.pose.position.x = 0.0;
1897  target.pose.position.y = 0.0;
1898  target.pose.position.z = 0.0;
1899  target.header.frame_id = impl_->getPoseReferenceFrame();
1900  }
1901 
1902  target.pose.orientation.x = x;
1903  target.pose.orientation.y = y;
1904  target.pose.orientation.z = z;
1905  target.pose.orientation.w = w;
1906  bool result = setPoseTarget(target, end_effector_link);
1907  impl_->setTargetType(ORIENTATION);
1908  return result;
1909 }
1910 
1912 {
1913  impl_->setPoseReferenceFrame(pose_reference_frame);
1914 }
1915 
1917 {
1918  return impl_->getPoseReferenceFrame();
1919 }
1920 
1922 {
1923  return impl_->getGoalJointTolerance();
1924 }
1925 
1927 {
1928  return impl_->getGoalPositionTolerance();
1929 }
1930 
1932 {
1934 }
1935 
1937 {
1938  setGoalJointTolerance(tolerance);
1939  setGoalPositionTolerance(tolerance);
1940  setGoalOrientationTolerance(tolerance);
1941 }
1942 
1944 {
1945  impl_->setGoalJointTolerance(tolerance);
1946 }
1947 
1949 {
1950  impl_->setGoalPositionTolerance(tolerance);
1951 }
1952 
1954 {
1955  impl_->setGoalOrientationTolerance(tolerance);
1956 }
1957 
1959 {
1961 }
1962 
1964 {
1965  return impl_->startStateMonitor(wait);
1966 }
1967 
1969 {
1970  robot_state::RobotStatePtr current_state;
1971  std::vector<double> values;
1972  if (impl_->getCurrentState(current_state))
1973  current_state->copyJointGroupPositions(getName(), values);
1974  return values;
1975 }
1976 
1978 {
1979  std::vector<double> r;
1980  impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
1981  return r;
1982 }
1983 
1984 geometry_msgs::PoseStamped
1986 {
1987  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
1988  Eigen::Isometry3d pose;
1989  pose.setIdentity();
1990  if (eef.empty())
1991  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
1992  else
1993  {
1994  robot_state::RobotStatePtr current_state;
1995  if (impl_->getCurrentState(current_state))
1996  {
1997  current_state->setToRandomPositions(impl_->getJointModelGroup());
1998  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
1999  if (lm)
2000  pose = current_state->getGlobalLinkTransform(lm);
2001  }
2002  }
2003  geometry_msgs::PoseStamped pose_msg;
2004  pose_msg.header.stamp = ros::Time::now();
2005  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2006  pose_msg.pose = tf2::toMsg(pose);
2007  return pose_msg;
2008 }
2009 
2010 geometry_msgs::PoseStamped
2012 {
2013  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2014  Eigen::Isometry3d pose;
2015  pose.setIdentity();
2016  if (eef.empty())
2017  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2018  else
2019  {
2020  robot_state::RobotStatePtr current_state;
2021  if (impl_->getCurrentState(current_state))
2022  {
2023  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2024  if (lm)
2025  pose = current_state->getGlobalLinkTransform(lm);
2026  }
2027  }
2028  geometry_msgs::PoseStamped pose_msg;
2029  pose_msg.header.stamp = ros::Time::now();
2030  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2031  pose_msg.pose = tf2::toMsg(pose);
2032  return pose_msg;
2033 }
2034 
2035 std::vector<double> moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link)
2036 {
2037  std::vector<double> result;
2038  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2039  if (eef.empty())
2040  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2041  else
2042  {
2043  robot_state::RobotStatePtr current_state;
2044  if (impl_->getCurrentState(current_state))
2045  {
2046  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2047  if (lm)
2048  {
2049  result.resize(3);
2050  geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));
2051  double pitch, roll, yaw;
2052  tf2::getEulerYPR<geometry_msgs::Quaternion>(tfs.transform.rotation, yaw, pitch, roll);
2053  result[0] = roll;
2054  result[1] = pitch;
2055  result[2] = yaw;
2056  }
2057  }
2058  }
2059  return result;
2060 }
2061 
2063 {
2064  return impl_->getJointModelGroup()->getActiveJointModelNames();
2065 }
2066 
2067 const std::vector<std::string>& moveit::planning_interface::MoveGroupInterface::getJoints() const
2068 {
2069  return impl_->getJointModelGroup()->getJointModelNames();
2070 }
2071 
2073 {
2074  return impl_->getJointModelGroup()->getVariableCount();
2075 }
2076 
2078 {
2079  robot_state::RobotStatePtr current_state;
2080  impl_->getCurrentState(current_state, wait);
2081  return current_state;
2082 }
2083 
2085  const std::vector<double>& values)
2086 {
2087  remembered_joint_values_[name] = values;
2088 }
2089 
2091 {
2092  remembered_joint_values_.erase(name);
2093 }
2094 
2096 {
2097  impl_->allowLooking(flag);
2098 }
2099 
2101 {
2102  impl_->allowReplanning(flag);
2103 }
2104 
2106 {
2107  return impl_->getKnownConstraints();
2108 }
2109 
2111 {
2112  return impl_->getPathConstraints();
2113 }
2114 
2116 {
2117  return impl_->setPathConstraints(constraint);
2118 }
2119 
2120 void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2121 {
2122  impl_->setPathConstraints(constraint);
2123 }
2124 
2126 {
2128 }
2129 
2131 {
2132  return impl_->getTrajectoryConstraints();
2133 }
2134 
2136  const moveit_msgs::TrajectoryConstraints& constraint)
2137 {
2138  impl_->setTrajectoryConstraints(constraint);
2139 }
2140 
2142 {
2144 }
2145 
2146 void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2147 {
2148  impl_->initializeConstraintsStorage(host, port);
2149 }
2150 
2151 void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx,
2152  double maxy, double maxz)
2153 {
2154  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2155 }
2156 
2159 {
2160  impl_->setPlanningTime(seconds);
2161 }
2162 
2165 {
2166  return impl_->getPlanningTime();
2167 }
2168 
2170 {
2172 }
2173 
2175 {
2176  return impl_->getRobotModel()->getModelFrame();
2177 }
2178 
2180 {
2181  return impl_->getRobotModel()->getJointModelGroupNames();
2182 }
2183 
2184 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2185 {
2186  return attachObject(object, link, std::vector<std::string>());
2187 }
2188 
2189 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2190  const std::vector<std::string>& touch_links)
2191 {
2192  return impl_->attachObject(object, link, touch_links);
2193 }
2194 
2196 {
2197  return impl_->detachObject(name);
2198 }
2199 
2201  moveit_msgs::MotionPlanRequest& goal_out)
2202 {
2203  impl_->constructMotionPlanRequest(goal_out);
2204 }
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;.
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 ...
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.
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
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< double > getCurrentJointValues()
Get the current joint values for the joints planned for by this instance (see getJoints()) ...
XmlRpcServer s
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 remebered 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...
MoveItErrorCode place(const std::string &object, const std::vector< moveit_msgs::PlaceLocation > &locations, bool plan_only=false)
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)
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. Return true on success.
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)
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()
MoveItErrorCode pick(const std::string &object, const std::vector< moveit_msgs::Grasp > &grasps, bool plan_only=false)
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. Return true on success.
MoveItErrorCode place(const std::string &object, bool plan_only=false)
Place an object somewhere safe in the world (a safe location will be detected)
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,...)
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.
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 constructGoal(moveit_msgs::PickupGoal &goal_out, const std::string &object)
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
void constructGoal(moveit_msgs::PlaceGoal &goal_out, const std::string &object)
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)
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
MoveItErrorCode place(const std::string &object, const std::vector< geometry_msgs::PoseStamped > &poses, bool plan_only=false)
Place an object at one of the specified possible locations.
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
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.
r
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.
#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 Mar 11 2019 02:58:04