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 
66 #include <std_msgs/String.h>
67 #include <tf/transform_listener.h>
68 #include <tf/transform_datatypes.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:
97  const ros::WallDuration& wait_for_servers)
98  : opt_(opt), node_handle_(opt.node_handle_), tf_(tf)
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  // TODO: after deprecation period, i.e. for L-turtle, switch back to standard waitForAction function
164  // waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time);
165  waitForExecuteActionOrService(timeout_for_servers);
166 
168  node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
173 
176 
178 
179  ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_
180  << ".");
181  }
182 
183  template <typename T>
184  void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time)
185  {
186  ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str());
187 
188  // wait for the server (and spin as needed)
189  if (timeout == ros::WallTime()) // wait forever
190  {
191  while (node_handle_.ok() && !action->isServerConnected())
192  {
193  ros::WallDuration(0.001).sleep();
194  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
196  if (queue)
197  {
198  queue->callAvailable();
199  }
200  else // in case of nodelets and specific callback queue implementations
201  {
202  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
203  "handling.");
204  }
205  }
206  }
207  else // wait with timeout
208  {
209  while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now())
210  {
211  ros::WallDuration(0.001).sleep();
212  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
214  if (queue)
215  {
216  queue->callAvailable();
217  }
218  else // in case of nodelets and specific callback queue implementations
219  {
220  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
221  "handling.");
222  }
223  }
224  }
225 
226  if (!action->isServerConnected())
227  {
228  std::stringstream error;
229  error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time
230  << "s)";
231  throw std::runtime_error(error.str());
232  }
233  else
234  {
235  ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str());
236  }
237  }
238 
240  {
241  ROS_DEBUG("Waiting for move_group action server (%s)...", move_group::EXECUTE_ACTION_NAME.c_str());
242 
243  // Deprecated service
245  node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
246 
247  // wait for either of action or service
248  if (timeout == ros::WallTime()) // wait forever
249  {
250  while (!execute_action_client_->isServerConnected() && !execute_service_.exists())
251  {
252  ros::WallDuration(0.001).sleep();
253  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
255  if (queue)
256  {
257  queue->callAvailable();
258  }
259  else // in case of nodelets and specific callback queue implementations
260  {
261  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
262  "handling.");
263  }
264  }
265  }
266  else // wait with timeout
267  {
268  while (!execute_action_client_->isServerConnected() && !execute_service_.exists() &&
269  timeout > ros::WallTime::now())
270  {
271  ros::WallDuration(0.001).sleep();
272  // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
274  if (queue)
275  {
276  queue->callAvailable();
277  }
278  else // in case of nodelets and specific callback queue implementations
279  {
280  ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue "
281  "handling.");
282  }
283  }
284  }
285 
286  // issue warning
287  if (!execute_action_client_->isServerConnected())
288  {
289  if (execute_service_.exists())
290  {
291  ROS_WARN_NAMED("move_group_interface",
292  "\nDeprecation warning: Trajectory execution service is deprecated (was replaced by an action)."
293  "\nReplace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in "
294  "move_group.launch");
295  }
296  else
297  {
298  ROS_ERROR_STREAM_NAMED("move_group_interface",
299  "Unable to find execution action on topic: "
302  throw std::runtime_error("No Trajectory execution capability available.");
303  }
304  execute_action_client_.reset();
305  }
306  }
307 
309  {
311  constraints_init_thread_->join();
312  }
313 
315  {
316  return tf_;
317  }
318 
319  const Options& getOptions() const
320  {
321  return opt_;
322  }
323 
324  const robot_model::RobotModelConstPtr& getRobotModel() const
325  {
326  return robot_model_;
327  }
328 
329  const robot_model::JointModelGroup* getJointModelGroup() const
330  {
331  return joint_model_group_;
332  }
333 
334  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc)
335  {
336  moveit_msgs::QueryPlannerInterfaces::Request req;
337  moveit_msgs::QueryPlannerInterfaces::Response res;
338  if (query_service_.call(req, res))
339  if (!res.planner_interfaces.empty())
340  {
341  desc = res.planner_interfaces.front();
342  return true;
343  }
344  return false;
345  }
346 
347  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "")
348  {
349  moveit_msgs::GetPlannerParams::Request req;
350  moveit_msgs::GetPlannerParams::Response res;
351  req.planner_config = planner_id;
352  req.group = group;
353  std::map<std::string, std::string> result;
354  if (get_params_service_.call(req, res))
355  {
356  for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i)
357  result[res.params.keys[i]] = res.params.values[i];
358  }
359  return result;
360  }
361 
362  void setPlannerParams(const std::string& planner_id, const std::string& group,
363  const std::map<std::string, std::string>& params, bool replace = false)
364  {
365  moveit_msgs::SetPlannerParams::Request req;
366  moveit_msgs::SetPlannerParams::Response res;
367  req.planner_config = planner_id;
368  req.group = group;
369  req.replace = replace;
370  for (std::map<std::string, std::string>::const_iterator it = params.begin(), end = params.end(); it != end; ++it)
371  {
372  req.params.keys.push_back(it->first);
373  req.params.values.push_back(it->second);
374  }
375  set_params_service_.call(req, res);
376  }
377 
378  std::string getDefaultPlannerId(const std::string& group) const
379  {
380  std::stringstream param_name;
381  param_name << "move_group";
382  if (!group.empty())
383  param_name << "/" << group;
384  param_name << "/default_planner_config";
385 
386  std::string default_planner_config;
387  node_handle_.getParam(param_name.str(), default_planner_config);
388  return default_planner_config;
389  }
390 
391  void setPlannerId(const std::string& planner_id)
392  {
393  planner_id_ = planner_id;
394  }
395 
396  const std::string& getPlannerId() const
397  {
398  return planner_id_;
399  }
400 
401  void setNumPlanningAttempts(unsigned int num_planning_attempts)
402  {
403  num_planning_attempts_ = num_planning_attempts;
404  }
405 
406  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
407  {
408  max_velocity_scaling_factor_ = max_velocity_scaling_factor;
409  }
410 
411  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
412  {
413  max_acceleration_scaling_factor_ = max_acceleration_scaling_factor;
414  }
415 
416  robot_state::RobotState& getJointStateTarget()
417  {
418  return *joint_state_target_;
419  }
420 
421  void setStartState(const robot_state::RobotState& start_state)
422  {
423  considered_start_state_.reset(new robot_state::RobotState(start_state));
424  }
425 
427  {
428  considered_start_state_.reset();
429  }
430 
431  robot_state::RobotStatePtr getStartState()
432  {
435  else
436  {
437  robot_state::RobotStatePtr s;
438  getCurrentState(s);
439  return s;
440  }
441  }
442 
443  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link,
444  const std::string& frame, bool approx)
445  {
446  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
447  // this only works if we have an end-effector
448  if (!eef.empty())
449  {
450  // first we set the goal to be the same as the start state
451  moveit::core::RobotStatePtr c = getStartState();
452  if (c)
453  {
454  setTargetType(JOINT);
455  c->enforceBounds();
456  getJointStateTarget() = *c;
457  if (!getJointStateTarget().satisfiesBounds(getGoalJointTolerance()))
458  return false;
459  }
460  else
461  return false;
462 
463  // we may need to do approximate IK
465  o.return_approximate_solution = approx;
466 
467  // if no frame transforms are needed, call IK directly
468  if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame()))
469  return getJointStateTarget().setFromIK(getJointModelGroup(), eef_pose, eef, 0, 0.0,
471  else
472  {
473  if (c->knowsFrameTransform(frame))
474  {
475  // transform the pose first if possible, then do IK
476  const Eigen::Affine3d& t = getJointStateTarget().getFrameTransform(frame);
477  Eigen::Affine3d p;
478  tf::poseMsgToEigen(eef_pose, p);
479  return getJointStateTarget().setFromIK(getJointModelGroup(), t * p, eef, 0, 0.0,
481  }
482  else
483  {
484  ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(),
485  getRobotModel()->getModelFrame().c_str());
486  return false;
487  }
488  }
489  }
490  else
491  return false;
492  }
493 
494  void setEndEffectorLink(const std::string& end_effector)
495  {
496  end_effector_link_ = end_effector;
497  }
498 
499  void clearPoseTarget(const std::string& end_effector_link)
500  {
501  pose_targets_.erase(end_effector_link);
502  }
503 
505  {
506  pose_targets_.clear();
507  }
508 
509  const std::string& getEndEffectorLink() const
510  {
511  return end_effector_link_;
512  }
513 
514  const std::string& getEndEffector() const
515  {
516  if (!end_effector_link_.empty())
517  {
518  const std::vector<std::string>& possible_eefs =
519  getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames();
520  for (std::size_t i = 0; i < possible_eefs.size(); ++i)
521  if (getRobotModel()->getEndEffector(possible_eefs[i])->hasLinkModel(end_effector_link_))
522  return possible_eefs[i];
523  }
524  static std::string empty;
525  return empty;
526  }
527 
528  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& end_effector_link)
529  {
530  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
531  if (eef.empty())
532  {
533  ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for");
534  return false;
535  }
536  else
537  {
538  pose_targets_[eef] = poses;
539  // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors
540  std::vector<geometry_msgs::PoseStamped>& stored_poses = pose_targets_[eef];
541  for (std::size_t i = 0; i < stored_poses.size(); ++i)
542  stored_poses[i].header.stamp = ros::Time(0);
543  }
544  return true;
545  }
546 
547  bool hasPoseTarget(const std::string& end_effector_link) const
548  {
549  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
550  return pose_targets_.find(eef) != pose_targets_.end();
551  }
552 
553  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const
554  {
555  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
556 
557  // if multiple pose targets are set, return the first one
558  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
559  if (jt != pose_targets_.end())
560  if (!jt->second.empty())
561  return jt->second.at(0);
562 
563  // or return an error
564  static const geometry_msgs::PoseStamped unknown;
565  ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str());
566  return unknown;
567  }
568 
569  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link) const
570  {
571  const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link;
572 
573  std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator jt = pose_targets_.find(eef);
574  if (jt != pose_targets_.end())
575  if (!jt->second.empty())
576  return jt->second;
577 
578  // or return an error
579  static const std::vector<geometry_msgs::PoseStamped> empty;
580  ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str());
581  return empty;
582  }
583 
584  void setPoseReferenceFrame(const std::string& pose_reference_frame)
585  {
586  pose_reference_frame_ = pose_reference_frame;
587  }
588 
589  void setSupportSurfaceName(const std::string& support_surface)
590  {
591  support_surface_ = support_surface;
592  }
593 
594  const std::string& getPoseReferenceFrame() const
595  {
596  return pose_reference_frame_;
597  }
598 
599  void setTargetType(ActiveTargetType type)
600  {
601  active_target_ = type;
602  }
603 
604  ActiveTargetType getTargetType() const
605  {
606  return active_target_;
607  }
608 
609  bool startStateMonitor(double wait)
610  {
612  {
613  ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state");
614  return false;
615  }
616 
617  // if needed, start the monitor and wait up to 1 second for a full robot state
618  if (!current_state_monitor_->isActive())
619  current_state_monitor_->startStateMonitor();
620 
621  current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
622  return true;
623  }
624 
625  bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0)
626  {
628  {
629  ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state");
630  return false;
631  }
632 
633  // if needed, start the monitor and wait up to 1 second for a full robot state
634  if (!current_state_monitor_->isActive())
635  current_state_monitor_->startStateMonitor();
636 
637  if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds))
638  {
639  ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state");
640  return false;
641  }
642 
643  current_state = current_state_monitor_->getCurrentState();
644  return true;
645  }
646 
648  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
649  {
650  std::vector<moveit_msgs::PlaceLocation> locations;
651  for (std::size_t i = 0; i < poses.size(); ++i)
652  {
653  moveit_msgs::PlaceLocation location;
654  location.pre_place_approach.direction.vector.z = -1.0;
655  location.post_place_retreat.direction.vector.x = -1.0;
656  location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame();
657  location.post_place_retreat.direction.header.frame_id = end_effector_link_;
658 
659  location.pre_place_approach.min_distance = 0.1;
660  location.pre_place_approach.desired_distance = 0.2;
661  location.post_place_retreat.min_distance = 0.0;
662  location.post_place_retreat.desired_distance = 0.2;
663  // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody
664 
665  location.place_pose = poses[i];
666  locations.push_back(location);
667  }
668  ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
669  (unsigned int)locations.size());
670  return place(object, locations);
671  }
672 
673  MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
674  {
676  {
677  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found");
678  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
679  }
680  if (!place_action_client_->isServerConnected())
681  {
682  ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected");
683  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
684  }
685  moveit_msgs::PlaceGoal goal;
686  constructGoal(goal, object);
687  goal.place_locations = locations;
688  goal.planning_options.plan_only = false;
689  goal.planning_options.look_around = can_look_;
690  goal.planning_options.replan = can_replan_;
691  goal.planning_options.replan_delay = replan_delay_;
692  goal.planning_options.planning_scene_diff.is_diff = true;
693  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
694 
695  place_action_client_->sendGoal(goal);
696  ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size());
697  if (!place_action_client_->waitForResult())
698  {
699  ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early");
700  }
702  {
703  return MoveItErrorCode(place_action_client_->getResult()->error_code);
704  }
705  else
706  {
707  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": "
708  << place_action_client_->getState().getText());
709  return MoveItErrorCode(place_action_client_->getResult()->error_code);
710  }
711  }
712 
713  MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
714  {
715  if (!pick_action_client_)
716  {
717  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found");
718  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
719  }
720  if (!pick_action_client_->isServerConnected())
721  {
722  ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected");
723  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
724  }
725  moveit_msgs::PickupGoal goal;
726  constructGoal(goal, object);
727  goal.possible_grasps = grasps;
728  goal.planning_options.plan_only = false;
729  goal.planning_options.look_around = can_look_;
730  goal.planning_options.replan = can_replan_;
731  goal.planning_options.replan_delay = replan_delay_;
732  goal.planning_options.planning_scene_diff.is_diff = true;
733  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
734 
735  pick_action_client_->sendGoal(goal);
736  if (!pick_action_client_->waitForResult())
737  {
738  ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early");
739  }
741  {
742  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
743  }
744  else
745  {
746  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": "
747  << pick_action_client_->getState().getText());
748  return MoveItErrorCode(pick_action_client_->getResult()->error_code);
749  }
750  }
751 
752  MoveItErrorCode planGraspsAndPick(const std::string& object)
753  {
754  if (object.empty())
755  {
756  return planGraspsAndPick(moveit_msgs::CollisionObject());
757  }
759 
760  std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));
761 
762  if (objects.size() < 1)
763  {
764  ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '"
765  << object << "', but the object could not be found");
766  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
767  }
768 
769  return planGraspsAndPick(objects[object]);
770  }
771 
772  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object)
773  {
775  {
776  ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '"
777  << GRASP_PLANNING_SERVICE_NAME
778  << "' is not available."
779  " This has to be implemented and started separately.");
780  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
781  }
782 
783  moveit_msgs::GraspPlanning::Request request;
784  moveit_msgs::GraspPlanning::Response response;
785 
786  request.group_name = opt_.group_name_;
787  request.target = object;
788  request.support_surfaces.push_back(support_surface_);
789 
790  ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner...");
791  if (!plan_grasps_service_.call(request, response) ||
792  response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
793  {
794  ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick.");
795  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
796  }
797 
798  return pick(object.id, response.grasps);
799  }
800 
802  {
803  if (!move_action_client_)
804  {
805  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
806  }
807  if (!move_action_client_->isServerConnected())
808  {
809  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
810  }
811 
812  moveit_msgs::MoveGroupGoal goal;
813  constructGoal(goal);
814  goal.planning_options.plan_only = true;
815  goal.planning_options.look_around = false;
816  goal.planning_options.replan = false;
817  goal.planning_options.planning_scene_diff.is_diff = true;
818  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
819 
820  move_action_client_->sendGoal(goal);
821  if (!move_action_client_->waitForResult())
822  {
823  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
824  }
826  {
827  plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
828  plan.start_state_ = move_action_client_->getResult()->trajectory_start;
829  plan.planning_time_ = move_action_client_->getResult()->planning_time;
830  return MoveItErrorCode(move_action_client_->getResult()->error_code);
831  }
832  else
833  {
834  ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": "
835  << move_action_client_->getState().getText());
836  return MoveItErrorCode(move_action_client_->getResult()->error_code);
837  }
838  }
839 
841  {
842  if (!move_action_client_)
843  {
844  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
845  }
846  if (!move_action_client_->isServerConnected())
847  {
848  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
849  }
850 
851  moveit_msgs::MoveGroupGoal goal;
852  constructGoal(goal);
853  goal.planning_options.plan_only = false;
854  goal.planning_options.look_around = can_look_;
855  goal.planning_options.replan = can_replan_;
856  goal.planning_options.replan_delay = replan_delay_;
857  goal.planning_options.planning_scene_diff.is_diff = true;
858  goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
859 
860  move_action_client_->sendGoal(goal);
861  if (!wait)
862  {
863  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
864  }
865 
866  if (!move_action_client_->waitForResult())
867  {
868  ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early");
869  }
870 
872  {
873  return MoveItErrorCode(move_action_client_->getResult()->error_code);
874  }
875  else
876  {
877  ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString()
878  << ": " << move_action_client_->getState().getText());
879  return MoveItErrorCode(move_action_client_->getResult()->error_code);
880  }
881  }
882 
883  MoveItErrorCode execute(const Plan& plan, bool wait)
884  {
886  {
887  // TODO: Remove this backwards compatibility code in L-turtle
888  moveit_msgs::ExecuteKnownTrajectory::Request req;
889  moveit_msgs::ExecuteKnownTrajectory::Response res;
890  req.trajectory = plan.trajectory_;
891  req.wait_for_execution = wait;
892  if (execute_service_.call(req, res))
893  {
894  return MoveItErrorCode(res.error_code);
895  }
896  else
897  {
898  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
899  }
900  }
901 
902  if (!execute_action_client_->isServerConnected())
903  {
904  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
905  }
906 
907  moveit_msgs::ExecuteTrajectoryGoal goal;
908  goal.trajectory = plan.trajectory_;
909 
910  execute_action_client_->sendGoal(goal);
911  if (!wait)
912  {
913  return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
914  }
915 
916  if (!execute_action_client_->waitForResult())
917  {
918  ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early");
919  }
920 
922  {
923  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
924  }
925  else
926  {
927  ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString()
928  << ": " << execute_action_client_->getState().getText());
929  return MoveItErrorCode(execute_action_client_->getResult()->error_code);
930  }
931  }
932 
933  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double step, double jump_threshold,
934  moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints,
935  bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code)
936  {
937  moveit_msgs::GetCartesianPath::Request req;
938  moveit_msgs::GetCartesianPath::Response res;
939 
941  robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state);
942  else
943  req.start_state.is_diff = true;
944 
945  req.group_name = opt_.group_name_;
946  req.header.frame_id = getPoseReferenceFrame();
947  req.header.stamp = ros::Time::now();
948  req.waypoints = waypoints;
949  req.max_step = step;
950  req.jump_threshold = jump_threshold;
951  req.path_constraints = path_constraints;
952  req.avoid_collisions = avoid_collisions;
953  req.link_name = getEndEffectorLink();
954 
955  if (cartesian_path_service_.call(req, res))
956  {
957  error_code = res.error_code;
958  if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
959  {
960  msg = res.solution;
961  return res.fraction;
962  }
963  else
964  return -1.0;
965  }
966  else
967  {
968  error_code.val = error_code.FAILURE;
969  return -1.0;
970  }
971  }
972 
973  void stop()
974  {
976  {
977  std_msgs::String event;
978  event.data = "stop";
980  }
981  }
982 
983  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links)
984  {
985  std::string l = link.empty() ? getEndEffectorLink() : link;
986  if (l.empty())
987  {
988  const std::vector<std::string>& links = joint_model_group_->getLinkModelNames();
989  if (!links.empty())
990  l = links[0];
991  }
992  if (l.empty())
993  {
994  ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str());
995  return false;
996  }
997  moveit_msgs::AttachedCollisionObject aco;
998  aco.object.id = object;
999  aco.link_name.swap(l);
1000  if (touch_links.empty())
1001  aco.touch_links.push_back(aco.link_name);
1002  else
1003  aco.touch_links = touch_links;
1004  aco.object.operation = moveit_msgs::CollisionObject::ADD;
1006  return true;
1007  }
1008 
1009  bool detachObject(const std::string& name)
1010  {
1011  moveit_msgs::AttachedCollisionObject aco;
1012  // if name is a link
1013  if (!name.empty() && joint_model_group_->hasLinkModel(name))
1014  aco.link_name = name;
1015  else
1016  aco.object.id = name;
1017  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
1018  if (aco.link_name.empty() && aco.object.id.empty())
1019  {
1020  // we only want to detach objects for this group
1021  const std::vector<std::string>& lnames = joint_model_group_->getLinkModelNames();
1022  for (std::size_t i = 0; i < lnames.size(); ++i)
1023  {
1024  aco.link_name = lnames[i];
1026  }
1027  }
1028  else
1030  return true;
1031  }
1032 
1034  {
1035  return goal_position_tolerance_;
1036  }
1037 
1039  {
1041  }
1042 
1043  double getGoalJointTolerance() const
1044  {
1045  return goal_joint_tolerance_;
1046  }
1047 
1048  void setGoalJointTolerance(double tolerance)
1049  {
1050  goal_joint_tolerance_ = tolerance;
1051  }
1052 
1053  void setGoalPositionTolerance(double tolerance)
1054  {
1055  goal_position_tolerance_ = tolerance;
1056  }
1057 
1058  void setGoalOrientationTolerance(double tolerance)
1059  {
1060  goal_orientation_tolerance_ = tolerance;
1061  }
1062 
1063  void setPlanningTime(double seconds)
1064  {
1065  if (seconds > 0.0)
1066  allowed_planning_time_ = seconds;
1067  }
1068 
1069  double getPlanningTime() const
1070  {
1071  return allowed_planning_time_;
1072  }
1073 
1074  void allowLooking(bool flag)
1075  {
1076  can_look_ = flag;
1077  ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no");
1078  }
1079 
1080  void allowReplanning(bool flag)
1081  {
1082  can_replan_ = flag;
1083  ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no");
1084  }
1085 
1086  void setReplanningDelay(double delay)
1087  {
1088  if (delay >= 0.0)
1089  replan_delay_ = delay;
1090  }
1091 
1092  double getReplanningDelay() const
1093  {
1094  return replan_delay_;
1095  }
1096 
1097  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request)
1098  {
1099  request.group_name = opt_.group_name_;
1100  request.num_planning_attempts = num_planning_attempts_;
1101  request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
1102  request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_;
1103  request.allowed_planning_time = allowed_planning_time_;
1104  request.planner_id = planner_id_;
1105  request.workspace_parameters = workspace_parameters_;
1106 
1108  robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
1109  else
1110  request.start_state.is_diff = true;
1111 
1112  if (active_target_ == JOINT)
1113  {
1114  request.goal_constraints.resize(1);
1115  request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
1117  }
1118  else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION)
1119  {
1120  // find out how many goals are specified
1121  std::size_t goal_count = 0;
1122  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1123  it != pose_targets_.end(); ++it)
1124  goal_count = std::max(goal_count, it->second.size());
1125 
1126  // start filling the goals;
1127  // each end effector has a number of possible poses (K) as valid goals
1128  // but there could be multiple end effectors specified, so we want each end effector
1129  // to reach the goal that corresponds to the goals of the other end effectors
1130  request.goal_constraints.resize(goal_count);
1131 
1132  for (std::map<std::string, std::vector<geometry_msgs::PoseStamped> >::const_iterator it = pose_targets_.begin();
1133  it != pose_targets_.end(); ++it)
1134  {
1135  for (std::size_t i = 0; i < it->second.size(); ++i)
1136  {
1137  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
1138  it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_);
1139  if (active_target_ == ORIENTATION)
1140  c.position_constraints.clear();
1141  if (active_target_ == POSITION)
1142  c.orientation_constraints.clear();
1143  request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c);
1144  }
1145  }
1146  }
1147  else
1148  ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation");
1149 
1150  if (path_constraints_)
1151  request.path_constraints = *path_constraints_;
1153  request.trajectory_constraints = *trajectory_constraints_;
1154  }
1155 
1156  void constructGoal(moveit_msgs::MoveGroupGoal& goal)
1157  {
1158  constructMotionPlanRequest(goal.request);
1159  }
1160 
1161  void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object)
1162  {
1163  moveit_msgs::PickupGoal goal;
1164  goal.target_name = object;
1165  goal.group_name = opt_.group_name_;
1166  goal.end_effector = getEndEffector();
1167  goal.allowed_planning_time = allowed_planning_time_;
1168  goal.support_surface_name = support_surface_;
1169  goal.planner_id = planner_id_;
1170  if (!support_surface_.empty())
1171  goal.allow_gripper_support_collision = true;
1172 
1173  if (path_constraints_)
1174  goal.path_constraints = *path_constraints_;
1175 
1176  goal_out = goal;
1177  }
1178 
1179  void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object)
1180  {
1181  moveit_msgs::PlaceGoal goal;
1182  goal.attached_object_name = object;
1183  goal.group_name = opt_.group_name_;
1184  goal.allowed_planning_time = allowed_planning_time_;
1185  goal.support_surface_name = support_surface_;
1186  goal.planner_id = planner_id_;
1187  if (!support_surface_.empty())
1188  goal.allow_gripper_support_collision = true;
1189 
1190  if (path_constraints_)
1191  goal.path_constraints = *path_constraints_;
1192 
1193  goal_out = goal;
1194  }
1195 
1196  void setPathConstraints(const moveit_msgs::Constraints& constraint)
1197  {
1198  path_constraints_.reset(new moveit_msgs::Constraints(constraint));
1199  }
1200 
1201  bool setPathConstraints(const std::string& constraint)
1202  {
1204  {
1206  if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
1207  {
1208  path_constraints_.reset(new moveit_msgs::Constraints(static_cast<moveit_msgs::Constraints>(*msg_m)));
1209  return true;
1210  }
1211  else
1212  return false;
1213  }
1214  else
1215  return false;
1216  }
1217 
1219  {
1220  path_constraints_.reset();
1221  }
1222 
1223  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint)
1224  {
1225  trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint));
1226  }
1227 
1229  {
1230  trajectory_constraints_.reset();
1231  }
1232 
1233  std::vector<std::string> getKnownConstraints() const
1234  {
1236  {
1237  static ros::WallDuration d(0.01);
1238  d.sleep();
1239  }
1240 
1241  std::vector<std::string> c;
1243  constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
1244 
1245  return c;
1246  }
1247 
1248  moveit_msgs::Constraints getPathConstraints() const
1249  {
1250  if (path_constraints_)
1251  return *path_constraints_;
1252  else
1253  return moveit_msgs::Constraints();
1254  }
1255 
1256  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
1257  {
1259  return *trajectory_constraints_;
1260  else
1261  return moveit_msgs::TrajectoryConstraints();
1262  }
1263 
1264  void initializeConstraintsStorage(const std::string& host, unsigned int port)
1265  {
1268  constraints_init_thread_->join();
1270  new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port)));
1271  }
1272 
1273  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
1274  {
1275  workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame();
1276  workspace_parameters_.header.stamp = ros::Time::now();
1277  workspace_parameters_.min_corner.x = minx;
1278  workspace_parameters_.min_corner.y = miny;
1279  workspace_parameters_.min_corner.z = minz;
1280  workspace_parameters_.max_corner.x = maxx;
1281  workspace_parameters_.max_corner.y = maxy;
1282  workspace_parameters_.max_corner.z = maxz;
1283  }
1284 
1285 private:
1286  void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
1287  {
1288  // Set up db
1289  try
1290  {
1292  conn->setParams(host, port);
1293  if (conn->connect())
1294  {
1296  }
1297  }
1298  catch (std::exception& ex)
1299  {
1300  ROS_ERROR_NAMED("move_group_interface", "%s", ex.what());
1301  }
1302  initializing_constraints_ = false;
1303  }
1304 
1308  robot_model::RobotModelConstPtr robot_model_;
1309  planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
1310  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
1311  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> > execute_action_client_;
1312  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
1313  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;
1314 
1315  // general planning params
1316  robot_state::RobotStatePtr considered_start_state_;
1317  moveit_msgs::WorkspaceParameters workspace_parameters_;
1319  std::string planner_id_;
1329 
1330  // joint state goal
1331  robot_state::RobotStatePtr joint_state_target_;
1332  const robot_model::JointModelGroup* joint_model_group_;
1333 
1334  // pose goal;
1335  // for each link we have a set of possible goal locations;
1336  std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
1337 
1338  // common properties for goals
1339  ActiveTargetType active_target_;
1340  std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
1341  std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
1342  std::string end_effector_link_;
1344  std::string support_surface_;
1345 
1346  // ROS communication
1355  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
1356  std::unique_ptr<boost::thread> constraints_init_thread_;
1358 };
1359 }
1360 }
1361 
1364  const ros::WallDuration& wait_for_servers)
1365 {
1366  if (!ros::ok())
1367  throw std::runtime_error("ROS does not seem to be running");
1368  impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf ? tf : getSharedTF(), wait_for_servers);
1369 }
1370 
1373  const ros::Duration& wait_for_servers)
1374  : MoveGroupInterface(group, tf, ros::WallDuration(wait_for_servers.toSec()))
1375 {
1376 }
1377 
1380  const ros::WallDuration& wait_for_servers)
1381 {
1382  impl_ = new MoveGroupInterfaceImpl(opt, tf ? tf : getSharedTF(), wait_for_servers);
1383 }
1384 
1387  const ros::Duration& wait_for_servers)
1388  : MoveGroupInterface(opt, tf, ros::WallDuration(wait_for_servers.toSec()))
1389 {
1390 }
1391 
1393 {
1394  delete impl_;
1395 }
1396 
1399 {
1400  other.impl_ = nullptr;
1401 }
1402 
1405 {
1406  if (this != &other)
1407  {
1408  delete impl_;
1409  impl_ = std::move(other.impl_);
1410  remembered_joint_values_ = std::move(other.remembered_joint_values_);
1411  other.impl_ = nullptr;
1412  }
1413 
1414  return *this;
1415 }
1416 
1418 {
1419  return impl_->getOptions().group_name_;
1420 }
1421 
1423 {
1424  const robot_model::RobotModelConstPtr& robot = getRobotModel();
1425  const std::string& group = getName();
1426  const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group);
1427 
1428  if (joint_group)
1429  {
1430  return joint_group->getDefaultStateNames();
1431  }
1432 
1433  std::vector<std::string> empty;
1434  return empty;
1435 }
1436 
1438 {
1439  return impl_->getRobotModel();
1440 }
1441 
1443 {
1444  return impl_->getOptions().node_handle_;
1445 }
1446 
1448  moveit_msgs::PlannerInterfaceDescription& desc)
1449 {
1450  return impl_->getInterfaceDescription(desc);
1451 }
1452 
1454  const std::string& planner_id, const std::string& group)
1455 {
1456  return impl_->getPlannerParams(planner_id, group);
1457 }
1458 
1460  const std::string& group,
1461  const std::map<std::string, std::string>& params,
1462  bool replace)
1463 {
1464  impl_->setPlannerParams(planner_id, group, params, replace);
1465 }
1466 
1468 {
1469  return impl_->getDefaultPlannerId(group);
1470 }
1471 
1473 {
1474  impl_->setPlannerId(planner_id);
1475 }
1476 
1478 {
1479  return impl_->getPlannerId();
1480 }
1481 
1483 {
1484  impl_->setNumPlanningAttempts(num_planning_attempts);
1485 }
1486 
1488 {
1489  impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
1490 }
1491 
1493  double max_acceleration_scaling_factor)
1494 {
1495  impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
1496 }
1497 
1499 {
1500  return impl_->move(false);
1501 }
1502 
1504 {
1505  return impl_->move(true);
1506 }
1507 
1510 {
1511  return impl_->execute(plan, false);
1512 }
1513 
1515 {
1516  return impl_->execute(plan, true);
1517 }
1518 
1520 {
1521  return impl_->plan(plan);
1522 }
1523 
1526 {
1527  return impl_->pick(object, std::vector<moveit_msgs::Grasp>());
1528 }
1529 
1531 moveit::planning_interface::MoveGroupInterface::pick(const std::string& object, const moveit_msgs::Grasp& grasp)
1532 {
1533  return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp));
1534 }
1535 
1537  const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps)
1538 {
1539  return impl_->pick(object, grasps);
1540 }
1541 
1544 {
1545  return impl_->planGraspsAndPick(object);
1546 }
1547 
1550 {
1551  return impl_->planGraspsAndPick(object);
1552 }
1553 
1556 {
1557  return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>());
1558 }
1559 
1561  const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations)
1562 {
1563  return impl_->place(object, locations);
1564 }
1565 
1567  const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses)
1568 {
1569  return impl_->place(object, poses);
1570 }
1571 
1573 moveit::planning_interface::MoveGroupInterface::place(const std::string& object, const geometry_msgs::PoseStamped& pose)
1574 {
1575  return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose));
1576 }
1577 
1579  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1580  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code)
1581 {
1582  moveit_msgs::Constraints path_constraints_tmp;
1583  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
1584  error_code);
1585 }
1586 
1588  const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
1589  moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions,
1590  moveit_msgs::MoveItErrorCodes* error_code)
1591 {
1592  if (error_code)
1593  {
1594  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1595  avoid_collisions, *error_code);
1596  }
1597  else
1598  {
1599  moveit_msgs::MoveItErrorCodes error_code_tmp;
1600  return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
1601  avoid_collisions, error_code_tmp);
1602  }
1603 }
1604 
1606 {
1607  impl_->stop();
1608 }
1609 
1610 void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state)
1611 {
1612  robot_state::RobotStatePtr rs;
1613  impl_->getCurrentState(rs);
1614  robot_state::robotStateMsgToRobotState(start_state, *rs);
1615  setStartState(*rs);
1616 }
1617 
1618 void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state)
1619 {
1620  impl_->setStartState(start_state);
1621 }
1622 
1624 {
1626 }
1627 
1629 {
1630  impl_->getJointStateTarget().setToRandomPositions();
1631  impl_->setTargetType(JOINT);
1632 }
1633 
1635 {
1636  return impl_->getJointModelGroup()->getVariableNames();
1637 }
1638 
1640 {
1641  return impl_->getJointModelGroup()->getLinkModelNames();
1642 }
1643 
1644 std::map<std::string, double>
1646 {
1647  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1648  std::map<std::string, double> positions;
1649 
1650  if (it != remembered_joint_values_.end())
1651  {
1652  std::vector<std::string> names = impl_->getJointModelGroup()->getVariableNames();
1653  for (size_t x = 0; x < names.size(); ++x)
1654  {
1655  positions[names[x]] = it->second[x];
1656  }
1657  }
1658  else
1659  {
1660  impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions);
1661  }
1662  return positions;
1663 }
1664 
1666 {
1667  std::map<std::string, std::vector<double> >::const_iterator it = remembered_joint_values_.find(name);
1668  if (it != remembered_joint_values_.end())
1669  {
1670  return setJointValueTarget(it->second);
1671  }
1672  else
1673  {
1674  if (impl_->getJointStateTarget().setToDefaultValues(impl_->getJointModelGroup(), name))
1675  {
1676  impl_->setTargetType(JOINT);
1677  return true;
1678  }
1679  ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str());
1680  return false;
1681  }
1682 }
1683 
1684 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
1685 {
1686  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
1687  return false;
1688  impl_->setTargetType(JOINT);
1689  impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
1691 }
1692 
1694  const std::map<std::string, double>& joint_values)
1695 {
1696  impl_->setTargetType(JOINT);
1697  impl_->getJointStateTarget().setVariablePositions(joint_values);
1698  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1699 }
1700 
1702 {
1703  impl_->setTargetType(JOINT);
1704  impl_->getJointStateTarget() = rstate;
1705  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1706 }
1707 
1708 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value)
1709 {
1710  std::vector<double> values(1, value);
1711  return setJointValueTarget(joint_name, values);
1712 }
1713 
1715  const std::vector<double>& values)
1716 {
1717  impl_->setTargetType(JOINT);
1718  const robot_model::JointModel* jm = impl_->getJointStateTarget().getJointModel(joint_name);
1719  if (jm && jm->getVariableCount() == values.size())
1720  {
1721  impl_->getJointStateTarget().setJointPositions(jm, values);
1722  return impl_->getJointStateTarget().satisfiesBounds(jm, impl_->getGoalJointTolerance());
1723  }
1724  return false;
1725 }
1726 
1728 {
1729  impl_->setTargetType(JOINT);
1730  impl_->getJointStateTarget().setVariableValues(state);
1731  return impl_->getJointStateTarget().satisfiesBounds(impl_->getGoalJointTolerance());
1732 }
1733 
1735  const std::string& end_effector_link)
1736 {
1737  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false);
1738 }
1739 
1740 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
1741  const std::string& end_effector_link)
1742 {
1743  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false);
1744 }
1745 
1747  const std::string& end_effector_link)
1748 {
1749  geometry_msgs::Pose msg;
1750  tf::poseEigenToMsg(eef_pose, msg);
1751  return setJointValueTarget(msg, end_effector_link);
1752 }
1753 
1755  const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link)
1756 {
1757  return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true);
1758 }
1759 
1761  const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link)
1762 {
1763  return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true);
1764 }
1765 
1767  const Eigen::Affine3d& eef_pose, const std::string& end_effector_link)
1768 {
1769  geometry_msgs::Pose msg;
1770  tf::poseEigenToMsg(eef_pose, msg);
1771  return setApproximateJointValueTarget(msg, end_effector_link);
1772 }
1773 
1775 {
1776  return impl_->getJointStateTarget();
1777 }
1778 
1780 {
1781  return impl_->getEndEffectorLink();
1782 }
1783 
1785 {
1786  return impl_->getEndEffector();
1787 }
1788 
1790 {
1791  if (impl_->getEndEffectorLink().empty() || link_name.empty())
1792  return false;
1793  impl_->setEndEffectorLink(link_name);
1794  impl_->setTargetType(POSE);
1795  return true;
1796 }
1797 
1799 {
1800  const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name);
1801  if (jmg)
1802  return setEndEffectorLink(jmg->getEndEffectorParentGroup().second);
1803  return false;
1804 }
1805 
1807 {
1808  impl_->clearPoseTarget(end_effector_link);
1809 }
1810 
1812 {
1814 }
1815 
1817  const std::string& end_effector_link)
1818 {
1819  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1820  tf::poseEigenToMsg(pose, pose_msg[0].pose);
1821  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1822  pose_msg[0].header.stamp = ros::Time::now();
1823  return setPoseTargets(pose_msg, end_effector_link);
1824 }
1825 
1827  const std::string& end_effector_link)
1828 {
1829  std::vector<geometry_msgs::PoseStamped> pose_msg(1);
1830  pose_msg[0].pose = target;
1831  pose_msg[0].header.frame_id = getPoseReferenceFrame();
1832  pose_msg[0].header.stamp = ros::Time::now();
1833  return setPoseTargets(pose_msg, end_effector_link);
1834 }
1835 
1836 bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target,
1837  const std::string& end_effector_link)
1838 {
1839  std::vector<geometry_msgs::PoseStamped> targets(1, target);
1840  return setPoseTargets(targets, end_effector_link);
1841 }
1842 
1844  const std::string& end_effector_link)
1845 {
1846  std::vector<geometry_msgs::PoseStamped> pose_out(target.size());
1847  ros::Time tm = ros::Time::now();
1848  const std::string& frame_id = getPoseReferenceFrame();
1849  for (std::size_t i = 0; i < target.size(); ++i)
1850  {
1851  tf::poseEigenToMsg(target[i], pose_out[i].pose);
1852  pose_out[i].header.stamp = tm;
1853  pose_out[i].header.frame_id = frame_id;
1854  }
1855  return setPoseTargets(pose_out, end_effector_link);
1856 }
1857 
1858 bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector<geometry_msgs::Pose>& target,
1859  const std::string& end_effector_link)
1860 {
1861  std::vector<geometry_msgs::PoseStamped> target_stamped(target.size());
1862  ros::Time tm = ros::Time::now();
1863  const std::string& frame_id = getPoseReferenceFrame();
1864  for (std::size_t i = 0; i < target.size(); ++i)
1865  {
1866  target_stamped[i].pose = target[i];
1867  target_stamped[i].header.stamp = tm;
1868  target_stamped[i].header.frame_id = frame_id;
1869  }
1870  return setPoseTargets(target_stamped, end_effector_link);
1871 }
1872 
1874  const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link)
1875 {
1876  if (target.empty())
1877  {
1878  ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target");
1879  return false;
1880  }
1881  else
1882  {
1883  impl_->setTargetType(POSE);
1884  return impl_->setPoseTargets(target, end_effector_link);
1885  }
1886 }
1887 
1888 const geometry_msgs::PoseStamped&
1889 moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const
1890 {
1891  return impl_->getPoseTarget(end_effector_link);
1892 }
1893 
1894 const std::vector<geometry_msgs::PoseStamped>&
1895 moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const
1896 {
1897  return impl_->getPoseTargets(end_effector_link);
1898 }
1899 
1900 namespace
1901 {
1902 inline void transformPose(const tf::Transformer& tf, const std::string& desired_frame,
1903  geometry_msgs::PoseStamped& target)
1904 {
1905  if (desired_frame != target.header.frame_id)
1906  {
1907  tf::Pose pose;
1908  tf::poseMsgToTF(target.pose, pose);
1909  tf::Stamped<tf::Pose> stamped_target(pose, target.header.stamp, target.header.frame_id);
1910  tf::Stamped<tf::Pose> stamped_target_out;
1911  tf.transformPose(desired_frame, stamped_target, stamped_target_out);
1912  target.header.frame_id = stamped_target_out.frame_id_;
1913  // target.header.stamp = stamped_target_out.stamp_; // we leave the stamp to ros::Time(0) on purpose
1914  tf::poseTFToMsg(stamped_target_out, target.pose);
1915  }
1916 }
1917 }
1918 
1920  const std::string& end_effector_link)
1921 {
1922  geometry_msgs::PoseStamped target;
1923  if (impl_->hasPoseTarget(end_effector_link))
1924  {
1925  target = getPoseTarget(end_effector_link);
1926  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1927  }
1928  else
1929  {
1930  target.pose.orientation.x = 0.0;
1931  target.pose.orientation.y = 0.0;
1932  target.pose.orientation.z = 0.0;
1933  target.pose.orientation.w = 1.0;
1934  target.header.frame_id = impl_->getPoseReferenceFrame();
1935  }
1936 
1937  target.pose.position.x = x;
1938  target.pose.position.y = y;
1939  target.pose.position.z = z;
1940  bool result = setPoseTarget(target, end_effector_link);
1941  impl_->setTargetType(POSITION);
1942  return result;
1943 }
1944 
1946  const std::string& end_effector_link)
1947 {
1948  geometry_msgs::PoseStamped target;
1949  if (impl_->hasPoseTarget(end_effector_link))
1950  {
1951  target = getPoseTarget(end_effector_link);
1952  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1953  }
1954  else
1955  {
1956  target.pose.position.x = 0.0;
1957  target.pose.position.y = 0.0;
1958  target.pose.position.z = 0.0;
1959  target.header.frame_id = impl_->getPoseReferenceFrame();
1960  }
1961 
1962  tf::quaternionTFToMsg(tf::createQuaternionFromRPY(r, p, y), target.pose.orientation);
1963  bool result = setPoseTarget(target, end_effector_link);
1964  impl_->setTargetType(ORIENTATION);
1965  return result;
1966 }
1967 
1969  const std::string& end_effector_link)
1970 {
1971  geometry_msgs::PoseStamped target;
1972  if (impl_->hasPoseTarget(end_effector_link))
1973  {
1974  target = getPoseTarget(end_effector_link);
1975  transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target);
1976  }
1977  else
1978  {
1979  target.pose.position.x = 0.0;
1980  target.pose.position.y = 0.0;
1981  target.pose.position.z = 0.0;
1982  target.header.frame_id = impl_->getPoseReferenceFrame();
1983  }
1984 
1985  target.pose.orientation.x = x;
1986  target.pose.orientation.y = y;
1987  target.pose.orientation.z = z;
1988  target.pose.orientation.w = w;
1989  bool result = setPoseTarget(target, end_effector_link);
1990  impl_->setTargetType(ORIENTATION);
1991  return result;
1992 }
1993 
1995 {
1996  impl_->setPoseReferenceFrame(pose_reference_frame);
1997 }
1998 
2000 {
2001  return impl_->getPoseReferenceFrame();
2002 }
2003 
2005 {
2006  return impl_->getGoalJointTolerance();
2007 }
2008 
2010 {
2011  return impl_->getGoalPositionTolerance();
2012 }
2013 
2015 {
2017 }
2018 
2020 {
2021  setGoalJointTolerance(tolerance);
2022  setGoalPositionTolerance(tolerance);
2023  setGoalOrientationTolerance(tolerance);
2024 }
2025 
2027 {
2028  impl_->setGoalJointTolerance(tolerance);
2029 }
2030 
2032 {
2033  impl_->setGoalPositionTolerance(tolerance);
2034 }
2035 
2037 {
2038  impl_->setGoalOrientationTolerance(tolerance);
2039 }
2040 
2042 {
2044 }
2045 
2047 {
2048  return impl_->startStateMonitor(wait);
2049 }
2050 
2052 {
2053  robot_state::RobotStatePtr current_state;
2054  std::vector<double> values;
2055  if (impl_->getCurrentState(current_state))
2056  current_state->copyJointGroupPositions(getName(), values);
2057  return values;
2058 }
2059 
2061 {
2062  std::vector<double> r;
2063  impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getJointStateTarget().getRandomNumberGenerator(), r);
2064  return r;
2065 }
2066 
2067 geometry_msgs::PoseStamped
2069 {
2070  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2071  Eigen::Affine3d pose;
2072  pose.setIdentity();
2073  if (eef.empty())
2074  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2075  else
2076  {
2077  robot_state::RobotStatePtr current_state;
2078  if (impl_->getCurrentState(current_state))
2079  {
2080  current_state->setToRandomPositions(impl_->getJointModelGroup());
2081  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2082  if (lm)
2083  pose = current_state->getGlobalLinkTransform(lm);
2084  }
2085  }
2086  geometry_msgs::PoseStamped pose_msg;
2087  pose_msg.header.stamp = ros::Time::now();
2088  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2089  tf::poseEigenToMsg(pose, pose_msg.pose);
2090  return pose_msg;
2091 }
2092 
2093 geometry_msgs::PoseStamped
2095 {
2096  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2097  Eigen::Affine3d pose;
2098  pose.setIdentity();
2099  if (eef.empty())
2100  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2101  else
2102  {
2103  robot_state::RobotStatePtr current_state;
2104  if (impl_->getCurrentState(current_state))
2105  {
2106  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2107  if (lm)
2108  pose = current_state->getGlobalLinkTransform(lm);
2109  }
2110  }
2111  geometry_msgs::PoseStamped pose_msg;
2112  pose_msg.header.stamp = ros::Time::now();
2113  pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
2114  tf::poseEigenToMsg(pose, pose_msg.pose);
2115  return pose_msg;
2116 }
2117 
2118 std::vector<double> moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link)
2119 {
2120  std::vector<double> result;
2121  const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
2122  if (eef.empty())
2123  ROS_ERROR_NAMED("move_group_interface", "No end-effector specified");
2124  else
2125  {
2126  robot_state::RobotStatePtr current_state;
2127  if (impl_->getCurrentState(current_state))
2128  {
2129  const robot_model::LinkModel* lm = current_state->getLinkModel(eef);
2130  if (lm)
2131  {
2132  result.resize(3);
2133  tf::Matrix3x3 ptf;
2134  tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
2135  tfScalar pitch, roll, yaw;
2136  ptf.getRPY(roll, pitch, yaw);
2137  result[0] = roll;
2138  result[1] = pitch;
2139  result[2] = yaw;
2140  }
2141  }
2142  }
2143  return result;
2144 }
2145 
2147 {
2148  return impl_->getJointModelGroup()->getActiveJointModelNames();
2149 }
2150 
2151 const std::vector<std::string>& moveit::planning_interface::MoveGroupInterface::getJoints() const
2152 {
2153  return impl_->getJointModelGroup()->getJointModelNames();
2154 }
2155 
2157 {
2158  return impl_->getJointModelGroup()->getVariableCount();
2159 }
2160 
2162 {
2163  robot_state::RobotStatePtr current_state;
2164  impl_->getCurrentState(current_state, wait);
2165  return current_state;
2166 }
2167 
2169  const std::vector<double>& values)
2170 {
2171  remembered_joint_values_[name] = values;
2172 }
2173 
2175 {
2176  remembered_joint_values_.erase(name);
2177 }
2178 
2180 {
2181  impl_->allowLooking(flag);
2182 }
2183 
2185 {
2186  impl_->allowReplanning(flag);
2187 }
2188 
2190 {
2191  return impl_->getKnownConstraints();
2192 }
2193 
2195 {
2196  return impl_->getPathConstraints();
2197 }
2198 
2200 {
2201  return impl_->setPathConstraints(constraint);
2202 }
2203 
2204 void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint)
2205 {
2206  impl_->setPathConstraints(constraint);
2207 }
2208 
2210 {
2212 }
2213 
2215 {
2216  return impl_->getTrajectoryConstraints();
2217 }
2218 
2220  const moveit_msgs::TrajectoryConstraints& constraint)
2221 {
2222  impl_->setTrajectoryConstraints(constraint);
2223 }
2224 
2226 {
2228 }
2229 
2230 void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port)
2231 {
2232  impl_->initializeConstraintsStorage(host, port);
2233 }
2234 
2235 void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx,
2236  double maxy, double maxz)
2237 {
2238  impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz);
2239 }
2240 
2243 {
2244  impl_->setPlanningTime(seconds);
2245 }
2246 
2249 {
2250  return impl_->getPlanningTime();
2251 }
2252 
2254 {
2256 }
2257 
2259 {
2260  return impl_->getRobotModel()->getModelFrame();
2261 }
2262 
2263 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link)
2264 {
2265  return attachObject(object, link, std::vector<std::string>());
2266 }
2267 
2268 bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link,
2269  const std::vector<std::string>& touch_links)
2270 {
2271  return impl_->attachObject(object, link, touch_links);
2272 }
2273 
2275 {
2276  return impl_->detachObject(name);
2277 }
2278 
2280  moveit_msgs::MotionPlanRequest& goal_out)
2281 {
2282  impl_->constructMotionPlanRequest(goal_out);
2283 }
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...
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
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...
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_) ...
#define ROS_INFO_NAMED(name,...)
boost::shared_ptr< tf::Transformer > getSharedTF()
robot_model::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
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.
void publish(const boost::shared_ptr< M > &message) const
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...
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
#define ROS_WARN_NAMED(name,...)
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).
std::vector< double > values
void clearPoseTargets()
Forget any poses specified for all end-effectors.
double tfScalar
static const std::string SET_PLANNER_PARAMS_SERVICE_NAME
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
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, const std::vector< moveit_msgs::Grasp > &grasps)
std::map< std::string, std::vector< geometry_msgs::PoseStamped > > pose_targets_
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
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
MoveItErrorCode place(const std::string &object)
Place an object somewhere safe in the world (a safe location will be detected)
std::map< std::string, std::vector< double > > remembered_joint_values_
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link) 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.
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link) const
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...
void setPlannerId(const std::string &planner_id)
Specify a planner to be used for further planning.
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...
void initializeConstraintsStorageThread(const std::string &host, unsigned int port)
Specification of options to use when constructing the MoveGroupInterface class.
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr< tf::Transformer > &tf)
getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelCons...
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)
const std::string & getPlanningFrame() const
Get the name of the frame in which the robot is planning.
void setPlanningTime(double seconds)
Specify the maximum amount of time to use when planning.
MoveItErrorCode place(const std::string &object, const std::vector< geometry_msgs::PoseStamped > &poses)
Place an object at one of the specified possible locations.
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...
const std::string & getEndEffectorLink() const
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly ...
bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription &desc)
const std::string & getName() const
Get the name of the group this instance operates on.
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,...)
std::vector< std::string > getKnownConstraints() const
Get the names of the known constraints as read from the Mongo database, if a connection was achieved...
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PlaceAction > > place_action_client_
bool getCurrentState(robot_state::RobotStatePtr &current_state, double wait_seconds=1.0)
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...
const ros::NodeHandle & getNodeHandle() const
Get the ROS node handle of this instance operates on.
bool sleep() const
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.
std::map< std::string, std::string > getPlannerParams(const std::string &planner_id, const std::string &group="")
MoveGroupInterfaceImpl(const Options &opt, const boost::shared_ptr< tf::Transformer > &tf, const ros::WallDuration &wait_for_servers)
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.
double getGoalPositionTolerance() const
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where ...
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...
MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject &object)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
#define ROS_FATAL_STREAM_NAMED(name, args)
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
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)
double getPlanningTime() const
Get the number of seconds set by setPlanningTime()
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::ExecuteTrajectoryAction > > execute_action_client_
double getGoalOrientationTolerance() const
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll...
const geometry_msgs::PoseStamped & getPoseTarget(const std::string &end_effector_link="") const
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...
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
bool setJointValueTarget(const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link, const std::string &frame, bool approx)
void transformPose(const std::string &target_frame, const Stamped< tf::Pose > &stamped_in, Stamped< tf::Pose > &stamped_out) const
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.
moveit_msgs::Constraints getPathConstraints() const
Get the actual set of constraints in use with this MoveGroupInterface.
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()
const std::string & getNamespace() const
MoveItErrorCode place(const std::string &object, const std::vector< moveit_msgs::PlaceLocation > &locations)
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...
MoveGroupInterface(const Options &opt, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const ros::WallDuration &wait_for_servers=ros::WallDuration())
Construct a MoveGroupInterface instance call using a specified set of options opt.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
MoveItErrorCode execute(const Plan &plan)
Given a plan, execute it while waiting for completion. Return true on success.
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)
const std::vector< std::string > & getActiveJoints() const
Get only the active (actuated) joints this instance operates on.
const std::vector< std::string > & getJoints() const
Get all the joints this instance operates on (including fixed joints)
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.
const robot_state::RobotState & getJointValueTarget() const
Get the currently set joint state goal.
const std::vector< geometry_msgs::PoseStamped > & getPoseTargets(const std::string &end_effector_link="") const
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.
moveit_msgs::RobotState start_state_
The full starting state used for planning.
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
std::string frame_id_
static WallTime now()
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)
bool getParam(const std::string &key, std::string &s) const
void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &constraint)
const std::string & getEndEffector() const
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by s...
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,...)
double getGoalJointTolerance() const
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configur...
static Time now()
std::unique_ptr< moveit_msgs::TrajectoryConstraints > trajectory_constraints_
const std::string & getPlannerId() const
Get the current planner_id.
robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
MoveItErrorCode pick(const std::string &object)
Pick up an object.
static const std::string GET_PLANNER_PARAMS_SERVICE_NAME
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
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.
bool ok() const
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...
CallbackQueueInterface * getCallbackQueue() const
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)
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
std::string getDefaultPlannerId(const std::string &group="") const
Get the default planner for a given group (or global default)
double x
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.
The representation of a motion plan (as ROS messasges)
MoveItErrorCode planGraspsAndPick(const std::string &object="")
Pick up an object.
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.
unsigned int getVariableCount() const
Get the number of variables used to describe the state of this group. This is larger or equal to the ...
void setStartStateToCurrentState()
Set the starting state for planning to be that reported by the robot&#39;s joint state publication...
const boost::shared_ptr< tf::Transformer > & getTF() const
const std::string & getPoseReferenceFrame() const
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the...
#define ROS_WARN_STREAM_NAMED(name, args)
std::unique_ptr< actionlib::SimpleActionClient< moveit_msgs::PickupAction > > pick_action_client_
moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const
#define ROS_DEBUG(...)
static const std::string EXECUTE_SERVICE_NAME


planning_interface
Author(s): Ioan Sucan
autogenerated on Fri Sep 21 2018 02:50:36