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


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Nov 14 2018 03:59:17